initial commit

Container.py

@@ -0,0 +1,16 @@
+class Container:
+  instances = {}
+  
+  def set(self, name, instance):
+    self.instances[name] = instance
+  
+  def get(self, name):
+    if name not in self.instances:
+      return None
+    return self.instances[name]
+
+  def count(self):
+    return len(self.instances)
+
+  def getInstances(self):
+    return self.instances

PWMDriver.py

@@ -0,0 +1,38 @@
+from Adafruit_PCA9685 import PCA9685
+
+class PWMDriver:
+  def __init__(self):
+    self.driver = PCA9685()
+    self.driver.set_pwm_freq(50)
+    self.servoProfiles = {
+      'default': {'range': 180, 'min': 65, 'max': 530},
+      'lidar': {'range': 180, 'min': 75, 'max': 510},
+      'china': {'range': 180, 'min': 75, 'max': 510},
+      'flag': {'range': 180, 'min': 100, 'max': 480},
+      'rev': {'range': 270, 'min': 95, 'max': 600}, # 552
+    }
+  
+  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

Robot.py

@@ -0,0 +1,48 @@
+import RobotCompletCode
+Controls = RobotCompletCode.MotorizedPlatform()
+class Robot:
+    def init(self):
+            Controls.init()
+    def translation(self, direction, vitesse):
+        if direction == 'N':
+            Controls.northTranslation(vitesse)
+        elif direction == 'E':
+            Controls.eastTranslation(vitesse)
+        elif direction == 'W':
+            Controls.westTranslation(vitesse)
+        elif direction == 'S':
+            Controls.southTranslation(vitesse)
+        elif direction =='SE':
+            Controls.southEastTranslation(vitesse)
+        elif direction == 'SW':
+            Controls.southWestTranslation(vitesse)
+        elif direction == 'NE':
+            Controls.northEastTranslation(vitesse)
+        elif direction =='NW':
+            Controls.northWestTranslation(vitesse)
+        else: 
+            print('error')
+            return 'error'
+    def rotation(self, direction, vitesse):
+        if direction == 'CW':
+            Controls.clockwiseRotation(vitesse)
+        elif direction == 'ACW':
+            Controls.antiClockwiseRotation(vitesse)
+        else:
+            print('error')
+            return 'error'
+    def stop(self):
+        Controls.stop()
+    def easy(self, mode, direction, vitesse):
+        if mode=='R':
+            self.rotation(direction, vitesse)
+        elif mode=='T':
+            self.translation(diretion, vitesse)
+        else:
+            print('error')
+            return 'error'
+    def easyjson(self, jsondata):
+        # Easy but from JSON datas
+        self.easy(jsondata["mode"], jsondata["direction"], jsondata["vitesse"])
+
+

RobotCompletCode.py

