servo/controller/fw/embed/src/main.cpp

441 lines
12 KiB
C++
Raw Normal View History

// clang-format off
2025-04-16 22:51:39 +03:00
#include "Arduino.h"
#include "stm32f446xx.h"
#include <SimpleFOC.h>
#include <STM32_CAN.h>
#include <AS5045.h>
#include <DRV8313.h>
#include <cstring>
2025-05-14 19:26:10 +03:00
#include <iostream>
#include <iterator>
#include "common/base_classes/FOCMotor.h"
#include "hal_conf_extra.h"
#include "wiring_analog.h"
#include "wiring_constants.h"
// clang-format on
2025-05-04 22:53:42 +03:00
2025-04-16 22:51:39 +03:00
#include "reg_cah.h"
#include "flash.h"
void SysTick_Handler(void) {
HAL_IncTick();
}
STM32_CAN Can(CAN2, DEF);
2025-04-18 12:50:07 +03:00
/* FLASH Related Definitions */
2025-04-16 22:51:39 +03:00
uint32_t flash_flag;
uint8_t flag_can = 0;
uint32_t flash_error;
FLASH_EraseInitTypeDef pEraseInit;
uint32_t SectorError;
2025-04-16 22:51:39 +03:00
volatile uint16_t msg_id;
volatile uint16_t id_x;
volatile uint8_t msg_ch;
volatile uint8_t crc_h;
volatile uint8_t crc_l;
2025-05-04 22:53:42 +03:00
/* bool for test CAN */
volatile bool CAN_GET = false;
2025-04-18 12:50:07 +03:00
volatile float kt = 0.1; // Torque calculation constant
2025-04-16 22:51:39 +03:00
static FLASH_RECORD* flash_rec;
static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;
SPIClass spi;
MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
BLDCMotor motor(POLE_PAIRS);
DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
RESET_DRIVER, FAULT_DRIVER);
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
CURRENT_SENSOR_2, CURRENT_SENSOR_3);
Commander command(Serial);
struct MotorControlInputs {
float target_angle = 0.0;
float target_velocity = 0.0;
bool motor_enabled = false;
2025-04-16 22:51:39 +03:00
bool foc_state = false;
};
MotorControlInputs motor_control_inputs;
void doMotor(char *cmd) {
command.motor(&motor, cmd);
digitalWrite(PC10, !digitalRead(PC10));
delayMicroseconds(2);
}
2025-05-04 22:53:42 +03:00
2025-04-16 22:51:39 +03:00
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
Commander *commander, CommandCallback callback) {
encoder->init(&spi);
2025-02-12 21:57:27 +08:00
2025-04-18 12:50:07 +03:00
// Driver configuration
driver->pwm_frequency = 20000;
2025-02-12 21:57:27 +08:00
driver->voltage_power_supply = 24;
driver->voltage_limit = 24;
2025-02-17 18:53:37 +03:00
driver->init();
2025-02-12 21:57:27 +08:00
2025-04-18 12:50:07 +03:00
// Current sense initialization
current_sense->linkDriver(driver);
current_sense->init();
2025-02-12 21:57:27 +08:00
2025-04-18 12:50:07 +03:00
// Motor configuration
2025-02-12 21:57:27 +08:00
motor->linkSensor(encoder);
motor->linkDriver(driver);
motor->linkCurrentSense(current_sense);
motor->useMonitoring(Serial);
2025-04-18 12:50:07 +03:00
motor->monitor_downsample = 5000; // Default monitoring interval
motor->controller = MotionControlType::angle;
motor->torque_controller = TorqueControlType::voltage;
2025-02-12 21:57:27 +08:00
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
2025-04-18 12:50:07 +03:00
// PID Configuration
2025-02-12 21:57:27 +08:00
motor->PID_velocity.P = 0.75;
motor->PID_velocity.I = 20;
2025-02-17 18:53:37 +03:00
motor->LPF_velocity.Tf = 0.005;
motor->P_angle.P = 0.5;
motor->LPF_angle.Tf = 0.001;
2025-02-12 21:57:27 +08:00
2025-04-18 12:50:07 +03:00
// Motor limits
motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm)
2025-02-12 21:57:27 +08:00
motor->voltage_limit = 24;
motor->current_limit = 0.5;
2025-02-17 18:53:37 +03:00
motor->sensor_direction = Direction::CCW;
motor->init();
motor->initFOC();
}
2025-05-14 19:26:10 +03:00
template <typename T>
void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
2025-04-18 12:50:07 +03:00
// Create CAN message
2025-05-05 14:53:37 +03:00
CAN_message_t msg_l;
msg_l.id = id;
2025-05-13 14:43:45 +03:00
// msg_l.len = 8; // Protocol-defined message length
2025-05-13 18:55:55 +03:00
memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
2025-05-14 19:26:10 +03:00
memcpy(&msg_l.buf[1], data, sizeof(T));
2025-04-17 11:10:56 +03:00
2025-04-18 12:50:07 +03:00
// Prepare CRC calculation buffer (ID + data)
2025-05-13 19:05:54 +03:00
uint8_t crc_data[CAN_MSG_MAX_LEN];
2025-04-17 11:10:56 +03:00
2025-05-13 18:55:55 +03:00
// Copy message ID
2025-05-14 19:26:10 +03:00
memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
2025-04-18 12:50:07 +03:00
// Copy all data bytes
2025-05-14 20:03:10 +03:00
memcpy(crc_data + 1, msg_l.buf, 6);
// Calculate CRC
2025-05-13 19:05:54 +03:00
uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
2025-05-05 14:53:37 +03:00
2025-04-18 12:50:07 +03:00
// Insert CRC into buffer
2025-05-05 14:53:37 +03:00
msg_l.buf[6] = crc_value & 0xFF;
msg_l.buf[7] = (crc_value >> 8) & 0xFF;
2025-04-17 11:10:56 +03:00
2025-04-18 12:50:07 +03:00
// Send message
2025-05-05 14:53:37 +03:00
Can.write(msg_l);
2025-04-17 11:10:56 +03:00
}
2025-05-04 22:53:42 +03:00
void send_velocity() {
2025-05-05 14:53:37 +03:00
float current_velocity = motor.shaftVelocity();
2025-05-04 22:53:42 +03:00
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
2025-04-17 11:10:56 +03:00
return;
}
2025-05-15 17:38:59 +03:00
float value = flash_rec[vel].value;
2025-04-17 11:10:56 +03:00
uint8_t id = flash_rec[addr_id].value;
2025-05-14 19:26:10 +03:00
send_can_with_id_crc(id,'V',&value);
}
void send_angle() {
float current_angle = motor.shaftAngle();
2025-04-18 12:50:07 +03:00
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
2025-04-17 11:10:56 +03:00
return;
}
uint8_t id = flash_rec[addr_id].value;
2025-05-14 19:26:10 +03:00
send_can_with_id_crc(id,'A',&current_angle);
}
void send_motor_enabled() {
2025-05-15 17:38:59 +03:00
int motor = 1;
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'M',&motor);
}
2025-04-16 22:51:39 +03:00
void send_foc_state() {
2025-04-18 12:50:07 +03:00
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
return;
}
2025-04-17 11:10:56 +03:00
uint8_t value = flash_rec[foc_id].value;
uint8_t id = flash_rec[addr_id].value;
2025-05-14 19:26:10 +03:00
send_can_with_id_crc(id,'F',&value);
2025-04-16 22:51:39 +03:00
}
void send_id() {
2025-04-18 12:50:07 +03:00
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc.
2025-04-16 22:51:39 +03:00
return;
}
2025-05-13 14:43:45 +03:00
2025-04-17 11:10:56 +03:00
uint8_t id = flash_rec[addr_id].value;
2025-05-14 19:26:10 +03:00
send_can_with_id_crc(id,'I',&id);
2025-04-16 22:51:39 +03:00
__NOP();
}
2025-05-15 17:38:59 +03:00
// 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);
// }
2025-04-16 22:51:39 +03:00
2025-04-17 16:55:48 +03:00
void send_pid(uint8_t param_pid){
2025-04-18 12:50:07 +03:00
if (flash_rec == nullptr) { // Null check
2025-04-17 16:49:39 +03:00
return;
}
uint8_t id = flash_rec[addr_id].value;
conv_float_to_int.i = flash_rec[param_pid].value;
uint32_t data = conv_float_to_int.i;
2025-05-15 17:38:59 +03:00
switch(param_pid){
case pid_p:
param_pid = REG_MOTOR_POSPID_Kp;
break;
case pid_i:
param_pid = REG_MOTOR_POSPID_Ki;
break;
case pid_d:
param_pid = REG_MOTOR_POSPID_Kd;
break;
}
send_can_with_id_crc(id,param_pid,&data);
2025-04-17 16:49:39 +03:00
}
2025-04-16 22:51:39 +03:00
void setup_id(uint8_t my_id) {
write_param(addr_id,my_id);
}
2025-04-17 15:53:03 +03:00
void setup_angle(float target_angle) {
2025-04-18 12:50:07 +03:00
motor.enable(); // Enable motor if disabled
2025-04-17 14:30:22 +03:00
motor.controller = MotionControlType::angle;
motor.move(target_angle);
2025-04-16 22:51:39 +03:00
}
2025-04-17 16:49:39 +03:00
void setup_pid_angle(uint8_t param_pid, float data){
2025-04-18 12:50:07 +03:00
switch (param_pid) {
case pid_p:
motor.P_angle.P = data;
break;
case pid_i:
motor.P_angle.I = data;
break;
case pid_d:
motor.P_angle.D = data;
break;
default:
break;
2025-04-17 16:49:39 +03:00
}
2025-05-15 17:38:59 +03:00
write_param(param_pid,data);
2025-04-17 16:49:39 +03:00
}
2025-04-16 22:51:39 +03:00
void listen_can(const CAN_message_t &msg) {
msg_id = msg.id;
2025-04-18 12:50:07 +03:00
msg_ch = msg_id & 0xF; // Extract message channel
id_x = (msg_id >> 4) & 0x7FF; // Extract device address
2025-04-16 22:51:39 +03:00
2025-04-18 12:50:07 +03:00
/* CRC Calculation */
2025-04-16 22:51:39 +03:00
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
2025-04-18 12:50:07 +03:00
uint8_t data[10] = {0}; // Message buffer for CRC verification
2025-04-16 22:51:39 +03:00
2025-04-18 12:50:07 +03:00
// Copy message ID (2 bytes)
2025-04-16 22:51:39 +03:00
memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
2025-04-18 12:50:07 +03:00
// Copy message data (excluding CRC bytes)
2025-04-16 22:51:39 +03:00
memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
2025-04-18 12:50:07 +03:00
// Calculate CRC
2025-04-16 22:51:39 +03:00
uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
2025-04-18 12:50:07 +03:00
// Verify CRC match
2025-04-16 22:51:39 +03:00
if (calculated_crc != received_crc) {
2025-04-18 12:50:07 +03:00
return; // Ignore message on CRC mismatch
2025-04-16 22:51:39 +03:00
}
2025-05-05 14:53:37 +03:00
flash_rec = load_params();
2025-04-18 12:50:07 +03:00
/* Message Structure: 0x691
69 - Device address
1 - Action code */
2025-05-05 14:53:37 +03:00
if(id_x == flash_rec[addr_id].value){
2025-04-16 22:51:39 +03:00
if(msg_ch == REG_WRITE){
2025-04-18 12:50:07 +03:00
switch(msg.buf[0]) {
case REG_ID:
setup_id(msg.buf[1]);
break;
case REG_LED_BLINK:
for (int i = 0; i < 10; i++) {
GPIOC->ODR ^= GPIO_ODR_OD10;
delay(100);
}
break;
2025-04-18 12:50:07 +03:00
case MOTOR_ANGLE:
2025-05-14 20:03:10 +03:00
memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
2025-04-18 12:50:07 +03:00
sizeof(motor_control_inputs.target_angle));
setup_angle(motor_control_inputs.target_angle);
break;
2025-04-18 12:50:07 +03:00
case REG_MOTOR_POSPID_Kp:
memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.P;
write_param(pid_p,conv_float_to_int.i);
2025-04-18 12:50:07 +03:00
break;
2025-04-18 12:50:07 +03:00
case REG_MOTOR_POSPID_Ki:
memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.I;
write_param(pid_i,conv_float_to_int.i);
2025-04-18 12:50:07 +03:00
break;
2025-04-18 12:50:07 +03:00
case REG_MOTOR_POSPID_Kd:
memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
conv_float_to_int.f = motor.P_angle.D;
write_param(pid_d,conv_float_to_int.i);
2025-04-18 12:50:07 +03:00
break;
2025-04-18 12:50:07 +03:00
case MOTOR_ENABLED:
if (msg.buf[1] == 1) {
motor.enable();
motor_control_inputs.motor_enabled = 1;
} else {
motor.disable();
motor_control_inputs.motor_enabled = 0;
}
default:
break;
2025-04-16 22:51:39 +03:00
}
}
2025-04-18 12:50:07 +03:00
else if (msg_ch == REG_READ) {
switch (msg.buf[0]) {
case REG_ID: send_id(); break;
case MOTOR_VELOCITY: send_velocity(); break;
case MOTOR_ANGLE: send_angle(); break;
case MOTOR_ENABLED: send_motor_enabled(); break;
2025-05-15 17:38:59 +03:00
// case MOTOR_TORQUE: send_motor_torque(); break;
2025-04-18 12:50:07 +03:00
case FOC_STATE: send_foc_state(); break;
case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break;
case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break;
case REG_MOTOR_POSPID_Kd: send_pid(pid_d); break;
default: break;
}
2025-04-16 22:51:39 +03:00
}
}
}
volatile uint32_t ipsr_value = 0;
void foc_step(BLDCMotor *motor, Commander *commander) {
if (motor_control_inputs.target_velocity != 0 ||
motor->controller == MotionControlType::velocity) {
if (motor->controller != MotionControlType::velocity) {
motor->controller = MotionControlType::velocity;
}
motor->target = motor_control_inputs.target_velocity;
} else {
if (motor->controller != MotionControlType::angle) {
motor->controller = MotionControlType::angle;
}
motor->target = motor_control_inputs.target_angle;
}
motor->loopFOC();
motor->move();
motor->monitor();
commander->run();
}
2025-05-04 22:53:42 +03:00
void setup(){
2025-04-18 12:50:07 +03:00
// Vector table initialization (commented out)
2025-05-04 22:53:42 +03:00
// __set_MSP(*(volatile uintф32_t*)0x08008000);
2025-04-16 22:51:39 +03:00
// SCB->VTOR = (volatile uint32_t)0x08008000;
2025-04-18 12:50:07 +03:00
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
Serial.begin(115200);
pinMode(PC11, OUTPUT);
pinMode(PC10,OUTPUT);
GPIOC->ODR &= ~GPIO_ODR_OD10;
2025-05-04 22:53:42 +03:00
// Can.enableMBInterrupts();
2025-04-18 12:50:07 +03:00
Can.begin();
Can.setBaudRate(1000000);
2025-05-04 22:53:42 +03:00
// Настройка прерываний CAN
CAN2->IER |= CAN_IER_FMPIE0;
2025-05-12 16:24:12 +03:00
flash_rec = load_params();
/* For test */
2025-05-15 17:38:59 +03:00
// float value = 3.46; //addr
// conv_float_to_int.f = value;
// write_param(pid_p,conv_float_to_int.i); //for update address in firmware
2025-05-15 17:38:59 +03:00
// // Load parameters from flash
// flash_rec = load_params();
2025-04-18 12:50:07 +03:00
// Initialize FOC system
setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
2025-04-18 12:50:07 +03:00
2025-05-04 22:53:42 +03:00
CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0
CAN_IER_FFIE0 | // FIFO0 full
CAN_IER_FOVIE0; // FIFO0 overflow
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 1, 0);
// HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
2025-04-18 12:50:07 +03:00
// Default motor configuration
2025-04-16 22:51:39 +03:00
GPIOC->ODR |= GPIO_ODR_OD11;
motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::torque;
__enable_irq();
2025-05-04 22:53:42 +03:00
2025-04-18 12:50:07 +03:00
}
2025-05-04 22:53:42 +03:00
void loop() {
2025-05-04 22:53:42 +03:00
__enable_irq();
2025-04-18 12:50:07 +03:00
foc_step(&motor, &command);
2025-04-16 22:51:39 +03:00
CAN_message_t msg;
2025-04-18 12:50:07 +03:00
GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED
2025-04-16 22:51:39 +03:00
delay(500);
2025-04-18 12:50:07 +03:00
2025-05-04 22:53:42 +03:00
// // Process incoming CAN messages
2025-04-16 22:51:39 +03:00
while (Can.read(msg)) {
2025-05-04 22:53:42 +03:00
CAN_GET = true;
}
/* If receive data from CAN */
if(CAN_GET) {
2025-04-16 22:51:39 +03:00
listen_can(msg);
}
2025-05-04 22:53:42 +03:00
}