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

For all types

Browse source
This commit is contained in:
Valentin Dabstep 2025-05-14 19:26:10 +03:00
parent fca10d4140
commit 3e35fd99a1
1 changed files with 11 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

20
controller/fw/embed/src/main.cpp
View file

@ -6,6 +6,7 @@
#include <AS5045.h> #include <AS5045.h>
#include <DRV8313.h> #include <DRV8313.h>
#include <cstring> #include <cstring>
#include <iostream>
#include <iterator> #include <iterator>
#include "common/base_classes/FOCMotor.h" #include "common/base_classes/FOCMotor.h"
#include "hal_conf_extra.h" #include "hal_conf_extra.h"
@ -117,22 +118,23 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
motor->initFOC(); motor->initFOC();
} }
void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length = 7) { template <typename T>
void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
// Create CAN message // Create CAN message
CAN_message_t msg_l; CAN_message_t msg_l;
msg_l.id = id; msg_l.id = id;
// msg_l.len = 8; // Protocol-defined message length // msg_l.len = 8; // Protocol-defined message length
memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t)); memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
memcpy(&msg_l.buf[1], data, ID_SIZE); memcpy(&msg_l.buf[1], data, sizeof(T));
// Prepare CRC calculation buffer (ID + data) // Prepare CRC calculation buffer (ID + data)
uint8_t crc_data[CAN_MSG_MAX_LEN]; uint8_t crc_data[CAN_MSG_MAX_LEN];
// Copy message ID // Copy message ID
memcpy(crc_data, (uint8_t*)&msg_l.id, ID_SIZE); memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
// Copy all data bytes // Copy all data bytes
for(int i = 1;i < CAN_MSG_MAX_LEN;i++) for(int i = 1;i < CAN_MSG_MAX_LEN;i++)
memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(uint8_t)); //As byte variable memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(T)); //As byte variable
// Calculate CRC // Calculate CRC
uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN); uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
@ -152,7 +154,7 @@ void send_velocity() {
} }
uint8_t value = flash_rec[vel].value; uint8_t value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value,sizeof(value)); send_can_with_id_crc(id,'V',&value);
} }
void send_angle() { void send_angle() {
@ -162,7 +164,7 @@ void send_angle() {
return; return;
} }
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle)); send_can_with_id_crc(id,'A',&current_angle);
} }
void send_motor_enabled() { void send_motor_enabled() {
@ -182,7 +184,7 @@ void send_foc_state() {
} }
uint8_t value = flash_rec[foc_id].value; uint8_t value = flash_rec[foc_id].value;
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'F',&value,sizeof(value)); send_can_with_id_crc(id,'F',&value);
} }
void send_id() { void send_id() {
@ -193,7 +195,7 @@ void send_id() {
} }
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'I',&id,sizeof(id)); send_can_with_id_crc(id,'I',&id);
__NOP(); __NOP();
} }
@ -227,7 +229,7 @@ void send_pid(uint8_t param_pid){
if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp; if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki; else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd; else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send)); send_can_with_id_crc(id,param_pid,&data_send);
} }
void setup_id(uint8_t my_id) { void setup_id(uint8_t my_id) {