Test for angle

controller/fw/embed/test/python_send_angle.py

@@ -3,35 +3,65 @@ import struct
 import time
 import argparse
 
-# Function to send the target angle
+# Константы
+CAN_INTERFACE = 'can0'
+DEVICE_ID = 0x00          # ID ADDR for servo
+REG_WRITE = 0x8
+REG_POS = 0x72             # MOTOR+ANGLE = 0x72
+
+def validate_crc16(data):
+    #   Calculate   CRC16
+    crc = 0xFFFF
+    for byte in data:
+        crc ^= byte
+        for _ in range(8):
+            if crc & 0x0001:
+                crc = (crc >> 1) ^ 0xA001
+            else:
+                crc >>= 1
+    return crc
+
 def send_target_angle(bus, target_angle):
-    msg = can.Message()
-    msg.arbitration_id = 1  # Message ID
-    msg.is_extended_id = False
-    msg.dlc = 5  # Message length
-    msg.data = [ord('A')] + list(struct.pack('<f', target_angle))  # 'A' for the command identifier, followed by the angle in float format
+    # ID and cmd
+    arbitration_id = (DEVICE_ID << 4) | REG_WRITE
+    id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
+
+    # cmd + parametrs
+    data_write = [REG_POS] + list(struct.pack('<f', target_angle))  
+
+    
+    full_data_for_crc = id_bytes + data_write
+    crc = validate_crc16(full_data_for_crc)
+    crc_bytes = list(crc.to_bytes(2, byteorder='little'))
+
+    # Full packet
+    packet = data_write + crc_bytes
+
+
+    msg = can.Message(
+        arbitration_id=arbitration_id,
+        is_extended_id=False,
+        data=packet
+    )
 
     try:
         bus.send(msg)
-        print(f"Sent message with target angle: {target_angle} degrees")
-        print(f"Message data: {msg.data}")
+        print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
     except can.CanError:
-        print("Message failed to send")
+        print("Ошибка отправки сообщения")
 
-# Main function
 def main():
-    parser = argparse.ArgumentParser(description="Send target angles over CAN bus.")
-    parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus")
+    parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
+    parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
     args = parser.parse_args()
 
-    target_angle = args.angle
+    # Инициализация CAN
+    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+    print("CAN шина инициализирована.")
 
-    # CAN interface setup
-    bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
-    print("CAN bus initialized, sending target angles...")
+    send_target_angle(bus, args.angle)
 
-    # Loop to send messages
-    send_target_angle(bus, target_angle)
+    bus.shutdown()
 
 if __name__ == '__main__':
     main()