Utrymme-Rover med Edison/Intel (3 / 3 steg)
Steg 3:
#! / usr/bin/python
importera mraa
Importera tid
PWM_PIN = 3
ADC_PIN = 0
RF = 6 # rätt motor fram
RB = 7 # rätt motor tillbaka
LF = 4 # lämnade motor fram
LB = 5 # vänster motor tillbaka
P_CENTER = 1373 # för VSTONE VS-95647 servo
P_STEP = 157 # eller 79. Oss på 50Hz
P_MAX = # 2 eller 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)
för i i range(times):
Time.Sleep(0.05)
m_lf.write(0)
def TurnRight(times):
m_rf.write(1)
Time.Sleep(0.05)
för i i 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)
om riktning > 0:
# spin CW
m_lf.write(1)
m_rb.write(1)
annat:
# spin CCW
m_rf.write(1)
m_lb.write(1)
för jag i intervall (100):
Time.Sleep(0.05)
värde = adc.read()
VO = float(value)
VO = vo * 5 / 4096
om vo < 0,75: # 0.75V = 40 cm
Break
StopMotors()
Time.Sleep(0.2)
# viktigaste #
# init PWM
x = mraa. PWM(PWM_PIN)
x.period_us(20000) # 50 Hz = 20mS = 20000uS
x.enable(true)
# init ADC
ADC = mraa. AIO(ADC_PIN)
adc.setBit(12) # ange upplösning 12 bitar
# init Motors (GPIO)
m_rf = mraa. Gpio(RF) # få objektet motor pin
m_rb = mraa. Gpio(RB)
m_lf = mraa. Gpio(LF)
m_lb = mraa. Gpio(lb)
m_rf.dir (mraa. DIR_OUT) # ange riktningen
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 går till höger
IX = 0 # index
VO = 0.0 # ADC Matningsspänning
#vix = [0,0 0,0 0,0, 0,0, 0,0, 0,0, 0,0] # för P_MAX = 3
VIX = [0,0 0,0 0,0, 0,0, 0,0] # för P_MAX = 2
wheretogo = 0
försök:
samtidigt sant:
x.pulsewidth_us (int(pw))
Time.Sleep(0.05)
värde = adc.read()
VO = float(value)
VO = vo * 5 / 4096
VIX [ix + P_MAX] = vo
om (ix == 0) och (rl == 1):
wheretogo = vix.index (min(vix)) - P_MAX
om max(vix) > 2.0: # 2.0V = 12 cm
EvasionAction(wheretogo)
annat:
om wheretogo < 0:
Svängvänster (abs(wheretogo))
annat:
om wheretogo > 0:
Svänghöger (abs(wheretogo))
annat:
GoStraight()
#print ix, vo
om rl == 0:
PW += P_STEP
IX + = 1
om ix > P_MAX:
PW-= P_STEP * 2
IX-= 2
RL = 1
annat:
PW-= P_STEP
IX-= 1
om ix < (-P_MAX):
PW += P_STEP * 2
IX + = 2
RL = 0
Förutom KeyboardInterrupt:
x.pulsewidth_us (int(P_CENTER))
StopMotors()
Time.Sleep(1)