Stap 3:
#!/usr/bin/python import mraa import time PWM_PIN = 3 ADC_PIN = 0 RF = 6 # Right motor Forward RB = 7 # Right motor Back LF = 4 # Left motor Forward LB = 5 # Left motor Back P_CENTER = 1373 # for VSTONE VS-95647 servo P_STEP = 157 # or 79 ; uS at 50Hz P_MAX = 2 # or 3 def StopMotors() : m_rf.write(0) m_rb.write(0) m_lf.write(0) m_lb.write(0) def GoStraight() : m_rf.write(1) m_lf.write(1) time.sleep(0.3) StopMotors() def TurnLeft(times): m_lf.write(1) time.sleep(0.05) for i in range(times): time.sleep(0.05) m_lf.write(0) def TurnRight(times): m_rf.write(1) time.sleep(0.05) for i in range(times): time.sleep(0.05) m_rf.write(0) def EvasionAction(direction) : time.sleep(0.1) m_rb.write(1) m_lb.write(1) time.sleep(0.2) StopMotors() time.sleep(0.3) if direction > 0 : # spin CW m_lf.write(1) m_rb.write(1) else : # spin CCW m_rf.write(1) m_lb.write(1) for i in range ( 100 ) : time.sleep(0.05) value = adc.read() vo = float(value) vo = vo * 5 / 4096 if vo < 0.75 : # 0.75V = 40cm break StopMotors() time.sleep(0.2) # main # # init PWM x = mraa.Pwm(PWM_PIN) x.period_us(20000) # 50Hz = 20mS = 20000uS x.enable(True) # init ADC adc = mraa.Aio(ADC_PIN) adc.setBit(12) # set resolution 12bit # init Motors (GPIO) m_rf = mraa.Gpio(RF) # Get the motor pin object m_rb = mraa.Gpio(RB) m_lf = mraa.Gpio(LF) m_lb = mraa.Gpio(LB) m_rf.dir(mraa.DIR_OUT) # Set the direction as output m_rb.dir(mraa.DIR_OUT) m_lf.dir(mraa.DIR_OUT) m_lb.dir(mraa.DIR_OUT) StopMotors() pw = P_CENTER rl = 0 # 0 is moving to right ix = 0 # index vo = 0.0 # ADC voltage output #vix = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # for P_MAX = 3 vix = [0.0, 0.0, 0.0, 0.0, 0.0] # for P_MAX = 2 wheretogo = 0 try: while True: x.pulsewidth_us( int(pw) ) time.sleep(0.05) value = adc.read() vo = float(value) vo = vo * 5 / 4096 vix[ix + P_MAX] = vo if (ix == 0) and (rl == 1) : wheretogo = vix.index( min(vix) ) - P_MAX if max(vix) > 2.0 : # 2.0V = 12cm EvasionAction(wheretogo) else : if wheretogo < 0 : TurnLeft( abs(wheretogo) ) else : if wheretogo > 0 : TurnRight( abs(wheretogo) ) else : GoStraight() #print ix, vo if rl == 0: pw += P_STEP ix += 1 if ix > P_MAX : pw -= P_STEP * 2 ix -= 2 rl = 1 else: pw -= P_STEP ix -= 1 if ix < (- P_MAX) : pw += P_STEP * 2 ix += 2 rl = 0 except KeyboardInterrupt: x.pulsewidth_us( int(P_CENTER) ) StopMotors() time.sleep(1)
