Touch sensor works

Examples/BB2B.py

@@ -17,10 +17,10 @@ touch_right = TouchSensor(parent = bb2b)
 while True:
     # The left wheel turns forward when nothing is touched on the right side
     # and turn backward when the roboid hits something on the right
-    wheel_speed_left = -600 if touch_right.touched_somthing else 600
+    wheel_speed_left = -600 if touch_right.touched_something else 600
     # The right wheel does the same, but instead is controlled by
     # touches on the left side
-    wheel_speed_right = -600 if touch_left.touched_somthing else 600
+    wheel_speed_right = -600 if touch_left.touched_something else 600
     # When both sides are touching something, both wheels will turn backward
     # and the roboid will move backwards
     bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)

Examples/BB2B_networking.py

@@ -15,6 +15,7 @@ participant = LocalParticipant(port=7681, local_port=7682, ip_address="127.0.0.1
 bb2b = DifferentialDrive(participant)
 bb2b.name = "BB2B"
 bb2b.SetDriveDimensions(0.064, 0.128)
+bb2b.SetOrientation(SwingTwist.Degrees(40, 0, 0))
 bb2b.SetPosition(Spherical(0.15, Direction.up))
 
 model = Thing(parent=bb2b)
@@ -24,21 +25,26 @@ model.SetOrientation(SwingTwist.Degrees(90, 0, 0))
 # It has a touch sensor at the front left of the roboid
 touch_left = TouchSensor(parent=bb2b)
 touch_left.name = "Touch left"
+touch_left.SetPosition(Spherical(0.12, Direction.Degrees(-30, 0)))
+
 # and other one on the right
 touch_right = TouchSensor(parent=bb2b)
 touch_right.name = "Touch right"
+touch_right.SetPosition(Spherical(0.12, Direction.Degrees(30, 0)))
 
 # Do forever:
 while True:
     # The left wheel turns forward when nothing is touched on the right side
     # and turn backward when the roboid hits something on the right
-    wheel_speed_left = -600 if touch_right.touched_somthing else 600
+    wheel_speed_left = 600 #-600 if touch_right.touched_something else 600
     # The right wheel does the same, but instead is controlled by
     # touches on the left side
-    wheel_speed_right = -600 if touch_left.touched_somthing else 600
+    wheel_speed_right = -600 if touch_left.touched_something else 600
     # When both sides are touching something, both wheels will turn backward
     # and the roboid will move backwards
     bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)
+    if touch_left.touched_something:
+        print(f'{wheel_speed_left} {wheel_speed_right} {bb2b.linear_velocity.distance} {bb2b.linear_velocity.direction.horizontal}')
 
     # Update the roboid state
     participant.Update()

LinearAlgebra/Angle.py

@@ -2,4 +2,12 @@ import math
 
 class Angle:
     Rad2Deg = 360 / (math.pi * 2)
-    Deg2Rad = (math.pi * 2) / 360
+    Deg2Rad = (math.pi * 2) / 360
+
+    @staticmethod
+    def Normalize(angle):
+        while angle <= -180:
+            angle += 360
+        while angle > 180:
+            angle -= 360
+        return angle

LinearAlgebra/Direction.py

@@ -1,13 +1,23 @@
 import math
+from LinearAlgebra.Angle import Angle
 
 class Direction:
     def __init__(self, horizontal=0, vertical=0):
-        self.horizontal: float = horizontal
-        self.vertical: float = vertical
+        self.horizontal: float = Angle.Normalize(horizontal)
+        self.vertical: float = Angle.Normalize(vertical)
+        self.Normalize()
+    
+    @staticmethod
+    def Degrees(horizontal: float, vertical: float):
+        direction = Direction (horizontal, vertical)
+        return direction
 
     def __add__(self, other):
         return Direction(self.horizontal + other.x, self.vertical + other.y)
 
+    def __neg__(self):
+        return Direction(self.horizontal + 180, -self.vertical)
+    
     def __sub__(self, other):
         return Direction(self.horizontal - other.x, self.vertical - other.y)
 
@@ -20,17 +30,15 @@ class Direction:
         else:
             raise ValueError("Cannot divide by zero")
 
-    def magnitude(self):
+    def Magnitude(self):
         return math.sqrt(self.horizontal**2 + self.vertical**2)
 
-    def normalize(self):
-        mag = self.magnitude()
-        if mag != 0:
-            return Direction(self.horizontal / mag, self.vertical / mag)
-        else:
-            return Direction(0, 0)
+    def Normalize(self):
+        if self.vertical > 90 or self.vertical < -90:
+            self.horizontal += 180
+            self.verical = 180 - self.verical
 
-    def dot(self, other):
+    def Dot(self, other):
         return self.horizontal * other.x + self.vertical * other.y
 
     def __repr__(self):

LinearAlgebra/Spherical.py

@@ -3,8 +3,12 @@ from LinearAlgebra.Direction import Direction
 
 class Spherical:
     def __init__(self, distance, direction):
-        self.distance: float = distance
-        self.direction: Direction = direction
+        if distance < 0:
+            self.distance = -distance
+            self.direction = -direction
+        else:
+            self.distance: float = distance
+            self.direction: Direction = direction
 
     # def __init__(self, distance, horizontal, vertical):
     #     self.distance = distance

