Travail du 2023-04-08, ajout du telescope (servo) + controle turbine

Robot-copy-of-eurobot-2020-main

@@ -0,0 +1 @@
+Subproject commit f0dbb17e830c7d70283bebdbdc246b375ef9c863

Robot.py

@@ -2,7 +2,7 @@ import RobotCompletCode
 Controls = RobotCompletCode.MotorizedPlatform()
 class Robot:
     def init(self):
-            Controls.init()
+        Controls.init()
     def set_range(self, name:str, mini:int, maxi:int):
         Controls.set_range(name,mini,maxi)
     def translation(self, direction, vitesse):
@@ -26,15 +26,26 @@ class Robot:
             print('error')
             return 'error'
     def rotation(self, direction, vitesse):
+        print(direction, vitesse)
         if direction == 'CW':
+            print('vitesse')
             Controls.clockwiseRotation(vitesse)
         elif direction == 'ACW':
+            print(vitesse)
             Controls.antiClockwiseRotation(vitesse)
         else:
             print('error')
             return 'error'
-    def turbine(self, vitesse):
-        pass
+
+    def turbine_aspirer(self):
+        Controls.set_raw_esc('turbine', 230)
+
+    def turbine_stop(self):
+        Controls.set_raw_esc('turbine', 0)
+
+    def turbine_souffler(self):
+        Controls.set_raw_esc('turbine', 251)
+
     def tuyau(self, axe, vitesse):
         #axe : vertical / horizontal
         pass

RobotCompletCode.py

@@ -11,7 +11,8 @@ class MotorizedPlatform:
         'frontLeft': 50,
         'frontRight': 50,
         'backLeft': 50,
-        'backRight': 50
+        'backRight': 50,
+        'turbine': 50
     }
     escMapping = {
         'frontLeft': {
@@ -29,16 +30,24 @@ class MotorizedPlatform:
         'backRight': {
             'slot': 9,
             'range': [307, 410]
-        }
+        },
+        'turbine': {
+            'slot': 7,
+            'range':[307,410],
+            'stop': 0
+         }
     }
     escSlotsList= [12,13,8,9]
     def set_range(self, name:str, mini:int, maxi:int):
         self.escMapping[name]['range'] = [mini,maxi]
     def init(self):
-        if drive != None:
-            for k,v in self.escMapping.items():
-                # 307 est le signal neutre sous 50 Hz (1.5 / 20 x 4096 = 307)
-                drive.setPwm(v['slot'], 0, 307)
+        print("drive init start")
+        v = self.escMapping['turbine']
+        for i in range(220, 250, 1):
+            drive.setPwm(v['slot'], 0, i)
+            sleep(0.04)
+        drive.setPwm(v['slot'], 0, 0)
+        print("drive init stop")
 
     #escSlots = [7, 6, 8, 9]
     def setSpeed(self, values):
@@ -59,7 +68,14 @@ class MotorizedPlatform:
                 self.convert_speed_to_esc(value, self.escMapping[esc_name]['range'])
                 )
 
-        # équivalent de la fonction map() de arduino
+    def set_raw_esc(self, esc_name, value):
+        drive.setPwm(
+            self.escMapping[esc_name]['slot'],
+            0,
+            value
+        )
+    
+    # équivalent de la fonction map() de arduino
     def mappyt(self, x, inMin, inMax, outMin, outMax):
         return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
 
@@ -88,21 +104,19 @@ class MotorizedPlatform:
         self.eastTranslation(- speed)
 
     def clockwiseRotation(self, speed):
-        a = self.convert_speed_to_esc(speed)
-        r = self.convert_speed_to_esc(-speed)
         self.setSpeed({
-                      'frontLeft': a,
-                      'frontRight': a,
-                      'backLeft': r,
-                      'backRight': r
+                      'frontLeft': speed,
+                      'frontRight':speed,
+                      'backLeft': speed,
+                      'backRight': speed
                   })
 
     def antiClockwiseRotation(self, speed):
         self.clockwiseRotation(-speed)
 
     def northEastTranslation(self, speed):
