collision workaround

Author: poiuytrezaur

spherov2/commands/sensor.py

@@ -113,11 +113,11 @@ def configure_collision_detection(toy, collision_detection_method: CollisionDete
 
     @staticmethod
     def __collision_detected_notify_helper(listener, packet):
-        unpacked = struct.unpack('>3hB3hBL', packet.data)
+        unpacked = struct.unpack('>3hB2hBL', packet.data)
         listener(CollisionDetected(acceleration_x=unpacked[0] / 4096, acceleration_y=unpacked[1] / 4096,
                                    acceleration_z=unpacked[2] / 4096, x_axis=bool(unpacked[3] & 1),
                                    y_axis=bool(unpacked[3] & 2), power_x=unpacked[4], power_y=unpacked[5],
-                                   power_z=unpacked[6], speed=unpacked[7], time=unpacked[8] / 1000))
+                                   speed=unpacked[6], time=unpacked[7] / 1000))
 
     collision_detected_notify = (24, 18, 0xff), __collision_detected_notify_helper.__func__
 

spherov2/controls/v2.py

@@ -57,6 +57,7 @@ class Error(IntEnum):
 
     @staticmethod
     def parse_response(data) -> 'Packet':
+        #print("Packet", ' '.join(format(x, '02x') for x in data))
         sop, *data, eop = data
         if sop != Packet.Encoding.start:
             raise PacketDecodingException('Unexpected start of packet')
@@ -75,12 +76,16 @@ def parse_response(data) -> 'Packet':
         sid = None
         if flags & Packet.Flags.has_source_id:
             sid = data.pop(0)
+        #print("Response packet", ' '.join(format(x, '02x') for x in data))
 
         did, cid, seq, *data = data
+        #print("Response seq ", seq)
+        #print("Response data", ' '.join(format(x, '02x') for x in data))
 
         err = Packet.Error.success
         if flags & Packet.Flags.is_response:
             err = Packet.Error(data.pop(0))
+        #print("Response code", err)
 
         return Packet(flags, did, cid, seq, tid, sid, bytearray(data), err)
 
@@ -149,12 +154,14 @@ def __init__(self):
             self.__seq = 0
 
         def new_packet(self, did, cid, tid=None, data=None):
+            #print("Packet for command", did, cid, self.__seq, tid)
             flags = Packet.Flags.requests_response | Packet.Flags.is_activity
             sid = None
             if tid is not None:
                 flags |= Packet.Flags.has_source_id | Packet.Flags.has_target_id
                 sid = 0x1
             packet = Packet(flags, did, cid, self.__seq, tid, sid, bytearray(data or []))
+            #print("New Packet",' '.join(format(x, '02x') for x in packet.build()))
             self.__seq = (self.__seq + 1) % 0xff
             return packet
 
@@ -270,6 +277,11 @@ def __new_data():
                     d = component.modifier(d)
                 n[name] = d
             data[sensor] = n
+        #print("len ", len(sensor_data))
+        #print("sensor_data", sensor_data)
+        #print("sensors", self.__toy.sensors.keys())
+        #print("sensors extended", self.__toy.extended_sensors.keys())
+        #print("enabled", self.__enabled.keys())
 
         for sensor, components in self.__toy.sensors.items():
             if sensor in self.__enabled:

spherov2/listeners/sensor.py

@@ -10,7 +10,6 @@ class CollisionDetected(NamedTuple):
     y_axis: bool
     power_x: int
     power_y: int
-    power_z: int
     speed: int
     time: float
 

spherov2/toy/__init__.py

@@ -113,6 +113,8 @@ def __new_packet(self, packet):
             queue.get().set_result(packet)
         for f in self.__listeners[key].values():
             threading.Thread(target=f, args=(packet,)).start()
+        for f in self.__listeners["all"].values():
+            threading.Thread(target=f, args=(packet,)).start()
 
     @classmethod
     def implements(cls, method, with_target=False):