@@ -0,0 +1,130 @@
+from time import sleep
+
+'''
+Abstration of motorized platform
+'''
+from PWMDriver import PWMDriver
+drive = PWMDriver()
+
+class MotorizedPlatform:
+    esc_speed_map = {
+        'frontLeft': 50,
+        'frontRight': 50,
+        'backLeft': 50,
+        'backRight': 50
+    }
+    escMapping = {
+        'frontLeft': {
+            'slot': 12, # 7
+            'range': [307, 410]
+        },
+        'frontRight': {
+            'slot': 13,
+            'range': [307, 410]
+        },
+        'backLeft': {
+            'slot': 8,
+            'range': [307, 410]
+        },
+        'backRight': {
+            'slot': 9,
+            'range': [307, 410]
+        }
+    }
+    escSlotsList= [12,13,8,9]
+
+    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)
+
+    #escSlots = [7, 6, 8, 9]
+    def setSpeed(self, values):
+        # print('values: '+str(values))
+        for k,v in values.items():
+            self.set_esc_speed(k, v)
+
+    def set_all(self, value):
+        for k in self.escMapping.keys():
+            self.set_esc_speed(k, value)
+
+    def set_esc_speed(self, esc_name, value):
+        drive.setPwm(
+            self.escMapping[esc_name]['slot'],
+            0,
+            self.convert_speed_to_esc(value, self.escMapping[esc_name]['range'])
+        )
+
+        # équivalent de la fonction map() de arduino
+    def mappyt(self, x, inMin, inMax, outMin, outMax):
+        return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
+
+    # fonction esc pour une vitesse de moteur de -100 à 100()
+    def convert_speed_to_esc(self, speed, pwm_range=[307, 410]):
+        return round(
+            self.mappyt(speed, 0, 100, pwm_range[0], pwm_range[1])
+        )
+
+    def eastTranslation(self, speed):
+        self.setSpeed({
+                          'frontLeft': -speed, 'frontRight': -speed,
+                          'backLeft':  speed,  'backRight': speed
+                      })
+
+    def southTranslation(self, speed):
+        self.northTranslation(-speed)
+
+    def northTranslation(self, speed):
+        self.setSpeed({
+                          'frontLeft': -speed, 'frontRight': speed,
+                          'backLeft': -speed, 'backRight':  speed
+                      })
+
+    def westTranslation(self, speed):
+        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
+                  })
+
+    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)
+        self.setSpeed({
+                          'frontLeft': a,
+                          'frontRight': s,
+                          'backLeft': s,
+                          'backRight': a
+                      })
+
+    def southWestTranslation(self, speed):
+        self.northEastTranslation(-speed)
+
+    def northWestTranslation(self, speed):
+        self.setSpeed({
+                          'frontLeft': 0,
+                          'frontRight': speed,
+                          'backLeft': 0,
+                          'backRight': speed
+                      })
+
+    def southEastTranslation(self, speed):
+        self.northWestTranslation(-speed)
+
+    def stop(self):
+        self.setSpeed({
+                          'frontLeft': 0,
+                          'frontRight': 0,
+                          'backLeft': 0,
+                          'backRight': 0
+                      })

beta/Driver.py

@@ -0,0 +1,28 @@
+from Adafruit_PCA9685 import PCA9685
+
+class PWMDriver:
+  def __init__(self):
+    self.driver = PCA9685()
+    self.driver.set_pwm_freq(50)
+
+  def set_pwm(self, slot, off, on):
+    self.driver.set_pwm(slot, off, on)
+  
+  # comme map() dans arduino
+  def mappyt(self, x, inMin, inMax, outMin, outMax):
+    return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
+
+  def init_esc(self, slot):
+      self.set_pwm(slot, 0, 307)
+
+  def set_esc(self, slot, speed):
+      self.set_pwm(
+        slot,
+        0,
+        self.convert_speed_to_esc(speed)
+      )
+
+  def convert_speed_to_esc(self, speed):
+    return round(self.mappyt(speed, 0, 100, 210, 410))
+
+

beta/oldRobot.py

@@ -0,0 +1,55 @@
+from Driver import PWMDriver as Driver
+class driver():
+    def setvitesse(self,pin,vitesse):
+        print("le moteur avec le pin ",pin," avance à la vitesse", vitesse,".")
+        dri=Driver()
+        dri.set_esc(pin,vitesse)
+class Robot():
+    d = [1,2]
+    g = [3,4]
+    motors = [d[0],d[1],g[0],g[1]]
+    class Translation():
+        motors = []
+        def init(self,v):
+            self.motors = v
+        def droit(self,v):
+            rb = driver().setvitesse
+            m = self.motors
+            for i in range(len(m)):
+                rb(m[i],v)
+        def trv(self,v,d):
+            rb = driver().setvitesse
+            m = self.motors
+            if d:
+                rb(m[0],v)
+                rb(m[3],v)
+            else:
+                rb(m[1],v)
+                rb(m[2],v)
+        def sd(self,v):
+            rb = driver().setvitesse
+            m = self.motors
+            rb(m[0],v)
+            rb(m[3],v)
+            rb(m[1],v*-1)
+            rb(m[2],v*-1)
+    def Rotation(self,v):
+        m = self.motors
+        rb = driver().setvitesse
+        rb(m[0],v)
+        rb(m[1],v)
+        rb(m[2],v*-1)
+        rb(m[3],v*-1)
+    Translation = Translation()
+    Translation.init(motors)
+
+
+"""
+r = Robot()
+r.Translation.droit(1)
+r.Translation.trv(2,True)
+r.Translation.sd(5)
+r.Translation.sd(-5)
+r.Rotation(5)
+r.Rotation(-5)
+"""