LocalParticipant.py

@@ -121,7 +121,7 @@ class LocalParticipant(Participant):
         self.Send(owner, NameMsg(self.network_id, thing))
         self.Send(owner, ModelUrlMsg(self.network_id, thing))
         self.Send(owner, PoseMsg(self.network_id, thing, True))
-        # self.Send(owner, BinaryMsg(self.network_id, thing))
+        self.Send(owner, BinaryMsg(self.network_id, thing))
 
         if recursively:
             for child in thing.children:
@@ -178,9 +178,8 @@ class LocalParticipant(Participant):
                 self.ProcessNameMsg(NameMsg(data))
             case ModelUrlMsg.id:
                 self.ProcessModelUrlMsg(ModelUrlMsg(data))
-            # case Messages.BinaryMsg.id:
-            #     msg = Messages.BinaryMsg(data)
-            #     msg.thing.ProcessBinary(msg.data)
+            case BinaryMsg.id:
+                self.ProcessBinaryMsg(BinaryMsg(data))
 
     def ProcessParticipantMsg(self, sender, msg: ParticipantMsg):
         pass
@@ -216,11 +215,11 @@ class LocalParticipant(Participant):
     def ProcessModelUrlMsg(self, msg: ModelUrlMsg):
         print(f'received model url: {msg.url}')
 
-    def ProcessBinary(self, msg: BinaryMsg):
+    def ProcessBinaryMsg(self, msg: BinaryMsg):
         print('received binary data')
-        if msg.thing != None:
-            msg.thing.ProcessBinary(msg.data)
-
+        thing: Thing = self.Get(msg.network_id, msg.thing_id)
+        if thing != None:
+            thing.ProcessBinary(msg.data)
 
     def Register(self, constructor, thing_type):
         self.thing_msg_processors[thing_type] = constructor 

Messages/BinaryMsg.py

@@ -5,7 +5,7 @@ class BinaryMsg():
     length = 3
 
     def __init__(self, data, thing = None):
-        if isinstance(data, bytearray):
+        if isinstance(data, bytes):
             self.network_id = data[1]
             self.thing_id = data[2]
             #self.thing: Thing = Thing.Get(self.network_id, self.thing_id)

Messages/LowLevelMessages.py

@@ -1,16 +1,13 @@
 import numpy as np
 from LinearAlgebra.SwingTwist import SwingTwist
+from LinearAlgebra.Angle import Angle
 
 def SendAngle8(buffer, ix_ref, angle):
     """! Send an 8-bit angle value """
-    # Normalize angle
-    while angle >= 180:
-        angle -= 360
-    while angle < -180:
-        angle += 360
+    angle = Angle.Normalize(angle)
 
     ix = ix_ref[0]
-    buffer[ix] = round((angle / 360) * 256).to_bytes(1, 'big')[0]
+    buffer[ix] = int((angle / 360) * 256).to_bytes(1, 'big', signed=True)[0]
     ix_ref[0] += 1
 
 def SendFloat16(buffer, ix_ref, value):

Participant.py

@@ -44,7 +44,8 @@ class Participant:
         """
         for thing in self.things:
             if thing is not None:
-                if thing.network_id == network_id and thing.id == thing_id:
+                #if thing.network_id == network_id and thing.id == thing_id:
+                if thing.id == thing_id:
                     return thing
         return None
 

Thing.py

@@ -14,6 +14,15 @@ class Thing:
         DistanceSensor = 0x02
         DirectionalSensor = 0x03
         TemperatureSensor = 0x04
+        TouchSensor = 0x05
+        # Motor
+        ControlledMotor = 0x06
+        UncontrolledMotor = 0x07
+        Servo = 0x08
+        # Other
+        Roboid = 0x09
+        Humanoid = 0x10
+        ExternalSensor = 0x11
         Animator = 0x40
 
     Position = 0x01

Things/DifferentialDrive.py

@@ -55,7 +55,7 @@ class DifferentialDrive(Thing):
         speed = self.wheel_radius * (speed_left + speed_right) / 2 * Angle.Deg2Rad
         self.SetLinearVelocity(Spherical(speed, Direction.forward))
         steer = self.wheel_radius * (speed_left - speed_right) / self.wheel_separation
-        #self.SetAngularVelocity(Spherical(steer, Direction.up))
+        self.SetAngularVelocity(Spherical(steer, Direction.up))
 
     def Update(self, currentTimeMs, recursive = True):
         """!

Things/TouchSensor.py

@@ -6,12 +6,14 @@ class TouchSensor(Thing):
     def __init__(self, owner = None, parent = None):
         """! Create a touch sensor
         """
-        super().__init__(owner = owner, parent = parent)
+        super().__init__(owner = owner, parent = parent, type = Thing.Type.TouchSensor)
         
         ## Value which is true when the sensor is touching something, false otherwise
-        self.touched_somthing = False
+        self.touched_something = False
 
-    def ProcessBinary(bytes):
+    def ProcessBinary(self, bytes):
         """! Function to extract the touch state received in a binary message
         """
-        pass
+        self.touched_something = bytes[0] == 1
+        if self.touched_something:
+            print("touched something!")