[Telecommande] Ajout controle robot

Telecommande/lib_robot_maqueen.py

+import microbit as mb
+import machine
+import struct
+import time
+adr=0x10
+
+class MaqueenPlus:
+  def __init__(self):
+    mb.i2c.init(freq=100000,sda=mb.pin20,scl=mb.pin19)
+    # Motors
+    self.MT_L=0
+    self.MT_R=1
+
+    # ServoMotors
+    self.S1=1
+    self.S2=2
+    self.S3=3
+
+    # RGB LED
+    self.RGB_L=1
+    self.RGB_R=2
+    self.RGB_ALL=3
+    self.RED=1
+    self.GREEN=2
+    self.BLUE=4
+    self.YELLOW=3
+    self.PINK=5
+    self.CYAN=6
+    self.WHITE=7
+    self.OFF=8
+
+    # Linetrack sensors
+    self.patrol={
+     "L1":0x04,
+     "L2":0x02,
+     "L3":0x01,
+     "R1":0x08,
+     "R2":0x10,
+     "R3":0x20
+    }
+
+  def motorControl(self,mot,dir,spd):
+    buf=bytearray(3)
+    if mot==self.MT_L:
+      buf[0]=0x00
+    else:
+      buf[0]=0x02
+    buf[1]=dir
+    buf[2]=spd
+    mb.i2c.write(adr,buf)
+
+  def go(self,dL,sL,dR,sR):
+    self.motorControl(self.MT_L,dL,sL)
+    self.motorControl(self.MT_R,dR,sR)
+
+  def servo(self,num,angle):
+    buf=bytearray(3)
+    if num==self.S1:
+      buf[0]=0x14
+    elif num==self.S2:
+      buf[0]=0x15
+    else:
+      buf[0]=0x16
+    buf[1]=angle
+    mb.i2c.write(adr,buf)
+
+  def RGBLight(self,rgbshow,color):
+    buf=bytearray(3)
+    buf[0]=0x0B
+    buf[1]=color
+    if rgbshow==self.RGB_R:
+      buf[0]=0x0C
+    elif rgbshow==self.RGB_ALL:
+      buf[2]=color
+    mb.i2c.write(adr,buf)
+
+  def stop(self):
+    self.go(1,0,1,0)
+
+  def move(self,dir,spd):
+    if dir=="F":
+      self.go(1,spd,1,spd)
+    elif dir=="L":
+      self.go(1,0,1,spd)
+    elif dir=="R":
+      self.go(1,spd,1,0)
+    elif dir=="B":
+      self.go(2,spd,2,spd)
+
+  def goto(self,dir,spd,dst):
+    en=self.getEncoders()
+    goal=dst
+    if dir=="F":
+      goal+=en[0]
+      while en[0]<goal:
+        self.go(1,spd,1,spd)
+        en=self.getEncoders()
+    if dir=="L":
+      goal+=en[1]
+      while en[1]<goal:
+        self.go(1,0,1,spd)
+        en=self.getEncoders()
+    elif dir=="R":
+      goal+=en[0]
+      while en[0]<goal:
+        self.go(1,spd,1,0)
+        en=self.getEncoders()
+    self.stop()
+
+  def ultrasonic(self, maxDist=0.4):
+  # pins: trig=2, echo=8
+    mb.pin2.write_digital(1)
+    time.sleep_us(10)
+    mb.pin2.write_digital(0)
+    mb.pin8.read_digital()
+    timeOut=int(maxDist*2*1000000/340.29)
+    t2 = machine.time_pulse_us(mb.pin8, 1, timeOut)
+    if t2>0:
+      dst=340.29*(t2/(2*1000000))
+    else:
+      dst=maxDist
+    return dst
+
+  def getLine(self):
+    mb.i2c.write(adr,b'\x1D')
+    patrol_y=mb.i2c.read(adr,1)
+    sens={
+     "L1":-1,
+     "L2":-1,
+     "L3":-1,
+     "R1":-1,
+     "R2":-1,
+     "R3":-1
+    }
+    for x in self.patrol:
+      if((patrol_y[0] & self.patrol[x])==self.patrol[x]):
+        sens[x]=1
+      else:
+        sens[x]=0
+    return sens
+
+  def getEncoders(self):
+    buf=bytearray(1)
+    buf[0]=0x04
+    mb.i2c.write(adr,buf)
+    return struct.unpack('>HH',mb.i2c.read(adr,4))
+
+  def clearEncoders(self):
+    buf=bytearray(5)
+    buf[0]=0x04
+    buf[1]=buf[2]=buf[3]=buf[4]=0x00
+    mb.i2c.write(adr,buf)

Telecommande/main.py

+'''
+Gestion télécommande Pour Micro:bit OC Robotique 2025
+Auteur·ice : Vincent Namy
+Version : 1.0
+Date : 3.2.25
+
+'''
+from math import *
+from microbit import *
+from protocole import *
+import lib_robot_maqueen as mqn
+display.off()
+
+# # CONSTANTES
+MOTOR_FORWARD = 0
+MOTOR_BACKWARD = 1
+SPEED = 70
+
+def run(x,y):
+    threshold = 0.2
+    motor = MOTOR_FORWARD
+    if abs(x) <= threshold and  abs(y) <= threshold: # neutral
+        g = 0
+        d = 0
+    elif abs(x) <= threshold: # forward
+        g= int(SPEED)
+        d= int(SPEED)
+        motor =  y < 0
+    elif abs(y) <= threshold: # right/left
+        g= int((x>0)*SPEED)
+        d= int((x<0)*SPEED)
+    else:                     # diag
+        g= int((0.1 + (x>0)*0.9) * SPEED)
+        d= int((0.1 + (x<0)*0.9) * SPEED)
+        motor =  y < 0
+        
+    
+    print("Motors :", g*(-2*motor+1),d*(-2*motor+1))
+    robot.motorControl(robot.MT_L,motor, g)
+    robot.motorControl(robot.MT_R,motor,  d)
+
+if __name__ == '__main__':
+    
+    userId = 1
+    destId = 0
+    robot =  mqn.MaqueenPlus()
+    lastMsgTime = running_time()
+    run(0,0)
+    
+    # Main
+    while True:
+        sleep(10)
+    
+        
+        m = receive_msg(userId)
+        if m and m.msgId==73 :#and len(m.payload) == 3:
+            x = m.payload[0]*2 /255 - 1 # [0;255] --> [-1;1]
+            y = m.payload[1]*2 /255 - 1 # [0;255] --> [-1;1]
+            z = m.payload[2]
+            print("Joystick :", x,y,z)
+            run(x,y)
+        elif (lastMsgTime - running_time()) > 200:
+            run(0,0)
+            
+
+

Telecommande/main_robot.py

-'''
-Gestion télécommande Pour Micro:bit OC Robotique 2025
-Auteur·ice : Vincent Namy
-Version : 1.0
-Date : 3.2.25
-
-'''
-from microbit import *
-from protocole import *
-display.off()
-
-if __name__ == '__main__':
-    
-    userId = 1
-    destId = 0
-    
-    # Main
-    while True:
-        sleep(10)
-        m = receive_msg(userId)
-        if m and m.msgId==73:
-            print("x", m.payload[0]*4, "y", m.payload[1]*4, "z", m.payload[2])
-            
-
-