diff --git a/controller/fw/embed/include/process_can.h b/controller/fw/embed/include/process_can.h index 90e4c66..f5d4f2a 100644 --- a/controller/fw/embed/include/process_can.h +++ b/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); \ No newline at end of file +void setup_velocity(float target_velocity); + +void listen_can(const CAN_message_t &msg); diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index 68f2405..a168e19 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/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_ diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index a15e7c9..f7f813d 100644 --- a/controller/fw/embed/platformio.ini +++ b/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 diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 79eede0..322b07d 100644 --- a/controller/fw/embed/src/main.cpp +++ b/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); diff --git a/controller/fw/embed/src/process_can.cpp b/controller/fw/embed/src/process_can.cpp index 08ad180..5cf46ff 100644 --- a/controller/fw/embed/src/process_can.cpp +++ b/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',¤t_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; + } + } } }