#!/usr/bin/env pybricks-micropython # EV3 / Pybricks 2 : menu et suivi de ligne TOR, P, PI, PD, PID. # Haut/bas : choisir un champ. Gauche/droite : modifier sa valeur. # Centre : lancer. Pendant le suivi, centre : freiner et revenir au menu. # Reglages conserves en memoire jusqu'a la fermeture du programme. from pybricks.hubs import EV3Brick from pybricks.ev3devices import Motor, ColorSensor from pybricks.parameters import Port, Button, Color from pybricks.tools import wait, StopWatch from pybricks.media.ev3dev import Font robot = EV3Brick() robot.screen.set_font(Font(size=10)) gauche = Motor(Port.B) droite = Motor(Port.C) capteur = ColorSensor(Port.S3) MODES = ("TOR", "P", "PI", "PD", "PID") MODE = "PID" VITESSE = 150 CORRECTION_MAX = 250 CORRECTION_TOR = 100 SENS = 1 SENS_GAUCHE = 1 SENS_DROITE = 1 PERIODE_MS = 20 W = 40 KP = 2.0 KI = 1.0 KD = 0.05 TAU_D = 0.05 # Cle, libelle, pas, minimum, maximum, decimales. REGLAGES = { "VITESSE": ("Vitesse deg/s", 10, 10, 750, 0), "W": ("Consigne %", 1, 0, 100, 0), "KP": ("Kp", 0.1, 0, 30, 1), "KI": ("Ki", 0.1, 0, 20, 1), "KD": ("Kd", 0.01, 0, 5, 2), "CORRECTION_TOR": ("Correction TOR", 10, 0, 500, 0), "CORRECTION_MAX": ("Correction max", 10, 10, 500, 0), "SENS": ("Sens du suivi", 2, -1, 1, 0), "TAU_D": ("Filtre D (s)", 0.01, 0, 1, 2), } valeurs = {"VITESSE": VITESSE, "W": W, "KP": KP, "KI": KI, "KD": KD, "CORRECTION_TOR": CORRECTION_TOR, "CORRECTION_MAX": CORRECTION_MAX, "SENS": SENS, "TAU_D": TAU_D} def limiter(valeur, minimum, maximum): return max(minimum, min(valeur, maximum)) def relacher_boutons(): while robot.buttons.pressed(): wait(10) def champs_mode(mode): champs = ["MODE", "VITESSE", "W"] if mode == "TOR": champs.append("CORRECTION_TOR") else: champs.append("KP") if mode in ("PI", "PID"): champs.append("KI") if mode in ("PD", "PID"): champs.extend(["KD", "TAU_D"]) return champs + ["CORRECTION_MAX", "SENS", "DEMARRER"] def afficher_menu(mode, champ, position, nombre): robot.screen.clear() lignes = ["Mode : " + mode, "Champ {}/{}".format(position + 1, nombre)] if champ == "MODE": lignes.append("> Mode : " + mode) elif champ == "DEMARRER": lignes.append("> DEMARRER") else: libelle, pas, mini, maxi, decimales = REGLAGES[champ] valeur = valeurs[champ] texte = str(int(valeur)) if decimales == 0 else ( "{:." + str(decimales) + "f}").format(valeur) lignes.append("> " + libelle + " : " + texte) lignes.extend(["H/B champ G/D valeur", "Centre : demarrer"]) for i, ligne in enumerate(lignes): robot.screen.draw_text(0, i * 18, ligne) def menu(mode): # Les moteurs restent arretes dans tout le menu. relacher_boutons() position = 0 precedent = [] horloge_menu = StopWatch() dernier_affichage = -100 rafraichir = True while True: champs = champs_mode(mode) champ = champs[position] if rafraichir: afficher_menu(mode, champ, position, len(champs)) rafraichir = False if horloge_menu.time() - dernier_affichage >= 100: robot.screen.draw_box(0, 94, 177, 127, fill=True, color=Color.WHITE) robot.screen.draw_text(0, 99, "Mesure : {} %".format( capteur.reflection())) dernier_affichage = horloge_menu.time() presses = robot.buttons.pressed() nouveaux = [b for b in presses if b not in precedent] precedent = presses if Button.CENTER in nouveaux: relacher_boutons() return mode elif Button.UP in nouveaux: position = (position - 1) % len(champs) rafraichir = True elif Button.DOWN in nouveaux: position = (position + 1) % len(champs) rafraichir = True elif Button.LEFT in nouveaux or Button.RIGHT in nouveaux: delta = 1 if Button.RIGHT in nouveaux else -1 if champ == "MODE": mode = MODES[(MODES.index(mode) + delta) % len(MODES)] elif champ in REGLAGES: libelle, pas, mini, maxi, decimales = REGLAGES[champ] valeurs[champ] = round(limiter( valeurs[champ] + delta * pas, mini, maxi), decimales) rafraichir = True wait(20) def suivre_ligne(): # L'integrale et la derivee repartent de zero a chaque essai. integrale = 0.0 derivee_filtree = 0.0 erreur_precedente = W - capteur.reflection() horloge = StopWatch() temps_precedent = horloge.time() robot.screen.clear() robot.screen.print("Mode : " + MODE) robot.screen.print("Centre : arreter") # -------------------------------------------------- # BOUCLE DE REGULATION # -------------------------------------------------- try: while Button.CENTER not in robot.buttons.pressed(): debut_cycle = horloge.time() # Temps reel entre deux mesures, en secondes dt = max((debut_cycle - temps_precedent) / 1000, 0.001) temps_precedent = debut_cycle mesure = capteur.reflection() erreur = W - mesure if MODE == "TOR": # Deux commandes possibles if erreur >= 0: correction = CORRECTION_TOR else: correction = -CORRECTION_TOR else: # Action proportionnelle action_p = KP * erreur # Action derivee filtree action_d = 0.0 if MODE in ("PD", "PID"): derivee = (erreur - erreur_precedente) / dt alpha = dt / (TAU_D + dt) derivee_filtree += alpha * ( derivee - derivee_filtree ) action_d = KD * derivee_filtree # Action integrale avec anti-emballement if MODE in ("PI", "PID"): integrale_candidate = integrale + erreur * dt commande_candidate = ( action_p + KI * integrale_candidate + action_d ) # Integrer si la commande n'est pas saturee, # ou si l'erreur permet de sortir de saturation. if ( abs(commande_candidate) <= CORRECTION_MAX or ( commande_candidate > CORRECTION_MAX and erreur < 0 ) or ( commande_candidate < -CORRECTION_MAX and erreur > 0 ) ): integrale = integrale_candidate action_i = 0.0 if MODE in ("PI", "PID"): action_i = KI * integrale correction = action_p + action_i + action_d erreur_precedente = erreur correction = limiter( correction, -CORRECTION_MAX, CORRECTION_MAX ) correction *= SENS vitesse_gauche = VITESSE - correction vitesse_droite = VITESSE + correction gauche.run(SENS_GAUCHE * vitesse_gauche) droite.run(SENS_DROITE * vitesse_droite) # Completer le cycle pour approcher la periode souhaitee duree_cycle = horloge.time() - debut_cycle wait(max(1, PERIODE_MS - duree_cycle)) finally: # Arret egalement si une erreur interrompt le programme gauche.brake() droite.brake() relacher_boutons() # Retour automatique au choix du mode apres chaque arret central. try: while True: MODE = menu(MODE) VITESSE = valeurs["VITESSE"] W = valeurs["W"] KP = valeurs["KP"] KI = valeurs["KI"] KD = valeurs["KD"] CORRECTION_TOR = valeurs["CORRECTION_TOR"] CORRECTION_MAX = valeurs["CORRECTION_MAX"] SENS = valeurs["SENS"] TAU_D = valeurs["TAU_D"] suivre_ligne() finally: gauche.brake() droite.brake()