# Full Obstruction # #################### # Each front_sensor returns 1 if obstructed_threshold is surpassed # If either one returns 1, full obstruction front_obstructed = front_sensor( obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor( obstructed_threshold, FRONT_ADC_INPUT_2) # If both motors have zero speed, full obstruction left_right_obstructed = ((uLeft == 0) and (uRight == 0)) if ((left_right_obstructed > 0) or (front_obstructed > 0)): # If fully obstructed, rotate in place client_sock.send("Something's in the way. I'm relocating.") setSpeed(ser, 0, 0, 127) setSpeed(ser, 1, 1, 127) else: # If not fully obstructed, proceed with PID control client_sock.send("Now tracking.") setSpeed(ser, 0, 1, int(uRight * 127 * biasRight)) setSpeed(ser, 1, 1, int(uLeft * 127 * biasLeft)) time.sleep(0.1) end = time.time() client_sock.send("I've been tracking for %s seconds now." % str(end - start)) # Return to scan mode
#################### # Full Obstruction # #################### # Each front_sensor returns 1 if obstructed_threshold is surpassed # If either one returns 1, full obstruction front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2) # If both motors have zero speed, full obstruction left_right_obstructed = ((uLeft == 0) and (uRight == 0)) if((left_right_obstructed > 0) or (front_obstructed > 0)): # If fully obstructed, rotate in place client_sock.send("Something's in the way. I'm relocating.") setSpeed(ser, 0, 0, 127) setSpeed(ser, 1, 1, 127) else: # If not fully obstructed, proceed with PID control client_sock.send("Now tracking.") setSpeed(ser, 0, 1, int(uRight*127*biasRight)) setSpeed(ser, 1, 1, int(uLeft*127*biasLeft)) time.sleep(0.1) end = time.time() client_sock.send("I've been tracking for %s seconds now." % str(end - start)) # Return to scan mode client_sock.send("Now scanning.")
import serial from setSpeed import setSpeed ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=1) setSpeed(ser, 0, 0, 0) setSpeed(ser, 1, 0, 0)
uLeft, errorLeft = PID(kp, ki, kd, n, uLeft, errorLeft, ideal_value, LEFT_ADC_INPUT) # left control uRight, errorRight = PID(kp, ki, kd, n, uRight, errorRight, ideal_value, RIGHT_ADC_INPUT) # right control print ("uLeft: %f \n") % (uLeft) print ("uRight: %f \n") % (uRight) print ("errorLeft: %s \n") % (str(errorLeft)) print ("errorRight: %s \n") % (str(errorRight)) print ("n: %i \n") % (n) print "--------------------------\n" n = n + 1 front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2) left_right_obstructed = ((uLeft == 0) and (uRight == 0)) if((left_right_obstructed > 0) or (front_obstructed > 0)): setSpeed(ser, 0, 0, 127) setSpeed(ser, 1, 1, 127) front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2) left_right_obstructed = front_sensor(obstructed_threshold, RIGHT_ADC_INPUT) + front_sensor(obstructed_threshold, LEFT_ADC_INPUT) # time.sleep(0.5) else: setSpeed(ser, 0, 1, int(uRight*127)) setSpeed(ser, 1, 1, int(uLeft*127)) """ setSpeed(ser, 0, 1, int(uRight*127)) setSpeed(ser, 1, 1, int(uLeft*127)) """ time.sleep(0.1)
import serial from setSpeed import setSpeed ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) setSpeed(ser, 0, 0, 0) setSpeed(ser, 1, 0, 0)