Projet_Volta/main.py

# 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()