def main(): ''' Main program to test I2C communication on the I2C ''' global SLEEP_TIME global TACH_THRESHOLD global rightTachValue global leftTachValue global prevRightTachValue global prevLeftTachValue global rightVelocity global rightStripCount global leftVelocity global leftStripCount global loopCount global leftHigh global leftThresholdHigh global leftThresholdLow global rightHigh global rightThresholdHigh global rightThresholdLow global steeringPotValue global startTime global elapsedTime global fractional global turnGoal # Setup bus pwm = Adafruit_PCA9685.PCA9685() adc = Adafruit_ADS1x15.ADS1115() # Thread goalThread = Thread(target=recvGoal) goalThread.start() try: values = [0] * 4 # motor motorChnl = 14 turnChnl = 15 freq = 60 * 2 pwm.set_pwm_freq(freq) print("Freq = {0}".format(freq)) rightWheelChnl = 1 leftWheelChnl = 2 potChnl = 0 # Data Rate of ADC DATA_RATE = 860 pulseDuration = STOP error = 0.0 avgVelocity = 0.0 steeringError = 0.0 steeringDuration = STOP turnGoal = STRAIGHT # straight steeringAvg = 0.0 # slow turn #steeringPID = PID(0.0, 0.5, 0.5, 25.0) steeringPID = PID(10.0, 4.0, 1.0, 25.0) steeringFilter = Filter(0.9) velocityPID = PID(50.0, 100.0, 0.0, 3.0) velocityFilter = Filter(0.9) velocityPID.setGoal(0.0) while True: if (loopCount % 1 == 0): prevRightTachValue = rightTachValue prevLeftTachValue = leftTachValue steeringPotValue = adc.read_adc(potChnl, gain=GAIN, data_rate=DATA_RATE) rightTachValue = adc.read_adc(rightWheelChnl, gain=GAIN, data_rate=DATA_RATE) leftTachValue = adc.read_adc(leftWheelChnl, gain=GAIN, data_rate=DATA_RATE) steeringFilter.recvMeasurement(steeringPotValue) steeringPotValue = steeringFilter.filter() #steeringPID.setCurrentMeasurement((steeringAvg / MAX_LOOP_COUNT) / 1000.0) steeringPID.setCurrentMeasurement(steeringPotValue / 1000.0) steeringPID.setGoal(turnGoal / 1000.0) #print("LT: {0}".format(rightTachValue)) #print("RT: {0}".format(leftTachValue)) #print("ST: {0}".format(steeringPotValue)) if (rightHigh == 0 and rightTachValue > rightThresholdHigh): rightStripCount += 0.5 rightHigh = 1 if (rightHigh == 1 and rightTachValue < rightThresholdHigh): rightStripCount += 0.5 rightHigh = 0 if (leftHigh == 0 and leftTachValue > leftThresholdHigh): leftStripCount += 0.5 leftHigh = 1 if (leftHigh == 1 and leftTachValue < leftThresholdHigh): leftStripCount += 0.5 leftHigh = 0 #steeringAvg += steeringPotValue loopCount += 1 #print(loopCount) # Steering steeringDuration = steeringPID.control() #steeringError = turnGoal - steeringAvg / MAX_LOOP_COUNT #steeringGain = 100 if steeringDuration > 0: #steeringDuration = 670 # Dead Band steeringDuration = 675 - steeringDuration elif steeringDuration < 0: #steeringDuration = 890 steeringDuration = 875 - steeringDuration #steeringDuration = 775 - steeringError / 7.0 # Max Zone if steeringDuration > 930: steeringDuration = 930 if steeringDuration < 630: steeringDuration = 630 controlChnl(pwm, motorChnl, int(pulseDuration)) controlChnl(pwm, turnChnl, int(steeringDuration)) if (loopCount >= MAX_LOOP_COUNT): elapsedTime = (time.time() * 1000) - startTime startTime = (time.time() * 1000) rightVelocity = ((rightStripCount / 30.0) * 0.36 * math.pi) / (elapsedTime / 1000.0) leftVelocity = ((leftStripCount / 30.0) * 0.36 * math.pi) / (elapsedTime / 1000.0) # Velocity avgVelocity = (leftVelocity + rightVelocity) / 2.0 if pulseDuration > STOP: avgVelocity *= -1 #error = velGoal - avgVelocity # gain #pulseDuration = pulseDuration - error * 60.0 velocityFilter.recvMeasurement(avgVelocity) avgVelocity = velocityFilter.filter() velocityPID.setCurrentMeasurement(avgVelocity) velocityPID.setGoal(velGoal) pulseDuration = STOP - velocityPID.control() if pulseDuration < 550: pulseDuration = 550 if pulseDuration > 1000: pulseDuration = 1000 ''' # Steering steeringDuration = steeringPID.control() #steeringError = turnGoal - steeringAvg / MAX_LOOP_COUNT #steeringGain = 100 if steeringDuration > 0: #steeringDuration = 670 steeringDuration = 648 - steeringDuration elif steeringDuration < 0: #steeringDuration = 890 steeringDuration = 907 - steeringDuration #steeringDuration = 775 - steeringError / 7.0 # Dead Zone if steeringDuration > 930: steeringDuration = 930 if steeringDuration < 630: steeringDuration = 630 ''' print("LV: {0}".format(leftVelocity)) print("RV: {0}".format(rightVelocity)) print("Avg Vel: {0}".format(avgVelocity)) print("S: {0}".format(steeringPotValue)) print("ET: {0}".format(elapsedTime)) print("PD: {0}".format(int(pulseDuration))) print("VG: {0}".format(velGoal)) print("error: {0}".format(error)) print("SD: {0}".format(int(steeringDuration))) print("SG: {0}".format(turnGoal)) print("steering error: {0}".format(steeringError)) print('Steering PID:') print(steeringPID) print('Velocity PID:') print(velocityPID) print('') loopCount = 0 rightStripCount = 0 leftStripCount = 0 steeringAvg = 0.0 #time.sleep(SLEEP_TIME); #for pulse in [ i for i in range(450, 1000) if i % 5 == 0 ]: #controlChnl(pwm, motorChnl, STOP) # tach # Read all ADC values #for pulse in [ i for i in range(450, 1000) if i % 5 == 0 ]: # controlChnl(pwm, motorChnl, pulse) # for i in range(4): # values[i] = adc.read_adc(i, gain=GAIN) # print("values[{0}] = {1}".format(i, values[i])) # time.sleep(1) # Reads ADC values #while True: # for i in range(4): # values[i] = adc.read_adc(i, gain=GAIN) # print("values[{0}] = {1}".format(i, values[i])) # print('') # time.sleep(1) finally: ramp(pwm, motorChnl, pulseDuration, STOP, 1, 5) #ramp(pwm, motorChnl, pulseDuration, STOP) ramp(pwm, turnChnl, steeringDuration, STOP, 1, 5) #print("ramp done") controlChnl(pwm, motorChnl, STOP) controlChnl(pwm, turnChnl, STOP) goalThread.join(timeout=2)
def main(): ''' Hacky version of our contigency plan ''' #------Variable Declarations------ # Tachometer Specific Data rightTachValue = 0 leftTachValue = 0 prevRightTachValue = 0 prevLeftTachValue = 0 rightVelocity = 0 rightStripCount = 0 leftVelocity = 0 leftStripCount = 0 loopCount = 0 leftHigh = 0 leftThresholdHigh = 8500 leftThresholdLow = 6500 rightHigh = 0 rightThresholdHigh = 10000 rightThresholdLow = 8000 steeringPotValue = 0 distTraveled = 0.0 distStartTime = 0.0 distEndTime = 0.0 # Timer startTime = 0 elapsedTime = 0 fractional = 0 # Steering / Turning Setup turnGoal = STRAIGHT steeringAvg = 0.0 steeringPID = PID(1.0, 0.2, 0.0, 3.0) steeringFilter = Filter(0.9) steeringPID.setGoal(turnGoal) # Velocity Setup velDuration = STOP avgVelocity = 0.0 velGoal = 0.5 velocityPID = PID(50.0, 100.0, 0.0, 3.0) velocityFilter = Filter(0.9) velocityPID.setGoal(velGoal) # Distance sensors leftDistSensor = DistanceSensor(echo=DIST_LEFT_ECHO_PIN, trigger=DIST_LEFT_TRIGGER_PIN, max_distance=DIST_MAX_DISTANCE, queue_len=DIST_QUEUE_LENGTH) rightDistSensor = DistanceSensor(echo=DIST_RIGHT_ECHO_PIN, trigger=DIST_RIGHT_TRIGGER_PIN, max_distance=DIST_MAX_DISTANCE, queue_len=DIST_QUEUE_LENGTH) distFilter = Filter(0.8) leftDist = 0.0 rightDist = 0.0 # Line Following Setup wallFollowPID = PID(10000.0, 0.0, 0.0, 0.8) wallFollowPID.setGoal(MAX_WALL_DIST) #--------------------------------- # Setup bus pwm = Adafruit_PCA9685.PCA9685() adc = Adafruit_ADS1x15.ADS1115() try: pwm.set_pwm_freq(FREQ) print("Start") distStartTime = time.time() while True: if (loopCount % 1 == 0): prevRightTachValue = rightTachValue prevLeftTachValue = leftTachValue steeringPotValue = adc.read_adc(POT_CHNL, gain=GAIN, data_rate=DATA_RATE) rightTachValue = adc.read_adc(RIGHT_WHEEL_CHNL, gain=GAIN, data_rate=DATA_RATE) leftTachValue = adc.read_adc(LEFT_WHEEL_CHNL, gain=GAIN, data_rate=DATA_RATE) distTraveled += (avgVelocity / (time.time() - distStartTime)) #distFilter.recvMeasurement(rightDistSensor.distance) #rightDist = distFilter.filter() #distFilter.recvMeasurement(leftDistSensor.distance) #leftDist = distFilter.filter() rightDist = rightDistSensor.distance leftDist = leftDistSensor.distance #print('Left Distance: ', leftDistSensor.distance * 100) #print('Right Distance: ', rightDistSensor.distance * 100) # Wall follow PID wallFollowPID.setCurrentMeasurement(rightDist) turnGoal = (wallFollowPID.control() + STRAIGHT) steeringFilter.recvMeasurement(steeringPotValue) steeringPotValue = steeringFilter.filter() steeringPID.setCurrentMeasurement(steeringPotValue / 1000.0) steeringPID.setGoal(turnGoal / 1000.0) if (rightHigh == 0 and rightTachValue > rightThresholdHigh): rightStripCount += 0.5 rightHigh = 1 if (rightHigh == 1 and rightTachValue < rightThresholdHigh): rightStripCount += 0.5 rightHigh = 0 if (leftHigh == 0 and leftTachValue > leftThresholdHigh): leftStripCount += 0.5 leftHigh = 1 if (leftHigh == 1 and leftTachValue < leftThresholdHigh): leftStripCount += 0.5 leftHigh = 0 loopCount += 1 # Steering steeringDuration = -steeringPID.control() + 0.5 ''' if steeringDuration > 0: #steeringDuration = 670 # Dead Band steeringDuration = 675 - steeringDuration elif steeringDuration < 0: #steeringDuration = 890 steeringDuration = 875 - steeringDuration # Max Zone if steeringDuration > 930: steeringDuration = 930 if steeringDuration < 630: steeringDuration = 630 ''' controlChnl(pwm, MOTOR_CHNL, velDuration) #controlChnl(pwm, MOTOR_CHNL, 0.75) #print("SD: {0}".format(steeringDuration)) controlChnl(pwm, TURN_CHNL, steeringDuration) #controlChnl(pwm, TURN_CHNL, 0.99) #controlChnl(pwm, TURN_CHNL, 930) #controlChnl(pwm, TURN_CHNL, 0.0) #controlChnl(pwm, TURN_CHNL, 780) #controlChnl(pwm, TURN_CHNL, velDuration) #controlChnl(pwm, TURN_CHNL, s) #print("VD: {0}".format(int(velDuration))) #print("SD: {0}".format(int(steeringDuration))) if (loopCount >= MAX_LOOP_COUNT): elapsedTime = (time.time() * 1000) - startTime startTime = (time.time() * 1000) rightVelocity = ((rightStripCount / 30.0) * 0.36 * math.pi) / (elapsedTime / 1000.0) leftVelocity = ((leftStripCount / 30.0) * 0.36 * math.pi) / (elapsedTime / 1000.0) # Velocity avgVelocity = (leftVelocity + rightVelocity) / 2.0 if velDuration > STOP: avgVelocity *= -1 velocityFilter.recvMeasurement(avgVelocity) avgVelocity = velocityFilter.filter() velocityPID.setCurrentMeasurement(avgVelocity) velocityPID.setGoal(velGoal) velDuration = STOP - velocityPID.control() #if velDuration < 550: # velDuration = 550 #if velDuration > 1000: # velDuration = 1000 print('Total Distance: ', distTraveled) print('Left Distance: ', leftDistSensor.distance * 100) print('Right Distance: ', rightDistSensor.distance * 100) print("LDist: {0}".format(leftDist)) print("RDist: {0}".format(rightDist)) print("LDist: {0}".format(leftDistSensor.distance * 100)) print("RDist: {0}".format(rightDistSensor.distance * 100)) print("LV: {0}".format(leftVelocity)) print("RV: {0}".format(rightVelocity)) print("Avg Vel: {0}".format(avgVelocity)) print("S: {0}".format(steeringPotValue)) print("ET: {0}".format(elapsedTime)) print("VD: {0}".format(velDuration)) print("VG: {0}".format(velGoal)) print("SD: {0}".format(steeringDuration)) print("SG: {0}".format(turnGoal)) print('Steering PID:') print(steeringPID) print('Velocity PID:') print(velocityPID) print('Wall Follow PID:') print(wallFollowPID) print('') loopCount = 0 rightStripCount = 0 leftStripCount = 0 steeringAvg = 0.0 finally: #ramp(pwm, MOTOR_CHNL, velDuration, STOP, 1, 5) #ramp(pwm, TURN_CHNL, steeringDuration, STOP, 1, 5) #controlChnl(pwm, MOTOR_CHNL, STOP) #controlChnl(pwm, TURN_CHNL, STOP) pass