You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
250 lines
7.1 KiB
250 lines
7.1 KiB
# Programme voiture autonome a completer
|
|
import pyb, time
|
|
from pyb import delay, Timer
|
|
|
|
class HBridge:
|
|
""" Control à H-Bridge. Also support PWM to control the speed (between 0 to 100%) """
|
|
PWM_FREQ = 100 # Frequency 100 Hz
|
|
(UNDEFINED,HALT,FORWARD,BACKWARD) = (-1, 0,1,2)
|
|
|
|
def __init__( self, input_pins, pwm = None ):
|
|
""":param input_pins: tuple with input1 and input 2 pins
|
|
:param pwm: dic with pin, timer and channel """
|
|
self.speed = 0
|
|
self.state = HBridge.UNDEFINED
|
|
|
|
# Init HBridge control pins
|
|
self.p_input1 = pyb.Pin( input_pins[0], pyb.Pin.OUT_PP )
|
|
self.p_input2 = pyb.Pin( input_pins[1], pyb.Pin.OUT_PP )
|
|
|
|
# Init PWM Control for speed control (without PWM, the L293D's
|
|
# Enable pin must be place to HIGH level)
|
|
self.has_pwm = (pwm != None)
|
|
if self.has_pwm:
|
|
self._timer = pyb.Timer( pwm['timer'], freq=self.PWM_FREQ )
|
|
self._channel = self._timer.channel( pwm['channel'], Timer.PWM, pin=pwm['pin'], pulse_width_percent=100 )
|
|
|
|
self.halt()
|
|
|
|
def set_speed( self, speed ):
|
|
if not(0 <= speed <= 100):
|
|
raise ValueError( 'invalid speed' )
|
|
# Use PWM speed ?
|
|
if self.has_pwm:
|
|
self._channel.pulse_width_percent( speed )
|
|
self.speed = speed
|
|
else:
|
|
# Non PWM
|
|
self.speed = 0 if speed == 0 else 100
|
|
if self.speed == 0 and self.state != HBridge.HALT:
|
|
self.halt() # force motor to stop by manipulating input1 & input2
|
|
|
|
|
|
def halt( self ):
|
|
self.p_input1.low()
|
|
self.p_input2.low()
|
|
self.state = HBridge.HALT # Do not invert ...
|
|
self.set_speed( 0 ) # thoses 2 lines
|
|
|
|
def forward(self, speed = 100 ):
|
|
# reconfigure HBridge
|
|
if self.state != HBridge.FORWARD :
|
|
self.halt()
|
|
self.p_input1.low()
|
|
self.p_input2.high()
|
|
self.state = HBridge.FORWARD
|
|
# Set speed
|
|
self.set_speed( speed )
|
|
|
|
def backward(self, speed = 100 ):
|
|
# reconfigure HBridge
|
|
if self.state != HBridge.BACKWARD:
|
|
self.halt()
|
|
self.p_input1.high()
|
|
self.p_input2.low()
|
|
self.state = HBridge.BACKWARD
|
|
# Set speed
|
|
self.set_speed( speed )
|
|
|
|
|
|
# Pont-H broches de commande Input 1 et Input 2
|
|
MOT1_PINS = (pyb.Pin.board.X6, pyb.Pin.board.X5)
|
|
# Commande PWM pont-H
|
|
MOT1_PWM = {'pin' : pyb.Pin.board.Y9, 'timer' : 2, 'channel' : 3 }
|
|
|
|
# Pont-H broches de commande Input 3 et Input 4
|
|
MOT2_PINS = (pyb.Pin.board.X7, pyb.Pin.board.X8)
|
|
# Commande PWM pont-H
|
|
#MOT2_PWM = {'pin' : pyb.Pin.board.X10, 'timer' : 5, 'channel' : 4 }
|
|
MOT2_PWM = {'pin' : pyb.Pin.board.Y10, 'timer' : 2, 'channel' : 4 }
|
|
# moteur droit h1 moteur gauche h2
|
|
h1 = HBridge( MOT1_PINS, MOT1_PWM )
|
|
h2 = HBridge( MOT2_PINS, MOT2_PWM )
|
|
|
|
# bouton sur carte prototype
|
|
btn = pyb.Pin( 'Y8', pyb.Pin.IN, pull=pyb.Pin.PULL_UP )
|
|
# bouton sur la carte USR
|
|
sw = pyb.Switch()
|
|
# photoresistance
|
|
ldr = pyb.ADC('X19')
|
|
# accelerometre
|
|
accel = pyb.Accel()
|
|
x = accel.x()
|
|
SENS = 3
|
|
# led sur la carte
|
|
led_lum = pyb.LED(1)
|
|
led_montee = pyb.LED(2)
|
|
led_descente = pyb.LED(3)
|
|
xlights = (pyb.LED(3), pyb.LED(4))
|
|
# servo direction
|
|
sd = pyb.Servo(1)
|
|
|
|
def dist_obstacle():
|
|
"""Fonction renvoyant la distance du plus proche objet"""
|
|
trigger = pyb.Pin(pyb.Pin.board.Y5, mode=pyb.Pin.OUT, pull=None)
|
|
trigger.low()
|
|
echo = pyb.Pin(pyb.Pin.board.Y6, mode=pyb.Pin.IN, pull=None)
|
|
trigger.high()
|
|
time.sleep(0.00001)
|
|
trigger.low()
|
|
while echo.value() == 0:
|
|
pass
|
|
start = time.ticks_us()
|
|
while echo.value() == 1:
|
|
pass
|
|
stop = time.ticks_us()
|
|
return round((stop-start)*34/2000,1)
|
|
|
|
def lectension():
|
|
"""Fonction renvoyant l'intensité du terrain"""
|
|
lecture = ldr.read()
|
|
tension = (lecture*3.3)/4095
|
|
pyb.delay(100)
|
|
return tension
|
|
|
|
def autoRestart():
|
|
"""Fonction faisant repartir automatiquement la voiture ou non"""
|
|
t = 1
|
|
while t:
|
|
mins, secs = divmod(t, 60)
|
|
timer = '{:02d}:{:02d}'.format(mins, secs)
|
|
time.sleep(0.5)
|
|
t -= 1
|
|
if t == 0:
|
|
t = 2
|
|
lst.append(dist_obstacle())
|
|
if len(lst) == 3:
|
|
if abs(lst[0]-lst[2]) <= 6:
|
|
backward()
|
|
lst.clear()
|
|
|
|
def checkLight():
|
|
"""Fonction allumant la led selon l'intensité du milieu"""
|
|
if lectension() < 1:
|
|
led_lum.on()
|
|
else:
|
|
led_lum.off()
|
|
|
|
def forward(speed):
|
|
"""Fonction renvoyant si la voiture est à l'arrêt ou non"""
|
|
if speed == 0:
|
|
forward = False
|
|
else:
|
|
forward = True
|
|
return forward
|
|
|
|
def backward():
|
|
"""Fonction faisant reculer la voiture"""
|
|
sd.angle(30)
|
|
h1.backward(35)
|
|
h2.backward(35)
|
|
tBack = 2
|
|
while tBack:
|
|
mins, secs = divmod(tBack, 60)
|
|
timer = '{:02d}:{:02d}'.format(mins, secs)
|
|
time.sleep(1)
|
|
tBack -= 1
|
|
if tBack == 0:
|
|
sd.angle(30)
|
|
h1.backward(35)
|
|
h2.backward(35)
|
|
|
|
def allSpeed(speed):
|
|
"""Fonction faisant avancer la voiture"""
|
|
if forward(vitesse()):
|
|
autoRestart()
|
|
sd.angle(1)
|
|
h1.forward(speed)
|
|
h2.forward(speed)
|
|
else:
|
|
backward()
|
|
lst.clear()
|
|
|
|
def vitesse():
|
|
"""Fonction renvoyant la vitesse selon le terrain"""
|
|
if accel.x() < 0:
|
|
terrain = lst_terrain[0]
|
|
led_montee.off()
|
|
led_descente.on()
|
|
#led orange = descente
|
|
if dist_obstacle() < 10:
|
|
speed = 0
|
|
elif dist_obstacle() < 15:
|
|
speed = 1
|
|
elif dist_obstacle() < 25:
|
|
speed = 2
|
|
elif dist_obstacle() < 40:
|
|
speed = 4
|
|
else:
|
|
speed = 5
|
|
elif accel.x() > 4:
|
|
terrain = lst_terrain[1]
|
|
led_descente.off()
|
|
led_montee.on()
|
|
#led verte = montee
|
|
if dist_obstacle() < 15:
|
|
speed = 0
|
|
elif dist_obstacle() < 25:
|
|
speed = 20
|
|
elif dist_obstacle() < 35:
|
|
speed = 30
|
|
elif dist_obstacle() < 45:
|
|
speed = 40
|
|
else:
|
|
speed = 50
|
|
else:
|
|
terrain = lst_terrain[2]
|
|
led_descente.off()
|
|
led_montee.off()
|
|
#plat
|
|
if dist_obstacle() < 25:
|
|
speed = 0
|
|
elif dist_obstacle() < 35:
|
|
speed = 15
|
|
elif dist_obstacle() < 45:
|
|
speed = 25
|
|
else:
|
|
speed = 45
|
|
#print(terrain)
|
|
return speed
|
|
|
|
marche = True
|
|
lst_terrain = ["descente", "montee", "plat"]
|
|
lst = []
|
|
sw = pyb.Switch()
|
|
|
|
while True:
|
|
"""Boucle principale"""
|
|
if sw():
|
|
marche = not marche
|
|
while sw():
|
|
pass
|
|
|
|
if marche:
|
|
allSpeed(vitesse())
|
|
else:
|
|
allSpeed(0)
|
|
|
|
checkLight()
|
|
|
|
#bug : - Détection du terrain défaillant (piste : fonctionnement du capteur accel.x()).
|