Last active
August 15, 2017 18:16
-
-
Save andywarburton/9a0cb7f42470a6d8f6ad04daf959c272 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
# coding: Latin-1 | |
# Load library functions we want | |
from inputs import get_gamepad | |
from explorerhat import motor | |
import math | |
import time | |
from blinkt import set_clear_on_exit, set_pixel, show, set_brightness | |
from threading import Thread | |
def mixer(inYaw, inThrottle,): | |
left = inThrottle + inYaw | |
right = inThrottle - inYaw | |
scaleLeft = abs(left / 125.0) | |
scaleRight = abs(right / 125.0) | |
scaleMax = max(scaleLeft, scaleRight) | |
scaleMax = max(1, scaleMax) | |
out_left = int(constrain(left / scaleMax, -125, 125)) | |
out_right = int(constrain(right / scaleMax, -125, 125)) | |
results = [out_right, out_left] | |
return results | |
def constrain(val, min_val, max_val): | |
return min(max_val, max(min_val, val)) | |
def larson_scanner(): | |
# setup larson scanner | |
set_clear_on_exit() | |
reds = [0, 0, 0, 0, 0, 16, 64, 255, 64, 16, 0, 0, 0, 0, 0] | |
start_time = time.time() | |
while True: | |
delta = (time.time() - start_time) * 16 | |
offset = int(abs((delta % 16) - 8)) | |
for i in range(8): | |
set_pixel(i , reds[offset + i], 0, 0) | |
show() | |
time.sleep(0.1) | |
def motor_control(): | |
# Setup motors | |
maxPower = 1.0 | |
power_left = 0.0 | |
power_right = 0.0 | |
x_axis = 0.0 | |
y_axis = 0.0 | |
while True: | |
events = get_gamepad() | |
for event in events: | |
print(event.code, event.state) | |
if event.code == "ABS_Y": | |
if event.state > 130: | |
print("Backwards") | |
elif event.state < 125: | |
print("Forward") | |
y_axis = event.state | |
if y_axis > 130: | |
y_axis = -(y_axis - 130) | |
elif y_axis < 125: | |
y_axis = ((-y_axis) + 125) | |
else: | |
y_axis = 0.0 | |
print("Y: " + str(-y_axis)) | |
if event.code == "ABS_Z": | |
if event.state > 130: | |
print("Right") | |
elif event.state < 125: | |
print("Left") | |
x_axis = event.state | |
if x_axis > 130: | |
x_axis = (x_axis - 130) | |
elif x_axis < 125: | |
x_axis = -((-x_axis) + 125) | |
else: | |
x_axis = 0.0 | |
print("X: " + str(x_axis)) | |
if event.code == "BTN_TL": | |
if event.state == True: | |
print("Botton Left") | |
if event.code == "BTN_TR": | |
if event.state == True: | |
print("Botton Right") | |
if event.code == "BTN_Z": | |
if event.state == True: | |
print("Top right") | |
if event.code == "BTN_WEST": | |
if event.state == True: | |
print("Top left") | |
if event.code == "BTN_TL2": | |
if event.state == True: | |
print("Select") | |
x_axis = 0 | |
y_axis = 0 | |
if event.code == "ABS_HAT0X": | |
if event.state == -1: | |
print("D pad Left") | |
elif event.state == 1: | |
print("D pad Right") | |
if event.code == "ABS_HAT0Y": | |
if event.state == -1: | |
print("D pad Up") | |
elif event.state == 1: | |
print("D pad Down") | |
mixer_results = mixer(x_axis, y_axis) | |
#print (mixer_results) | |
# print(event.ev_type, event.code, event.state) | |
power_left = int((-mixer_results[0] / 125.0)*100) | |
power_right = int((mixer_results[1] / 125.0)*100) | |
print("left: " + str(power_left) + " right: " + str(power_right)) | |
motor.one.speed((power_right * maxPower)) | |
motor.two.speed(power_left * maxPower) | |
try: | |
print('Press CTRL+C to quit') | |
t1 = Thread(target = motor_control) | |
t2 = Thread(target = larson_scanner) | |
t1.start() | |
t2.start() | |
t1.join() | |
t2.join() | |
except KeyboardInterrupt: | |
# CTRL+C exit, disable all drives | |
print("stop") | |
motor.stop() | |
print("bye") |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment