def openTimer(self, duration):
'''
Fully opens pinch valve for specified time in seconds
'''
pz.setMotor(self.motor_pin, self.max_power)
sleep(duration)
pz.setMotor(self.motor_pin, 0)
def handle_command(self, line):
if line != "":
sl = 0
sr = 0
if line == 'rdlt':
answer = 'rdlt ' + str(sl + 100) + '\n'
self.send(answer)
print answer
elif line == 'rdrt':
answer = 'rdrt ' + str(sr + 100) + '\n'
self.send(answer)
print answer
elif line == 'rdus':
us = int(hcsr04.getDistance())
if us > 100:
us = 100
answer = 'rdus ' + str(us) + '\n'
self.send(answer)
print answer
else:
c, p = line.split()
print line.split()
if c == 'lt':
self.send('\n')
print 'motor left ', p
sl = int(p) - 100
pz.setMotor(0, sl)
elif c == 'rt':
self.send('\n')
print 'motor right ', p
sr = int(p) - 100
pz.setMotor(1, sr)
else:
self.send('unknown command\n')
print 'Unknown command:', line
def fw(direction=1, duration=0.1, speed=100):
pz.setMotor(0, speed * -direction)
pz.setMotor(1, speed * -direction)
Movement.checkCross()
time.sleep(duration)
pz.stop() # stop motors after each movement
def set_speeds(power_left, power_right):
motor_left = 0 # Motor A
motor_right = 1 # Motor B
pz.setMotor(motor_left, power_left)
pz.setMotor(motor_right, power_right)
# print('Left: {}, Right {}'.format(power_left, power_right))
# We need a sleep here otherwise the controller code goes a bit awol
sleep(0.1)
def move(self, directionLeft, directionRight, speedLeft, speedRight):
self.setDirection(directionLeft, directionRight)
if (speedLeft >= 0 and speedRight >= 0 and speedLeft <= 1
and speedRight <= 1):
if (directionLeft == self.DIRECTION_FORWARD):
print("Forward Left {}, {}".format(directionLeft,
speedLeft * 123))
pz.setMotor(0, int(speedLeft * 123))
elif (directionLeft == self.DIRECTION_BACKWARD):
print("Backward Left {}, {}".format(directionLeft,
-1 * speedLeft * 123))
pz.setMotor(0, int(-1 * speedLeft * 123))
if (directionRight == self.DIRECTION_FORWARD):
print("Forward Right {}, {}".format(directionRight,
speedRight * 123))
pz.setMotor(1, int(speedRight * 123))
elif (directionRight == self.DIRECTION_BACKWARD):
print("Backward Right {}, {}".format(directionRight,
-1 * speedRight * 123))
pz.setMotor(1, int(-1 * speedRight * 123))
else:
print("Invalid values")
def set_right(self, right_speed):
pz.setMotor(1, max(min(int(right_speed), 100), -100))
def set_left(self, left_speed):
pz.setMotor(0, max(min(int(left_speed), 100), -100))
def set_motors(self, left_speed, right_speed):
pz.setMotor(0, max(min(int(left_speed), 100), -100))
pz.setMotor(1, max(min(int(right_speed), 100), -100))
left_stick = 0.0
print("Y: " + str(-left_stick))
# right stick
if event.code == "ABS_RZ":
right_stick = event.state
if right_stick > 130:
right_stick = -(right_stick - 130)
elif right_stick < 125:
right_stick = ((-right_stick) + 125)
else:
right_stick = 0.0
print("Y: " + str(-right_stick))
# engage the motors
power_left = int((left_stick / 125.0) * 100)
power_right = int((right_stick / 125.0) * 100)
pz.setMotor(0, power_left)
pz.setMotor(1, power_right)
# print(event.ev_type, event.code, event.state)
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
print("stop")
pz.setAllPixels(0, 0, 0, True)
pz.stop()
pz.cleanup()
print("bye")
def remotecontrolcomplex():
print ("Remote Control Program Active")
mylcd.lcd_display_string("Remote Control C", 1)
mylcd.lcd_display_string("Select Ends", 2)
time.sleep(2)
RUN = 1
while RUN == 1:
x = joystick.axes[0].corrected_value()
y = joystick.axes[1].corrected_value()
r = 0
l = 0
buttons_pressed = joystick.get_and_clear_button_press_history()
#mylcd.lcd_display_string("X: %d " % r, 1)
#mylcd.lcd_display_string("Y: %d " % l, 2)
if buttons_pressed & 1 << SixAxis.BUTTON_SELECT:
RUN = 0
elif 0.1 >= x >= -0.1 and 0.1 >= y >= -0.1: #stop
x = abs(x)
y = abs(y)
r = 0
l = 0
pz.setMotor(0,l)
pz.setMotor(1,r)
elif 0.1 >= x >= -0.1 and y <= -0.1: #full speed forwards
x = abs(x)
y = abs(y)
r = int(100 * y)
l = int(100 * y)
pz.setMotor(0,l)
pz.setMotor(1,r)
elif 0.1 >= x >= -0.1 and y >= 0.1: #full speed backwards
x = abs(x)
y = abs(y)
r = int(100 * y)
l = int(100 * y)
pz.setMotor(0,l)
pz.setMotor(1,r)
elif x <= -0.1 and 0.1 >= y >= -0.1: #spin right
x = abs(x)
y = abs(y)
r = int(-100 * x)
l = int(100 * x)
pz.setMotor(0,l)
pz.setMotor(1,r)
elif x >= 0.1 and 0.1 >= y >= -0.1: #spin left
x = abs(x)
y = abs(y)
r = int(100 * x)
l = int(-100 * x)
pz.setMotor(0,l)
pz.setMotor(1,r)
elif -0.9 < x < -0.1 and -0.9 < y < -0.1: #turnR - forwards
x = abs(x)
y = abs(y)
r = int(100 * x * (1-y))
l = int(100 * x)
pz.setMotor(0,l)
pz.setMotor(1,r)
elif 0.9 > x > 0.1 and -0.9 < y < -0.1: #turnL - forwards
x = abs(x)
y = abs(y)
r = int(100 * x)
l = int(100 * x * (1-y))
pz.setMotor(0,l)
pz.setMotor(1,r)
elif 0.9 > x > 0.1 and 0.1 > y > 0.1: #turnL - backwards
x = abs(x)
y = abs(y)
r = int(-100 * x)
l = int(-100 * x * (1-y))
pz.setMotor(0,l)
pz.setMotor(1,r)
elif x < -0.1 and y > 0.1: #turnR - backwards
x = abs(x)
y = abs(y)
r = int(-100 * x * (1-y))
l = int(-100 * x )
pz.setMotor(0,l)
pz.setMotor(1,r)
#swap these if it turns the wrong way
leftmotor = 0
rightmotor = 1
while True:
pygame.event.pump()
x = pad.get_axis(0)
y = pad.get_axis(1)
elif 0.1 >= x >= -0.1 and 0.1 >= y >= -0.1: #stop
x = abs(x)
y = abs(y)
r = 0
l = 0
pz.setMotor(leftmotor,l)
pz.setMotor(rightmotor,r)
elif 0.1 >= x >= -0.1 and y <= -0.1: #full speed forwards
x = abs(x)
y = abs(y)
r = 100 * y
l = 100 * y
pz.setMotor(leftmotor,l)
pz.setMotor(rightmotor,r)
elif 0.1 >= x >= -0.1 and y >= 0.1: #full speed backwards
x = abs(x)
y = abs(y)
r = 100 * y
l = 100 * y
pz.setMotor(leftmotor,l)
pz.setMotor(rightmotor,r)
# Apply steering speeds
if not joystick.get_button(buttonFastTurn):
leftRight *= 0.5
# Determine the drive power levels
driveLeft = -upDown
driveRight = -upDown
if leftRight < -0.05:
# Turning left
driveLeft *= 1.0 + (2.0 * leftRight)
elif leftRight > 0.05:
# Turning right
driveRight *= 1.0 - (2.0 * leftRight)
# Check for button presses
if joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
pz.setMotor(0,int((driveLeft * maxPower)*100))
pz.setMotor(1,int((driveRight * maxPower)*100))
# Change the LED to reflect the status of the EPO latch
# Wait for the interval period
time.sleep(interval)
# Disable all drives
pz.stop()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
pz.stop()
print
print("Use . or > to speed up")
print("Left track - 1 back,4 stop,7 for")
print("Right track - 3 back,6 stop ,9 for")
print("Space to stop")
print("Speed changes take effect when the next arrow key is pressed")
print("Press Ctrl-C to end")
print()
pz.init()
# main loop
try:
while True:
keyp = readkey()
if keyp == "1":
pz.setMotor(0, -speed)
print("Motor 0 back", speed)
elif keyp == "4":
pz.setMotor(0, 0)
print("Motor 0 stop")
elif keyp == "7":
pz.setMotor(0, speed)
print("Motor 0 forward", speed)
elif keyp == "3":
pz.setMotor(1, -speed)
print("Motor 1 back", speed)
elif keyp == "6":
pz.setMotor(1, 0)
print("Motor 1 stop")
elif keyp == "9":
pz.setMotor(1, speed)
def fineremotecontrol():
RUN = 1
mylcd.lcd_display_string("Remote Control F", 1)
mylcd.lcd_display_string("Select Ends ", 2)
time.sleep(2)
LEFTMOTOR = 1
RIGHTMOTOR = 0
while RUN == 1:
pygame.event.pump()
x = pad.get_axis(0)
y = pad.get_axis(1)
mylcd.lcd_display_string("X: %f " % x, 1)
mylcd.lcd_display_string("Y: %f " % y, 2)
if pad.get_button(0) == 1: #exit program
RUN = 0
elif 0.1 >= x >= -0.1 and 0.1 >= y >= -0.1: #stop
x = abs(x)
y = abs(y)
r = 0
l = 0
pz.stop()
elif 0.1 >= x >= -0.1 and y <= -0.1: #full speed forwards
x = abs(x)
y = abs(y)
r = int(100 * y)
l = int(100 * y)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif 0.1 >= x >= -0.1 and y >= 0.1: #full speed backwards
x = abs(x)
y = abs(y)
r = int(100 * y)
l = int(100 * y)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif x <= -0.1 and 0.1 >= y >= -0.1: #spin right
x = abs(x)
y = abs(y)
r = int(-100 * x)
l = int(100 * x)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif x >= 0.1 and 0.1 >= y >= -0.1: #spin left
x = abs(x)
y = abs(y)
r = int(100 * x)
l = int(-100 * x)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif -0.9 < x < -0.1 and -0.9 < y < -0.1: #turnR - forwards
x = abs(x)
y = abs(y)
r = int(100 * x * (1 - y))
l = int(100 * x)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif 0.9 > x > 0.1 and -0.9 < y < -0.1: #turnL - forwards
x = abs(x)
y = abs(y)
r = int(100 * x)
l = int(100 * x * (1 - y))
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif 0.9 > x > 0.1 and 0.1 > y > 0.1: #turnL - backwards
x = abs(x)
y = abs(y)
r = int(-100 * x)
l = int(-100 * x * (1 - y))
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
elif x < -0.1 and y > 0.1: #turnR - backwards
x = abs(x)
y = abs(y)
r = int(-100 * x * (1 - y))
l = int(-100 * x)
pz.setMotor(LEFTMOTOR, l)
pz.setMotor(RIGHTMOTOR, r)
time.sleep(0.1)
print("Tests the motors by using the arrow keys to control")
print("Use , or < to slow down")
print("Use . or > to speed up")
print("Speed changes take effect when the next arrow key is pressed")
print("Press Ctrl-C to end")
print()
pz.init()
# main loop
try:
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
pz.setMotor(1, speed)
pz.setMotor(0, speed)
# print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
pz.reverse(speed)
# print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
pz.spinRight(speed)
# print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
pz.spinLeft(speed)
# print 'Spin Left', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed + 10)
# print 'Speed+', speed
elif keyp == ',' or keyp == '<':