beta/robottest.py

@@ -0,0 +1,17 @@
+import Robot
+x=""
+r=Robot.Robot()
+t  = r.Translation
+rot = r.Rotation
+while x!="q":
+    x=input('command: ')
+    if x=="rotation":
+        rot(10)
+    elif x=="droit":
+        t.droit(10)
+    elif x=="diagonal":
+        t.trv(10, True)
+    elif x=="side":
+        t.sd(10)
+    else:
+        print('available commands: rotation , droit , diagonal , side')

oldserver.py

@@ -0,0 +1,79 @@
+import Robot
+import asyncio;
+import json
+import websockets;
+import sys;
+import Container
+from PWMDriver import PWMDriver
+robot = Robot.Robot()
+#robot = Robot.Robot()
+#t = robot.Translation
+#r = robot.Rotation
+#sd = t.sd
+#v = t.droit
+#d = t.trv
+class commandHandler:
+    def ping(self):
+        return "Pong"
+    """def set_vector(self,vector_type:str,vector_args):
+        print(f"{vector_type=}, {vector_args=}")
+        for i in range(len(vector_args)):
+            vector_args[i]= float(vector_args[i])
+        if vector_type =='trans':
+            if abs(vector_args[0])>=0.1 and abs(vector_args[1])<=0.1:
+                v((vector_args[0]*10))
+            if abs(vector_args[1])>=0.1 and abs(vector_args[0])<=0.1:
+                sd((vector_args[1]*10))
+            if abs(vector_args[1])>=0.1 and abs(vector_args[0])>=0.1:
+                if vector_args[0]>0:
+                    d((vector_args[0]+vector_args[1]//2)*10,True)
+                elif vector_args[0]<0:
+                    d((vector_args[0]+vector_args[1]//2)*10,False)
+        if vector_type == 'rot':
+            r(vector_args[1]*10)
+        return f"{vector_type=},{vector_args=}"
+        """
+    def set_direction(self,arg):
+        try:
+            return robot.translation(arg)
+        except Exception as e:
+            return 'error'
+
+cmd_handler = commandHandler()
+async def handler(websocket, path):
+    print('new handler created with '+websocket+' at path'+path)
+    while True:
+        try :
+            data = await websocket.recv()
+        except websockets.exceptions.ConnectionClosedOK:
+            break
+        print(data)
+        if not data:
+            data = {
+                'cmd':'ping',
+                'args':{}
+            }
+        else:
+            data = json.loads(data)
+        if data['cmd'] =='STOP':
+            sys.exit()
+        try:
+            callable = getattr(cmd_handler, data['cmd'])
+        except AttributeError:
+            await websocket.send(json.dumps({"type":"err","message":"Invalid Command"}))
+            continue
+        
+        try:
+            res = callable(**data['args'])
+        except TypeError:
+            await websocket.send(json.dumps({"type":"err","message":"Invalid Args"}))
+
+            continue
+        await websocket.send(json.dumps({"type":"success","message":res}))
+print('server running at 0.0.0.0 with port 1567')
+start_server = websockets.serve(handler, "0.0.0.0", 1567)
+
+
+asyncio.get_event_loop().run_until_complete(start_server)
+
+asyncio.get_event_loop().run_forever()

pwm_manual.py

@@ -0,0 +1,4 @@
+from RobotCompletCode import MotorizedPlatform
+
+platform = MotorizedPlatform()
+

server.py