-        a = self.convert_speed_to_esc(speed)
-        s = self.convert_speed_to_esc(0)
+        a = speed
+        s = 0
         self.setSpeed({
                           'frontLeft': a,
                           'frontRight': s,
@@ -123,9 +137,18 @@ class MotorizedPlatform:
 
     def southEastTranslation(self, speed):
         self.northWestTranslation(-speed)
+    
+    def setTurbine(self, speed):
+        self.setSpeed({
+            'turbine':0
+            })
 
     def stop(self):
-        self.init()
+        if drive != None:
+            for k,v in self.escMapping.items():
+                if k == 'turbine':
+                    continue
+                drive.setPwm(v['slot'], 0, v['range'][0])
         """
         realescMapping = {
             'frontLeft': {

ServoDriver.py

@@ -0,0 +1,40 @@
+from Adafruit_PCA9685 import PCA9685
+
+class ServoDriver:
+  def __init__(self):
+    self.driver = PCA9685()
+    self.driver.set_pwm_freq(50)
+    self.servoProfiles = {
+      'default': {'range': 180, 'min': 65, 'max': 530},
+      # telescope 2023-04-08
+      # deployé: 180 deg
+      # rangé: 40 deg
+      'scope': {'range': 180, 'min': 65, 'max': 530},
+    }
+
+  def setAngle(self, slot, angle, profileName = 'default'):
+    profile = None
+    if type(profileName) is dict:
+      profile = profileName
+    if profile == None:
+      if profileName not in self.servoProfiles:
+        profileName = 'default'
+      profile = self.servoProfiles[profileName]
+    if 'range' not in profile:
+      profile['range'] = 180
+    if angle < 0 or angle > profile['range']:
+      print('PWMDriver: Invalid range passed ' + angle + ' but range is ' + profile['range'])
+    if slot > 15 or slot < 0:
+      print('> ERR!!! Invalid slot passed to setAngle!')
+      return
+    pulse = int(self.mappyt(angle, 0, profile['range'], profile['min'], profile['max']))
+    #print('setting slot', slot, 'to angle', angle, 'with profile', profileName, profile, pulse)
+    self.driver.set_pwm(slot, 0, pulse)
+
+  def setPwm(self, slot, off, on):
+    self.driver.set_pwm(slot, off, on)
+
+  def mappyt(self, x, inMin, inMax, outMin, outMax):
+    return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
+
+

server.py

@@ -1,11 +1,15 @@
 import Robot
+import traceback
 import asyncio;
 import json
 import websockets;
 import sys;
 import Container
 from PWMDriver import PWMDriver
+from ServoDriver import ServoDriver
+
 robot = Robot.Robot()
+servo_driver = ServoDriver()
 #robot = Robot.Robot()
 #t = robot.Translation
 #r = robot.Rotation
@@ -48,6 +52,7 @@ class commandHandler:
         return 'initialised'
     def set_rotation(self, arg):
         try:
+            print(arg, self.speed);
             robot.rotation(arg,self.speed)
         except Exception as e:
             print(e)
@@ -62,6 +67,27 @@ class commandHandler:
     def set_range(self, name:str, mini:int, maxi:int):
         robot.set_range(name, mini, maxi)
 
+    def turbine_souffler(self):
+        robot.turbine_souffler()
+        return 'Turbine souffler'
+
+    def turbine_stop(self):
+        robot.turbine_stop()
+        return 'Turbine stopped'
+
+    def turbine_aspirer(self):
+        robot.turbine_aspirer()
+        return 'Turbine aspirer'
+
+    def telescope_deploy(self):
+        # slot 4
+        servo_driver.setAngle(4, 180, profileName='scope')
+        return ''
+
+    def telescope_zero(self):
+        servo_driver.setAngle(4, 40, profileName='scope')
+        # slot 4
+        return ''
 
 cmd_handler = commandHandler()
 def commands(data):
@@ -88,10 +114,21 @@ def commands(data):
             res=cmd_handler.init()
         elif(data['cmd']=='set_speed'):
             res=cmd_handler.set_speed(data["args"]["speed"])
+        elif(data['cmd']=='turbine_aspirer'):
+            res=cmd_handler.turbine_aspirer()
+        elif(data['cmd']=='turbine_stop'):
+            res=cmd_handler.turbine_stop()
+        elif(data['cmd']=='turbine_souffler'):
+            res=cmd_handler.turbine_souffler()
+        elif(data['cmd']=='telescope_deploy'):
+            res=cmd_handler.telescope_deploy()
+        elif(data['cmd']=='telescope_zero'):
+            res=cmd_handler.telescope_zero()
         else:
             return 'NOPE!!!'
     except Exception as e:
-        return e
+        traceback.print_exc()
+        return str(e)
     return res
 async def handler(websocket, path):
     print('new handler created  at path'+path)
@@ -110,6 +147,9 @@ print('server running at 0.0.0.0 with port 1567')
 start_server = websockets.serve(handler, "0.0.0.0", 1567)
 
 
+# init motors
+robot.init()
+
 asyncio.get_event_loop().run_until_complete(start_server)
 
 asyncio.get_event_loop().run_forever()