Add setup for angle using SimpleFOC

controller/fw/embed/src/main.cpp

@@ -139,6 +139,7 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s
 
   // Отправка
   Can.write(msg);
+  __NOP();
 }
 
 
@@ -215,17 +216,13 @@ void send_motor_torque() {
 
 void setup_id(uint8_t my_id) {
   write_param(addr_id,my_id);
-  send_id();
+  // send_id();
 }
 
-
-
-void send_data() {
-  // send_velocity();
-  // send_angle();
-  // send_motor_enabled();
-  // read_temperature();
-  // GPIOC->ODR ^= GPIO_ODR_OD11;
+void setup_angle(int target_angle_rad) {
+  float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы
+  motor.controller = MotionControlType::angle;
+  motor.move(target_angle);
 }
 
 void listen_can(const CAN_message_t &msg) {
@@ -275,6 +272,10 @@ void listen_can(const CAN_message_t &msg) {
         }
       break;
 
+      case MOTOR_ANGLE:
+        setup_angle(msg.buf[1]);
+        break;
+
       case MOTOR_ENABLED:
         if (msg.buf[1] == 1) {
           motor.enable();

controller/fw/embed/test/python_test_id.py

@@ -75,7 +75,7 @@ send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
 
 time.sleep(0.5)
 
-# ======= 2. Запрашиваем текущий ID (используем новй адрес) =======
+# ======= 2. Запрашиваем текущий ID (используем новы й адрес) =======
 
 # Теперь для запроса используем **уже новый id**
 NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