From 7f29caeb768199f60094cf88cdb50ad4fc1ae747 Mon Sep 17 00:00:00 2001
From: Valentin Dabstep <keklolov7@gmail.com>
Date: Tue, 13 May 2025 14:43:45 +0300
Subject: [PATCH] Test for angle

---
 controller/fw/embed/.gitignore                |  2 +-
 controller/fw/embed/src/main.cpp              | 19 +++---
 controller/fw/embed/test/python_send_angle.py | 66 ++++++++++++++-----
 3 files changed, 59 insertions(+), 28 deletions(-)

diff --git a/controller/fw/embed/.gitignore b/controller/fw/embed/.gitignore
index 8fb3160..2d22b6c 100644
--- a/controller/fw/embed/.gitignore
+++ b/controller/fw/embed/.gitignore
@@ -7,4 +7,4 @@
 .metadata/
 cubemx_config/
 compile_commands.json
-../embed.rar/
+../embed.rar
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index ba42018..14e9ce4 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -117,22 +117,22 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
   motor->initFOC();
 }
 
-void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) {
+void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length) {
   // Create CAN message
   CAN_message_t msg_l;
   msg_l.id = id;
-  msg_l.len = 8;  // Protocol-defined message length
+  // msg_l.len = 8;  // Protocol-defined message length
   msg_l.buf[0] = message_type;
-  memcpy(&msg_l.buf[1], data, data_length);
+  memcpy(&msg_l.buf[1], data, 1);
 
   // Prepare CRC calculation buffer (ID + data)
-  size_t crc_data_size = sizeof(msg_l.id) + data_length;
+  size_t crc_data_size = sizeof(msg_l.id) + 1;
   uint8_t crc_data[crc_data_size];
 
   // Copy message ID
   memcpy(crc_data, &msg_l.id, sizeof(msg_l.id));
   // Copy all data bytes
-  memcpy(crc_data + sizeof(msg_l.id), data, data_length);
+  memcpy(crc_data + sizeof(msg_l.id), data, 1);
 
   // Calculate CRC
   uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
@@ -192,6 +192,7 @@ void send_id() {
       // Error handling: logging, alerts, etc.
       return;
   }
+
   uint8_t id = flash_rec[addr_id].value;
   send_can_with_id_crc(id,'I',&id,sizeof(id));
   __NOP();
@@ -389,10 +390,10 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
 
   flash_rec = load_params();
   /*  For test          */
-  int value = 73;  //addr
-  write_param(addr_id,value); //for update address in firmware
-  // Load parameters from flash
-  flash_rec = load_params();
+  // int value = 73;  //addr
+  // write_param(addr_id,value); //for update address in firmware
+  // // Load parameters from flash
+  // flash_rec = load_params();
 
   for(int i = 0;i < PARAM_COUNT;i++)
     flash_buf[i] = flash_rec[i];
diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py
index dc23d9b..03d0f40 100644
--- a/controller/fw/embed/test/python_send_angle.py
+++ b/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()