Add new process msg

controller/fw/embed/include/process_can.h

@@ -19,9 +19,10 @@ void send_motor_enabled();
 void send_id();
 void firmware_update();
 void send_pid_angle(uint8_t param_pid);
-// void send_motor_torque();
+void send_torque();
 void send_pid(uint8_t param_pid);
 void setup_id(uint8_t my_id);
 void setup_angle(float target_angle);
-void setup_pid_angle(uint8_t param_pid, float data);
-void listen_can(const CAN_message_t &msg);
+void setup_velocity(float target_velocity);
+
+void listen_can(const CAN_message_t &msg);

controller/fw/embed/include/reg_cah.h

@@ -1,5 +1,4 @@
-#ifndef REG_CAH_H_
-#define REG_CAH_H_
+#pragma once
 
 #define APP_ADDR        0x0800400  // 16KB - Application
 #define ADDR_VAR        0x8040000
@@ -16,6 +15,10 @@
 #define REG_ID                  0x01
 #define REG_BAUDRATE            0x02
 
+#define DATA_TYPE_ANGLE         0x03
+#define DATA_TYPE_VELOCITY      0x04
+#define DATA_TYPE_TORQUE        0x05
+
 #define REG_MOTOR_POSPID_Kp     0x30
 #define REG_MOTOR_POSPID_Ki     0x31
 #define REG_MOTOR_POSPID_Kd     0x32
@@ -43,6 +46,3 @@
 #define CAN_MSG_MAX_LEN 7
 #define CRC_SIZE        2
 #define ID_SIZE         sizeof(uint8_t)
-
-
-#endif  // REG_CAH_H_

controller/fw/embed/platformio.ini

@@ -14,10 +14,10 @@ build_flags =
     -D STM32F446xx
     -D HAL_CAN_MODULE_ENABLED
     -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
-
+ 
 lib_deps = 
     askuric/Simple FOC@^2.3.4
-    pazi88/STM32_CAN@^1.1.2
+    pazi88/STM32_CAN@^1.1.2 
 
 extra_scripts = 
     pre:gen_compile_commands.py

controller/fw/embed/src/main.cpp

@@ -18,6 +18,7 @@
 #include "config.h"
 #include "process_can.h"
 
+
 void SysTick_Handler(void) {
   HAL_IncTick();
 }
@@ -98,9 +99,13 @@ MotorControlInputs motor_control_inputs;
 
 void loop() {
   __enable_irq();
+  send_angle();
+  send_velocity();
   foc_step(&motor);
   CAN_message_t msg;
   
+
+
   // Process incoming CAN messages
   while (Can.read(msg)) {
     listen_can(msg);

controller/fw/embed/src/process_can.cpp

@@ -40,9 +40,9 @@ void send_velocity() {
       // Error handling: logging, alerts, etc.  
       return;
   }
-  float value = flash_rec[vel].value;
+  // float value = flash_rec[vel].value;
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'V',&value);
+  send_can_with_id_crc(id,'V',&current_velocity);
 }
 
 void send_angle() {
@@ -67,6 +67,15 @@ void send_motor_enabled() {
   send_can_with_id_crc(id,'M',&value);
 }
 
+
+void send_torque() {
+  float i_q = motor.current.q;  // Q-axis current (A)
+  float torque = 100 * i_q;      // Torque calculation
+  if (flash_rec == nullptr) return;
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id, 'T', &torque);
+}
+
 void send_id() {
   /* Firmware data reading */
   if (flash_rec == nullptr) {  // Null check
@@ -78,17 +87,6 @@ void send_id() {
   send_can_with_id_crc(id,'I',&id);
 }
 
-// void send_motor_torque() {
-//   float i_q = motor.current.q;  // Q-axis current (A)
-//   float torque = kt * i_q;      // Torque calculation
-//   torque *= 100;
-//   CAN_TX_msg.id = flash_rec->value;
-//   CAN_TX_msg.buf[0] = 'T';
-//   CAN_TX_msg.len = 5;
-//   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
-//   Can.write(CAN_TX_msg);
-// }
-
 void send_pid_angle(uint8_t param_pid){
   if (flash_rec == nullptr) {  // Null check
     return;
@@ -129,26 +127,26 @@ void setup_angle(float target_angle) {
   // motor.move(target_angle);
 }
 
-// void setup_pid_angle(uint8_t param_pid, uint32_t data){
-//   conv_float_to_int.f = data;
-//   switch (param_pid) {
-//     case pid_p:
-//       motor.P_angle.P = conv_float_to_int.f;
-//       break;
-//     case pid_i:
-//       motor.P_angle.I = conv_float_to_int.f;
-//       break;
-//     case pid_d:
-//       motor.P_angle.D = conv_float_to_int.f;
-//       break;
-//     default:
-//       break;
-//   }
-
-//   write_param(param_pid,data);
-// }
-
+/**
+ * @brief Set the up velocity object
+ * 
+ * @param target_velocity 
+ */
+void setup_velocity(float target_velocity) {
+  motor.enable();
+  motor_control_inputs.target_velocity = target_velocity;
+}
 
+/**
+ * @brief Function for process data from CAN
+ * @details Function check your ID deviceю. Compare receive and calculated CRC.
+ * If ID && CRC == TRUE, then process message or dont send data.
+ * If if and crc = true:
+ * Check CAN_REG 
+ * Gives your data type 
+ * Write with data or send data
+ * @param msg 
+ */
 void listen_can(const CAN_message_t &msg) {
   msg_id = msg.id;
   msg_ch = msg_id & 0xF;        // Extract message channel
@@ -171,6 +169,7 @@ void listen_can(const CAN_message_t &msg) {
     return; // Ignore message on CRC mismatch
   }
   flash_rec = load_params();
+
   /* Message Structure: 0x691
      69 - Device address
      1 - Action code */
@@ -241,5 +240,29 @@ void listen_can(const CAN_message_t &msg) {
         default: break;
       }
     }
+
+    /* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
+    else{
+      switch(msg.buf[0]) {
+        /*  Read after write*/
+          case DATA_TYPE_ANGLE: 
+            send_angle();  
+            memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(float));
+            setup_angle(motor_control_inputs.target_angle);
+            break;
+          
+          case DATA_TYPE_VELOCITY:{
+            send_velocity(); 
+            float vel = 0.0f;
+            memcpy(&vel, &msg.buf[1], sizeof(float));
+            setup_velocity(vel);
+            break;
+          }
+      
+          case DATA_TYPE_TORQUE:
+            send_torque();
+            break;
+      }
+    }
   }
 }