def pwmControlThread():
# setup arduino serial comm
pwm.setPWMFreq(50)
t = Transform(True, False) #invert one motor and not the other when constructing
watchdog = time.time()
# thread main loop
while True:
time.sleep(.005)
# check for data that needs to be bridged to arduino
dataFlag = True
if not serialRealTimeQueue.empty():
data = serialRealTimeQueue.get()
else:
dataFlag = False
# if data was recieved parse and update pwm hat
if dataFlag:
data = data[5:-1]
print data
data_nums = [int(x) for x in data.split(':') if x.strip()]
print " ", data_nums[0], " ", data_nums[1]
leftMtr,rightMtr = t.transform(data_nums[0],data_nums[1])
print " ", leftMtr , " ", rightMtr
setServoPulse(LEFT_MOT, leftMtr)
setServoPulse(RIGHT_MOT, rightMtr)
Transform.MOTOR_MIN = data_nums[5]*10
Transform.MOTOR_IDLE = data_nums[6]*10
Transform.MOTOR_MAX = data_nums[7]*10
leftIntake = Transform.MOTOR_IDLE
rightIntake = Transform.MOTOR_IDLE
if data_nums[2] > 127 + 10: # if left trigger pressed (i think) spin motors in opposite direction
leftIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
rightIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
elif data_nums[3] > 127 + 10: # if right trigger pressed
leftIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
rightIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
else :
leftIntake = Transform.MOTOR_IDLE
rightIntake = Transform.MOTOR_IDLE
print " " , leftIntake, " ", rightIntake
setServoPulse(LEFT_MANIP,leftIntake)
setServoPulse(RIGHT_MANIP,rightIntake)
watchdog = time.time()
if watchdog + WATCHDOG_DELAY < time.time():
setServoPulse(LEFT_MOT, Transform.MOTOR_IDLE)
setServoPulse(RIGHT_MOT, Transform.MOTOR_IDLE)
setServoPulse(LEFT_MANIP, Transform.MOTOR_IDLE)
setServoPulse(RIGHT_MANIP, Transform.MOTOR_IDLE)
watchdog = time.time()
print "you need to feed the dogs"
def pwmControlThread():
# setup arduino serial comm
pwm.setPWMFreq(50)
t = Transform(True, False)
watchdog = time.time()
# thread main loop
while True:
# check for data that needs to be bridged to arduino
dataFlag = True
if not serialRealTimeQueue.empty():
data = serialRealTimeQueue.get()
else:
dataFlag = False
# if data was recieved send it to arduino
if dataFlag:
data = data[5:-1]
print data
data_nums = [int(x) for x in data.split(':') if x.strip()]
#print "have data"
print " ", data_nums[0], " ", data_nums[1]
leftMtr, rightMtr = t.transform(data_nums[0], data_nums[1])
print " ", leftMtr, " ", rightMtr
setServoPulse(LEFT_MOT, leftMtr)
setServoPulse(RIGHT_MOT, rightMtr)
leftIntake = Transform.MOTOR_IDLE
rightIntake = Transform.MOTOR_IDLE
if data_nums[2] > 127 + 10:
leftIntake = Transform.map_range(data_nums[2], 127, 255,
Transform.MOTOR_IDLE,
Transform.MOTOR_MAX)
rightIntake = Transform.map_range(data_nums[2], 127, 255,
Transform.MOTOR_IDLE,
Transform.MOTOR_MIN)
elif data_nums[3] > 127 + 10:
rightIntake = Transform.map_range(data_nums[3], 127, 255,
Transform.MOTOR_IDLE,
Transform.MOTOR_MAX)
leftIntake = Transform.map_range(data_nums[3], 127, 255,
Transform.MOTOR_IDLE,
Transform.MOTOR_MIN)
print " ", leftIntake, " ", rightIntake
setServoPulse(LEFT_MANIP, leftIntake)
setServoPulse(RIGHT_MANIP, rightIntake)
watchdog = time.time()
if watchdog + WATCHDOG_DELAY < time.time():
setServoPulse(LEFT_MOT, Transform.MOTOR_IDLE)
setServoPulse(RIGHT_MOT, Transform.MOTOR_IDLE)
setServoPulse(LEFT_MANIP, Transform.MOTOR_IDLE)
setServoPulse(RIGHT_MANIP, Transform.MOTOR_IDLE)
watchdog = time.time()
print "you need to feed the dogs"
