current_twist = yellow_twist
current_clamp_value = latest_yellow_clamp_value
current_twist_value = latest_yellow_twist_value
print("\rControl changed to Yellow")
elif input_key == '4':
current_clamp = red_clamp
current_twist = red_twist
current_clamp_value = latest_red_clamp_value
current_twist_value = latest_red_twist_value
print("\rControl changed to Red")
#movement commands
elif input_key == 'a':
current_twist_value -= 2
pwm.setPWM(current_twist, 0, current_twist_value)
elif input_key == 'd':
current_twist_value += 2
pwm.setPWM(current_twist, 0, current_twist_value)
elif input_key == 'x':
current_clamp_value -= 2
pwm.setPWM(current_clamp, 0, current_clamp_value)
elif input_key == 'w':
current_clamp_value += 2
pwm.setPWM(current_clamp, 0, current_clamp_value)
elif input_key == 'f':
if current_twist == 1:
whiteTwistCW = params[4]
blueClampClosed = params[5]
blueClampOpen = params[6]
blueTwistNeutral = params[7]
blueTwistCCW = params[8]
blueTwistCW = params[9]
yellowClampClosed = params[10]
yellowClampOpen = params[11]
yellowTwistNeutral = params[12]
yellowTwistCCW = params[13]
yellowTwistCW = params[14]
redClampClosed = params[15]
redClampOpen = params[16]
redTwistNeutral = params[17]
redTwistCCW = params[18]
redTwistCW = params[19]
pwm.setPWM(whiteClamp, 0, whiteClampClosed)
time.sleep(0.35)
pwm.setPWM(blueClamp, 0, blueClampClosed)
time.sleep(0.35)
pwm.setPWM(yellowClamp, 0, yellowClampClosed)
time.sleep(0.35)
pwm.setPWM(redClamp, 0, redClampClosed)
time.sleep(0.35)
else:
print("\rMissing Calibration File, run calibrate.py first")
pwm.softwareReset()
highest = -1
if whiteClampClosed < lowest:
lowest = whiteClampClosed
if blueClampClosed < lowest:
lowest = blueClampClosed
if yellowClampClosed < lowest:
lowest = yellowClampClosed
if redClampClosed < lowest:
lowest = redClampClosed
if whiteClampOpen > highest:
highest = whiteClampOpen
if blueClampOpen > highest:
highest = blueClampOpen
if yellowClampOpen > highest:
highest = yellowClampOpen
if redClampOpen > highest:
highest = redClampOpen
for i in range(lowest, highest, 5):
pwm.setPWM(whiteClamp, 0, i)
pwm.setPWM(blueClamp, 0, i)
pwm.setPWM(yellowClamp, 0, i)
pwm.setPWM(redClamp, 0, i)
else:
print("\rMissing Calibration File, run calibrate.py first")
pwm.softwareReset()