@@ -0,0 +1,109 @@
+import Robot
+import asyncio;
+import json
+import websockets;
+import sys;
+import Container
+from PWMDriver import PWMDriver
+robot = Robot.Robot()
+#robot = Robot.Robot()
+#t = robot.Translation
+#r = robot.Rotation
+#sd = t.sd
+#v = t.droit
+#d = t.trv
+class commandHandler:
+    def ping(self):
+        return "Pong"
+    """def set_vector(self,vector_type:str,vector_args):
+        print(f"{vector_type=}, {vector_args=}")
+        for i in range(len(vector_args)):
+            vector_args[i]= float(vector_args[i])
+        if vector_type =='trans':
+            if abs(vector_args[0])>=0.1 and abs(vector_args[1])<=0.1:
+                v((vector_args[0]*10))
+            if abs(vector_args[1])>=0.1 and abs(vector_args[0])<=0.1:
+                sd((vector_args[1]*10))
+            if abs(vector_args[1])>=0.1 and abs(vector_args[0])>=0.1:
+                if vector_args[0]>0:
+                    d((vector_args[0]+vector_args[1]//2)*10,True)
+                elif vector_args[0]<0:
+                    d((vector_args[0]+vector_args[1]//2)*10,False)
+        if vector_type == 'rot':
+            r(vector_args[1]*10)
+        return f"{vector_type=},{vector_args=}"
+        """
+    speed=0
+    def set_direction(self,arg):
+        try:
+            return robot.translation(arg,self.speed)
+        except Exception as e:
+            return 'error'
+    def stop(self):
+        robot.stop()
+        return 'stopped'
+    def init():
+        robot.init()
+        return 'initialised'
+    def set_rotation(self, arg):
+        try:
+            robot.rotation(arg,self.speed)
+        except Exception as e:
+            return 'error'
+    def set_speed(self,v):
+        if v>=50:
+            v = 50
+        else:
+            v=v
+        self.speed=v
+        return 'speed set to '+str(v)
+
+
+cmd_handler = commandHandler()
+def commands(data):
+    print('commands:')
+    print(data)
+    if not data:
+        data = {
+                }
+    else:
+        data = json.loads(data)
+        if 'STOP' in data:
+            if data['STOP']:
+                sys.exit()
+    try:
+        if(data['cmd']=='set_direction'):
+            res= cmd_handler.set_direction(data["args"]['direction'])
+        elif(data['cmd']=='set_rotation'):
+            res= cmd_handler.set_rotation(data["args"]['rotation'])
+        elif(data['cmd']=='stop_robot'):
+            res=cmd_handler.stop()
+        elif(data['cmd']=='init'):
+            res=cmd_handler.init()
+        elif(data['cmd']=='set_speed'):
+            res=cmd_handler.set_speed(data["args"]["speed"])
+        else:
+            return 'NOPE!!!'
+    except Exception as e:
+        return e
+    return res
+async def handler(websocket, path):
+    print('new handler created  at path'+path)
+    while True:
+        try :
+            data = await websocket.recv()
+        except websockets.exceptions.ConnectionClosedOK:
+            break
+        try:
+            res= commands(data)
+        except Exception as e:
+            await websocket.send(json.dumps({"type":"error","message":e}))
+            print(e)
+        await websocket.send(json.dumps({"type":"success","message":res}))
+print('server running at 0.0.0.0 with port 1567')
+start_server = websockets.serve(handler, "0.0.0.0", 1567)
+
+
+asyncio.get_event_loop().run_until_complete(start_server)
+
+asyncio.get_event_loop().run_forever()

test.py

@@ -0,0 +1,8 @@
+import Robot
+import time
+import PWMDriver
+Robot.Robot().init()
+e = Robot.Robot().easy
+e('R','CW',30)
+time.sleep(5)
+Robot.Robot().stop()

testofrobot.py

@@ -0,0 +1,19 @@
+import Robot
+import Container
+robot = Robot.MotorizedPlatform
+def translation(arg):
+    try:
+        callable = getattr(robot,arg)
+        possible=True
+    except AttributeError:
+        print('ERROR')
+        possible=False
+    if possible:
+        callable()
+x=""
+while x!="q":
+    x=input('command : ')
+    if x=="help":
+        print(robot.__dict__)
+    else:
+        translation(x)