2022-11-04 16:30:35 +01:00
|
|
|
from microbit import *
|
|
|
|
|
from machine import *
|
|
|
|
|
import music
|
|
|
|
|
import radio
|
2022-11-05 05:08:39 +01:00
|
|
|
import math
|
|
|
|
|
import time
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
###############################################################################
|
|
|
|
|
# rp_maqueen-robot.py
|
|
|
|
|
# @title: Jumeau Maqueen : Programme de la carte microbit du robot
|
|
|
|
|
# @project: Ropy (Blender-EduTech)
|
|
|
|
|
# @lang: fr
|
|
|
|
|
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
|
|
|
|
|
# @copyright: Copyright (C) 2022 Philippe Roy
|
|
|
|
|
# @license: GNU GPL
|
|
|
|
|
###############################################################################
|
|
|
|
|
|
|
|
|
|
# ###############################################################################
|
|
|
|
|
# Initialisation
|
|
|
|
|
# ###############################################################################
|
|
|
|
|
|
2022-11-05 02:20:39 +01:00
|
|
|
attente_image = Image("00000:00000:00900:00000:00000")
|
|
|
|
|
display.show(attente_image) # Témoin de fonctionnement
|
2022-11-05 05:08:39 +01:00
|
|
|
radio.config(group=1, queue=4, length=8)
|
2022-11-04 16:30:35 +01:00
|
|
|
radio.on()
|
|
|
|
|
|
2022-11-05 02:20:39 +01:00
|
|
|
vitesse = 50
|
|
|
|
|
distance = 50
|
|
|
|
|
angle = 90
|
|
|
|
|
|
2022-11-05 05:08:39 +01:00
|
|
|
vmax_roue = 85.5 # Vitesse maxi des roues en tr/min
|
|
|
|
|
diam_roue = 43 # Diamètre des roues en mm
|
|
|
|
|
dist_essieu = 70 # Distance entre les roues en mm
|
|
|
|
|
|
|
|
|
|
# ###############################################################################
|
|
|
|
|
# Boucle principale
|
|
|
|
|
# ###############################################################################
|
2022-11-04 16:30:35 +01:00
|
|
|
|
2022-11-05 02:20:39 +01:00
|
|
|
while True:
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Lecture de l'ordre
|
|
|
|
|
ordre=radio.receive()
|
|
|
|
|
|
2022-11-05 05:08:39 +01:00
|
|
|
# Configuration
|
|
|
|
|
if ordre=="CF":
|
|
|
|
|
display.scroll("CF")
|
|
|
|
|
i=1
|
|
|
|
|
while True:
|
|
|
|
|
conf=radio.receive()
|
|
|
|
|
if conf=="FC": # Fin de la configuration
|
|
|
|
|
# display.show(attente_image) # Témoin de fonctionnement
|
|
|
|
|
break
|
|
|
|
|
if conf is not None:
|
|
|
|
|
if i != 1:
|
|
|
|
|
text = str(i) + " : "+str(conf[2:-1])
|
|
|
|
|
# display.scroll(text)
|
|
|
|
|
if i == 2: # Configuration de la vitesse
|
|
|
|
|
vitesse = int(str(conf[2:-1]))
|
|
|
|
|
if i == 3: # Configuration de la distance
|
|
|
|
|
distance = int(str(conf[2:-1]))
|
|
|
|
|
if i == 4: # Configuration de l'angle
|
|
|
|
|
angle = int(str(conf[2:-1]))
|
|
|
|
|
i+=1
|
|
|
|
|
|
|
|
|
|
# Avancer d'un pas
|
2022-11-04 16:30:35 +01:00
|
|
|
if ordre=="AV":
|
2022-11-05 05:08:39 +01:00
|
|
|
|
|
|
|
|
display.show(Image.ARROW_N) # Afficher flèche avancer
|
2022-11-04 16:30:35 +01:00
|
|
|
pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
pin12.write_digital(1) # Led avant gauche
|
2022-11-05 05:08:39 +01:00
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, vitesse])) # Avance moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, vitesse])) # Avance moteur droit
|
|
|
|
|
|
|
|
|
|
v_roue=(vmax_roue/255)*vitesse
|
|
|
|
|
v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
|
|
|
|
|
time.sleep(distance/v_lin)
|
|
|
|
|
|
|
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
|
|
|
|
|
pin8.write_digital(0) # Led avant gauche
|
|
|
|
|
pin12.write_digital(0) # Led avant gauche
|
|
|
|
|
display.clear() # Effacer matrice de leds
|
|
|
|
|
|
|
|
|
|
# display.show(Image.ARROW_N)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(1) # Led avant gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, v_avancer])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x0, v_avancer])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Reculer
|
|
|
|
|
if ordre=="RE":
|
2022-11-05 05:08:39 +01:00
|
|
|
|
|
|
|
|
display.show(Image.ARROW_S) # Afficher flèche reculer
|
2022-11-04 16:30:35 +01:00
|
|
|
pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
pin12.write_digital(1) # Led avant gauche
|
2022-11-05 05:08:39 +01:00
|
|
|
i2c.write(0x10, bytearray([0x00, 0x1, vitesse])) # Avance moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x1, vitesse])) # Avance moteur droit
|
|
|
|
|
|
|
|
|
|
v_roue=(vmax_roue/255)*vitesse
|
|
|
|
|
v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
|
|
|
|
|
time.sleep(distance/v_lin)
|
|
|
|
|
|
|
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
|
|
|
|
|
pin8.write_digital(0) # Led avant gauche
|
|
|
|
|
pin12.write_digital(0) # Led avant gauche
|
|
|
|
|
display.clear() # Effacer matrice de leds
|
|
|
|
|
|
|
|
|
|
# display.show(Image.ARROW_S)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(1) # Led avant gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x1, v_avancer])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x1, v_avancer])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Gauche
|
|
|
|
|
if ordre=="GA":
|
2022-11-05 05:08:39 +01:00
|
|
|
display.show(Image.ARROW_E)
|
2022-11-04 16:30:35 +01:00
|
|
|
pin8.write_digital(1) # Led avant gauche
|
2022-11-05 05:08:39 +01:00
|
|
|
i2c.write(0x10, bytearray([0x00, 0x1, vitesse])) # Avance moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, vitesse])) # Avance moteur droit
|
|
|
|
|
|
|
|
|
|
v_roue=(vmax_roue/255)*vitesse
|
|
|
|
|
v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
|
|
|
|
|
angle2=(angle/4)*(2*math.pi/360)
|
|
|
|
|
time.sleep((dist_essieu*angle2)/v_lin)
|
|
|
|
|
|
|
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
|
|
|
|
|
pin8.write_digital(0) # Led avant gauche
|
|
|
|
|
display.clear() # Effacer matrice de leds
|
|
|
|
|
|
|
|
|
|
# display.show(Image.ARROW_W)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x0, v_tourner])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Avancer + gauche
|
2022-11-05 05:08:39 +01:00
|
|
|
# if ordre=="AG":
|
|
|
|
|
# display.show(Image.ARROW_W)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, v_tourner_faible])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x0, v_tourner])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Reculer + gauche
|
2022-11-05 05:08:39 +01:00
|
|
|
# if ordre=="RG":
|
|
|
|
|
# display.show(Image.ARROW_W)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x1, v_tourner_faible])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x1, v_tourner])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Droite
|
|
|
|
|
if ordre=="DR":
|
|
|
|
|
|
|
|
|
|
display.show(Image.ARROW_W)
|
2022-11-05 05:08:39 +01:00
|
|
|
pin12.write_digital(1) # Led avant droit
|
|
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, vitesse])) # Avance moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x1, vitesse])) # Avance moteur droit
|
|
|
|
|
|
|
|
|
|
v_roue=(vmax_roue/255)*vitesse
|
|
|
|
|
v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
|
|
|
|
|
angle2=(angle/4)*(2*math.pi/360)
|
|
|
|
|
time.sleep((dist_essieu*angle2)/v_lin)
|
|
|
|
|
|
|
|
|
|
i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
|
|
|
|
|
i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
pin12.write_digital(0) # Led avant droit
|
2022-11-05 05:08:39 +01:00
|
|
|
display.clear() # Effacer matrice de leds
|
|
|
|
|
|
|
|
|
|
# display.show(Image.ARROW_E)
|
|
|
|
|
# pin8.write_digital(0) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(1) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, v_tourner])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Moteur droit
|
|
|
|
|
|
|
|
|
|
# Avancer + droite
|
|
|
|
|
# if ordre=="AD":
|
|
|
|
|
# display.show(Image.ARROW_W)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, v_tourner])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x0, v_tourner_faible])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Reculer + droite
|
2022-11-05 05:08:39 +01:00
|
|
|
# if ordre=="RD":
|
|
|
|
|
# display.show(Image.ARROW_W)
|
|
|
|
|
# pin8.write_digital(1) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x1, v_tourner])) # Moteur gauche
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x1, v_tourner_faible])) # Moteur droit
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Stop
|
2022-11-05 05:08:39 +01:00
|
|
|
# if ordre=="ST":
|
|
|
|
|
# display.show(attente_image) # Témoin de fonctionnement
|
|
|
|
|
# pin8.write_digital(0) # Led avant gauche
|
|
|
|
|
# pin12.write_digital(0) # Led avant droit
|
|
|
|
|
# i2c.write(0x10, bytearray([0x00, 0x0, 0]))
|
|
|
|
|
# i2c.write(0x10, bytearray([0x02, 0x1, 0]))
|
2022-11-04 16:30:35 +01:00
|
|
|
|
|
|
|
|
# Cadencement
|
2022-11-05 02:20:39 +01:00
|
|
|
# sleep(100)
|
