diff --git a/Examples/BB2B.py b/Examples/BB2B.py index adab8dc..27c223e 100644 --- a/Examples/BB2B.py +++ b/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) diff --git a/Examples/BB2B_networking.py b/Examples/BB2B_networking.py index 138e579..58046c1 100644 --- a/Examples/BB2B_networking.py +++ b/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() diff --git a/LinearAlgebra/Angle.py b/LinearAlgebra/Angle.py index 5bb373b..aeba133 100644 --- a/LinearAlgebra/Angle.py +++ b/LinearAlgebra/Angle.py @@ -2,4 +2,12 @@ import math class Angle: Rad2Deg = 360 / (math.pi * 2) - Deg2Rad = (math.pi * 2) / 360 \ No newline at end of file + Deg2Rad = (math.pi * 2) / 360 + + @staticmethod + def Normalize(angle): + while angle <= -180: + angle += 360 + while angle > 180: + angle -= 360 + return angle \ No newline at end of file diff --git a/LinearAlgebra/Direction.py b/LinearAlgebra/Direction.py index 24d04ff..7be3a97 100644 --- a/LinearAlgebra/Direction.py +++ b/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): diff --git a/LinearAlgebra/Spherical.py b/LinearAlgebra/Spherical.py index f32923f..7b39253 100644 --- a/LinearAlgebra/Spherical.py +++ b/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 diff --git a/LocalParticipant.py b/LocalParticipant.py index 8d19cae..5b1e9be 100644 --- a/LocalParticipant.py +++ b/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 diff --git a/Messages/BinaryMsg.py b/Messages/BinaryMsg.py index be9a902..d3d5eda 100644 --- a/Messages/BinaryMsg.py +++ b/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) diff --git a/Messages/LowLevelMessages.py b/Messages/LowLevelMessages.py index 128af4f..6183e44 100644 --- a/Messages/LowLevelMessages.py +++ b/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): diff --git a/Participant.py b/Participant.py index bae7f2f..fe1fb1a 100644 --- a/Participant.py +++ b/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 diff --git a/Thing.py b/Thing.py index ea881fa..803ad8b 100644 --- a/Thing.py +++ b/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 diff --git a/Things/DifferentialDrive.py b/Things/DifferentialDrive.py index 585c5b0..c10e350 100644 --- a/Things/DifferentialDrive.py +++ b/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): """! diff --git a/Things/TouchSensor.py b/Things/TouchSensor.py index c0a867c..d612b84 100644 --- a/Things/TouchSensor.py +++ b/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 \ No newline at end of file + self.touched_something = bytes[0] == 1 + if self.touched_something: + print("touched something!") \ No newline at end of file