robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Add save data_type CAN

Browse source
This commit is contained in:
Valentin Dabstep 2025-06-06 18:37:48 +03:00
parent 58a051b217
commit 28ce1bb556
3 changed files with 41 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

3
controller/fw/embed/include/flash.h
View file

@ -22,7 +22,8 @@ enum {
firmw, firmw,
foc_id, foc_id,
angl, angl,
vel vel,
torq
}; };
/* for saved in FLASH float data*/ /* for saved in FLASH float data*/

7
controller/fw/embed/include/reg_cah.h
View file

@ -1,5 +1,12 @@
#pragma once #pragma once
enum{
error_foc = 0,
error_canRX,
error_canTX
};
#define APP_ADDR 0x0800400 // 16KB - Application #define APP_ADDR 0x0800400 // 16KB - Application
#define ADDR_VAR 0x8040000 #define ADDR_VAR 0x8040000

34
controller/fw/embed/src/process_can.cpp
View file

@ -87,6 +87,11 @@ void send_id() {
send_can_with_id_crc(id,'I',&id); send_can_with_id_crc(id,'I',&id);
} }
void send_error(uint8_t error_code){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'P',&error_code);
}
void send_pid_angle(uint8_t param_pid){ void send_pid_angle(uint8_t param_pid){
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
return; return;
@ -137,6 +142,11 @@ void setup_velocity(float target_velocity) {
motor_control_inputs.target_velocity = target_velocity; motor_control_inputs.target_velocity = target_velocity;
} }
void send_data_type(uint8_t type_d){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'D',&type_d);
}
/** /**
* @brief Function for process data from CAN * @brief Function for process data from CAN
* @details Function check your ID deviceю. Compare receive and calculated CRC. * @details Function check your ID deviceю. Compare receive and calculated CRC.
@ -151,7 +161,7 @@ void listen_can(const CAN_message_t &msg) {
msg_id = msg.id; msg_id = msg.id;
msg_ch = msg_id & 0xF; // Extract message channel msg_ch = msg_id & 0xF; // Extract message channel
uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
uint8_t data_type = 1; //type for work with foc
/* CRC Calculation */ /* CRC Calculation */
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8); uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; // Message buffer for CRC verification uint8_t data[10] = {0}; // Message buffer for CRC verification
@ -210,6 +220,18 @@ void listen_can(const CAN_message_t &msg) {
write_param(pid_d,conv_float_to_int.i); write_param(pid_d,conv_float_to_int.i);
break; break;
case DATA_TYPE_ANGLE:
data_type = DATA_TYPE_ANGLE;
break;
case DATA_TYPE_VELOCITY:
data_type = DATA_TYPE_VELOCITY;
break;
case DATA_TYPE_TORQUE:
data_type = DATA_TYPE_TORQUE;
break;
case FIRMWARE_UPDATE: case FIRMWARE_UPDATE:
firmware_update(); firmware_update();
break; break;
@ -232,6 +254,9 @@ void listen_can(const CAN_message_t &msg) {
case MOTOR_VELOCITY: send_velocity(); break; case MOTOR_VELOCITY: send_velocity(); break;
case MOTOR_ANGLE: send_angle(); break; case MOTOR_ANGLE: send_angle(); break;
case MOTOR_ENABLED: send_motor_enabled(); break; case MOTOR_ENABLED: send_motor_enabled(); break;
case DATA_TYPE_ANGLE: send_data_type(uint8_t(DATA_TYPE_ANGLE)); break;
case DATA_TYPE_VELOCITY: send_data_type(uint8_t(DATA_TYPE_VELOCITY)); break;
case DATA_TYPE_TORQUE: send_data_type(uint8_t(DATA_TYPE_TORQUE)); break;
// case MOTOR_TORQUE: send_motor_torque(); break; // case MOTOR_TORQUE: send_motor_torque(); break;
// case FOC_STATE: send_foc_state(); break; // case FOC_STATE: send_foc_state(); break;
case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break; case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
@ -243,7 +268,7 @@ void listen_can(const CAN_message_t &msg) {
/* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/ /* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
else{ else{
switch(msg.buf[0]) { switch(data_type) {
/* Read after write*/ /* Read after write*/
case DATA_TYPE_ANGLE: case DATA_TYPE_ANGLE:
send_angle(); send_angle();
@ -262,6 +287,11 @@ void listen_can(const CAN_message_t &msg) {
case DATA_TYPE_TORQUE: case DATA_TYPE_TORQUE:
send_torque(); send_torque();
break; break;
default:
send_error(error_foc);
break;
} }
} }
} }