from Maestro import Controller
x = Controller()
# 0 <- body
# 1 <- wheel
# 2 <- wheel
# 3 <- head left/right
# 4 <- head up/down
# forward 2400 - 1800
# reverse 6300 - 7100
x.setTarget(1, 6000)
x.setTarget(2, 6000)
x.setSpeed(1, 30)
x.setSpeed(2, 30)
x.setAccel(1, 150)
low_range = 4000
high_range = 5000
def take_instr(direction):
global low_range
global high_range
if direction == '8':
curr_pos = x.getPosition(4)
x.setTarget(4, curr_pos + 800)
class Move:
def __init__(self):
self.servo = Controller()
#self.root = tk.Tk()
os.system('xset r off')
#set targets to neutral
for i in range(17):
self.servo.setTarget(i, neutral)
self.servo.setSpeed(i, 0)
self.servo.setAccel(i, 60)
self.servo.setAccel(drive_target, 6)
#move head up
def w_pressed(self):
global head_angle
if head_angle < 9000:
head_angle += 1500
self.servo.setTarget(head_target, head_angle)
#move head down
def s_pressed(self):
global head_angle
if head_angle > 3000:
head_angle -= 1500
self.servo.setTarget(head_target, head_angle)
#move neck left
def a_pressed(self):
global neck_angle
if neck_angle < 9000:
neck_angle += 1500
self.servo.setTarget(neck_target, neck_angle)
#move neck right
def d_pressed(self):
global neck_angle
if neck_angle > 3000:
neck_angle -= 1500
self.servo.setTarget(neck_target, neck_angle)
#body turn left
def q_pressed(self):
self.servo.setTarget(body_target, 8600)
def q_released(self):
self.servo.setTarget(body_target, neutral)
#body turn right
def e_pressed(self):
self.servo.setTarget(body_target, 3400)
def e_released(self):
self.servo.setTarget(body_target, neutral)
#drive forward
def one_forward(self, seconds):
self.servo.setTarget(drive_target, one_forward)
time.sleep(seconds)
self.servo.setTarget(drive_target, neutral)
#drive backwards
def one_backward(self, seconds):
self.servo.setTarget(drive_target, one_backward)
time.sleep(seconds)
self.servo.setTarget(drive_target, neutral)
#turn left
def turn_left(self, seconds):
self.servo.setTarget(turn_target, 7000)
time.sleep(seconds)
self.servo.setTarget(turn_target, neutral)
#turn right
def turn_right(self, seconds):
self.servo.setTarget(turn_target, 5000)
time.sleep(seconds)
self.servo.setTarget(turn_target, neutral)
def fight(self):
self.servo.setTarget(head_target, head_angle)
self.servo.setTarget(6, 3000)
self.servo.setTarget(12, 3000)
time.sleep(.5)
self.servo.setTarget(head_target, neutral)
self.servo.setTarget(head_target, 3000)
self.servo.setTarget(6, 8000)
self.servo.setTarget(12, 8000)
time.sleep(.5)
self.servo.setTarget(head_target, neutral)
self.servo.setTarget(head_target, head_angle)
self.servo.setTarget(6, 3000)
self.servo.setTarget(12, 3000)
time.sleep(.5)
self.servo.setTarget(head_target, neutral)
self.servo.setTarget(head_target, 3000)
self.servo.setTarget(6, 8000)
self.servo.setTarget(12, 8000)
time.sleep(.5)
self.servo.setTarget(head_target, neutral)
self.servo.setTarget(head_target, head_angle)
self.servo.setTarget(6, neutral)
self.servo.setTarget(12, neutral)
def recharge(self):
self.servo.setTarget(neck_target, head_angle)
self.servo.setTarget(9, 3000)
self.servo.setTarget(16, 3000)
time.sleep(.5)
self.servo.setTarget(neck_target, neutral)
self.servo.setTarget(neck_target, 3000)
self.servo.setTarget(9, 8000)
self.servo.setTarget(16, 8000)
time.sleep(.5)
self.servo.setTarget(neck_target, neutral)
self.servo.setTarget(neck_target, head_angle)
self.servo.setTarget(9, 3000)
self.servo.setTarget(16, 3000)
time.sleep(.5)
self.servo.setTarget(neck_target, neutral)
self.servo.setTarget(neck_target, 3000)
self.servo.setTarget(9, 8000)
self.servo.setTarget(16, 8000)
time.sleep(.5)
self.servo.setTarget(neck_target, neutral)
self.servo.setTarget(neck_target, head_angle)
self.servo.setTarget(9, neutral)
self.servo.setTarget(16, neutral)
#set all servos/motors to neutral
def space_pressed(self):
for i in range(5):
self.servo.setTarget(i, neutral)
def executeMotion(self, mid, direction, seconds):
#HEAD
if mid == 1:
#if up
if direction == 1:
#move head up
self.w_pressed()
else:
#move head down
self.s_pressed()
#NECK
elif mid == 2:
if direction == 1:
#neck turn left
self.a_pressed()
else:
#neck turn right
self.d_pressed()
#BODY
elif mid == 3:
if direction == 1:
#body turn left
self.q_pressed()
else:
#body turn right
self.e_pressed()
#DRIVE
elif mid == 4:
if direction == 1:
#drive forward
self.one_forward(seconds)
else:
#drive backward
self.one_backward(seconds)
#TURN
elif mid == 5:
if direction == 1:
self.turn_left(seconds)
else:
self.turn_right(seconds)
#PAUSE
elif mid == 6:
time.sleep(seconds)
class Drivetrain:
Fast = 1500
Medium = 950
Slow = 650
def __init__(self, rotateChannel, forwardChannel, controller):
self._forwardChannel = forwardChannel
self.controller = controller
self.controller = Controller()
self.rotate = 0
self.forward = 0
self.enable = False
self.moveForward = False
self.moveBackward = False
self.rotateLeft = False
self.rotateRight = False
self.speed = Drivetrain.Slow
self.stop = False
self._rotateChannel = rotateChannel
def loop(self):
while (self.enable):
self.execute()
time.sleep(.02)
def execute(self, rotation=None, forward=None):
if rotation is not None:
self.rotate = rotation
if forward is not None:
self.forward = forward
tempForward = 6000
tempRotate = 6000
if self.moveForward:
tempForward += self.speed
print(str(tempForward))
if self.moveBackward:
tempForward -= self.speed
if self.rotateLeft:
tempRotate += self.speed
if self.rotateRight:
tempRotate -= self.speed
if self.stop:
print("Stopping drivetrain")
tempForward = 6000
tempRotate = 6000
self.forward = tempForward
self.rotate = tempRotate
self.controller.setSpeed(self._rotateChannel, self.rotate)
self.controller.setSpeed(self._forwardChannel, self.forward)
def run(self):
self.enable = True
# Create new thread here
self.thread = threading.Thread(None, self.loop)
self.thread.start()
def disable(self):
self.enable = False
# When disabled, set movement values to 0 and update
self.controller.setSpeed(self._rotateChannel, 0)
self.controller.setSpeed(self._forwardChannel, 0)
print(" K - Tilt head down\n")
print(" C - Center robot, reset all motors:")
print(" Q - Quit, disable robot\n")
print("BEGIN CONTROL\n\n")
# WHILE condition to check key
while key != "q":
# Get key
key = getch()
# Move the bot
move(key)
# Reset position when we are done
x.setSpeed(0, 60)
x.setSpeed(1, 50)
x.setSpeed(2, 50)
x.setSpeed(3, 60)
x.setSpeed(4, 60)
x.setAccel(1, 150)
x.setAccel(2, 150)
x.setTarget(0, 6000)
x.setTarget(1, 6000)
x.setTarget(2, 6000)
x.setTarget(3, 6000)
x.setTarget(4, 6000)
print("Robot reset; program ended.")