2024-05-08 10:05:29 +00:00
|
|
|
|
// clang-format off
|
2025-04-16 22:51:39 +03:00
|
|
|
|
#include "Arduino.h"
|
|
|
|
|
#include "stm32f446xx.h"
|
2024-05-08 10:05:29 +00:00
|
|
|
|
#include <SimpleFOC.h>
|
|
|
|
|
#include <STM32_CAN.h>
|
|
|
|
|
#include <AS5045.h>
|
|
|
|
|
#include <DRV8313.h>
|
2025-01-18 18:28:56 +03:00
|
|
|
|
#include <cstring>
|
|
|
|
|
#include <iterator>
|
|
|
|
|
#include "common/base_classes/FOCMotor.h"
|
2024-05-08 10:05:29 +00:00
|
|
|
|
#include "hal_conf_extra.h"
|
2025-01-18 18:28:56 +03:00
|
|
|
|
#include "wiring_analog.h"
|
|
|
|
|
#include "wiring_constants.h"
|
2024-05-08 10:05:29 +00:00
|
|
|
|
// clang-format on
|
2025-04-16 22:51:39 +03:00
|
|
|
|
#include "reg_cah.h"
|
|
|
|
|
#include "flash.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void SysTick_Handler(void) {
|
|
|
|
|
HAL_IncTick();
|
|
|
|
|
}
|
|
|
|
|
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
|
|
|
|
STM32_CAN Can(CAN2, DEF);
|
2025-04-16 22:51:39 +03:00
|
|
|
|
/* for FLASH */
|
|
|
|
|
uint32_t flash_flag;
|
|
|
|
|
uint8_t flag_can = 0;
|
|
|
|
|
uint32_t flash_error;
|
|
|
|
|
FLASH_EraseInitTypeDef pEraseInit;
|
|
|
|
|
uint32_t SectorError;
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
volatile float kt = 0.1; //for torgue calculation
|
|
|
|
|
|
|
|
|
|
static FLASH_RECORD* flash_rec;
|
|
|
|
|
static FLASH_RECORD flash_buf[PARAM_COUNT];
|
2024-05-08 10:05:29 +00:00
|
|
|
|
static CAN_message_t CAN_TX_msg;
|
|
|
|
|
static CAN_message_t CAN_inMsg;
|
|
|
|
|
|
|
|
|
|
SPIClass spi;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
|
|
|
|
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);
|
2025-01-18 18:28:56 +03:00
|
|
|
|
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
|
|
|
|
|
CURRENT_SENSOR_2, CURRENT_SENSOR_3);
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
|
|
|
|
Commander command(Serial);
|
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
struct MotorControlInputs {
|
2025-02-04 13:40:15 +03:00
|
|
|
|
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;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
MotorControlInputs motor_control_inputs;
|
|
|
|
|
|
2024-05-08 10:05:29 +00:00
|
|
|
|
void doMotor(char *cmd) {
|
2025-01-18 18:28:56 +03:00
|
|
|
|
command.motor(&motor, cmd);
|
2025-02-04 13:40:15 +03:00
|
|
|
|
digitalWrite(PC10, !digitalRead(PC10));
|
|
|
|
|
delayMicroseconds(2);
|
2025-01-18 18:28:56 +03:00
|
|
|
|
}
|
|
|
|
|
|
2025-04-16 22:51:39 +03:00
|
|
|
|
void CAN2_RX0_IRQHandler() {
|
|
|
|
|
// Пустая функция, но прерывание не приведет к Default Handler
|
|
|
|
|
}
|
|
|
|
|
|
2025-01-18 18:28:56 +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-01-18 18:28:56 +03:00
|
|
|
|
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-01-18 18:28:56 +03:00
|
|
|
|
current_sense->linkDriver(driver);
|
|
|
|
|
current_sense->init();
|
2025-02-12 21:57:27 +08:00
|
|
|
|
|
|
|
|
|
motor->linkSensor(encoder);
|
|
|
|
|
motor->linkDriver(driver);
|
2025-01-18 18:28:56 +03:00
|
|
|
|
motor->linkCurrentSense(current_sense);
|
|
|
|
|
motor->useMonitoring(Serial);
|
2025-02-17 18:53:37 +03:00
|
|
|
|
motor->monitor_downsample = 5000; // default 0
|
2025-01-18 18:28:56 +03:00
|
|
|
|
motor->controller = MotionControlType::angle;
|
|
|
|
|
motor->torque_controller = TorqueControlType::voltage;
|
2025-02-12 21:57:27 +08:00
|
|
|
|
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
|
|
|
|
|
|
2025-02-17 18:53:37 +03:00
|
|
|
|
// PID start
|
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;
|
|
|
|
|
// PID end
|
2025-02-12 21:57:27 +08:00
|
|
|
|
|
2025-02-17 18:53:37 +03:00
|
|
|
|
motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm)
|
2025-02-12 21:57:27 +08:00
|
|
|
|
motor->voltage_limit = 24;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
motor->current_limit = 0.5;
|
2025-02-17 18:53:37 +03:00
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
motor->sensor_direction = Direction::CCW;
|
|
|
|
|
motor->init();
|
|
|
|
|
motor->initFOC();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void send_velocity() {
|
|
|
|
|
float current_velocity = motor.shaftVelocity();
|
2025-04-16 22:51:39 +03:00
|
|
|
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
|
|
|
|
CAN_TX_msg.id = id;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
CAN_TX_msg.buf[0] = 'V';
|
|
|
|
|
CAN_TX_msg.len = 5;
|
|
|
|
|
memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity));
|
|
|
|
|
Can.write(CAN_TX_msg);
|
2024-05-08 10:05:29 +00:00
|
|
|
|
}
|
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
void send_angle() {
|
|
|
|
|
float current_angle = motor.shaftAngle();
|
2025-04-16 22:51:39 +03:00
|
|
|
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
|
|
|
|
CAN_TX_msg.id = id;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
CAN_TX_msg.buf[0] = 'A';
|
|
|
|
|
CAN_TX_msg.len = 5;
|
|
|
|
|
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
|
|
|
|
Can.write(CAN_TX_msg);
|
2024-05-08 10:05:29 +00:00
|
|
|
|
}
|
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
void send_motor_enabled() {
|
2025-04-16 22:51:39 +03:00
|
|
|
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
|
|
|
|
CAN_TX_msg.id = id;
|
2025-01-18 18:28:56 +03:00
|
|
|
|
CAN_TX_msg.buf[0] = 'E';
|
|
|
|
|
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
|
|
|
|
|
sizeof(motor_control_inputs.motor_enabled));
|
|
|
|
|
Can.write(CAN_TX_msg);
|
|
|
|
|
}
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
2025-04-16 22:51:39 +03:00
|
|
|
|
void send_foc_state() {
|
|
|
|
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
|
|
|
|
CAN_TX_msg.id = id;
|
|
|
|
|
CAN_TX_msg.buf[0] = 'F';
|
|
|
|
|
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state,
|
|
|
|
|
sizeof(motor_control_inputs.foc_state));
|
|
|
|
|
Can.write(CAN_TX_msg);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void send_id() {
|
|
|
|
|
/* data for reading of firmware */
|
|
|
|
|
flash_rec = load_params();
|
|
|
|
|
if (flash_rec == nullptr) { // Проверка на NULL
|
|
|
|
|
// Обработка ошибки: запись в лог, сигнализация и т.д.
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
CAN_TX_msg.id = flash_rec->value;
|
|
|
|
|
CAN_TX_msg.len = 8;
|
|
|
|
|
CAN_TX_msg.buf[0] = 'I';
|
|
|
|
|
memcpy(&CAN_TX_msg.buf[1], &(flash_rec->value), sizeof(uint8_t));
|
|
|
|
|
|
|
|
|
|
uint8_t crc_data[sizeof(CAN_TX_msg.id) + 2] = {0}; // Размер: размер ID + 2 байта данных
|
|
|
|
|
memcpy(crc_data, &CAN_TX_msg.id, sizeof(CAN_TX_msg.id)); // Копируем ID (11/29 бит)
|
|
|
|
|
memcpy(crc_data + sizeof(CAN_TX_msg.id), &CAN_TX_msg.buf[0], 2); // Копируем 'I' и value
|
|
|
|
|
// Расчет CRC
|
|
|
|
|
uint16_t crc_value = validate_crc16(crc_data, sizeof(crc_data));
|
|
|
|
|
|
|
|
|
|
// Добавление CRC к сообщению
|
|
|
|
|
CAN_TX_msg.buf[6] = crc_value & 0xFF; // Добавляем старший байт CRC
|
|
|
|
|
CAN_TX_msg.buf[7] = (crc_value >> 8) & 0xFF; // Добавляем младший байт CRC
|
|
|
|
|
|
|
|
|
|
Can.write(CAN_TX_msg);
|
|
|
|
|
__NOP();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void send_motor_torque() {
|
|
|
|
|
float i_q = motor.current.q; // Ток по оси q (А)
|
|
|
|
|
float torque = kt * i_q; // Расчет момента
|
|
|
|
|
torque *= 100;
|
|
|
|
|
flash_rec = load_params();
|
|
|
|
|
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 setup_id(uint8_t my_id) {
|
|
|
|
|
write_param(addr_id,my_id);
|
|
|
|
|
send_id();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2024-05-08 10:05:29 +00:00
|
|
|
|
void send_data() {
|
2025-04-16 22:51:39 +03:00
|
|
|
|
// send_velocity();
|
|
|
|
|
// send_angle();
|
|
|
|
|
// send_motor_enabled();
|
2025-01-18 18:28:56 +03:00
|
|
|
|
// read_temperature();
|
2025-04-16 22:51:39 +03:00
|
|
|
|
// GPIOC->ODR ^= GPIO_ODR_OD11;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void listen_can(const CAN_message_t &msg) {
|
|
|
|
|
msg_id = msg.id;
|
|
|
|
|
|
|
|
|
|
msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
|
|
|
|
|
id_x = (msg_id >> 4) & 0x7FF; //получение адреса устройства страшие 2 бита msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* Вычисление CRC */
|
|
|
|
|
// Объединение старшего и младшего байтов для получения полученного CRC
|
|
|
|
|
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
|
|
|
|
|
uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки
|
|
|
|
|
|
|
|
|
|
// Копируем ID сообщения в буфер данных для расчета CRC 2 байта
|
|
|
|
|
memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
|
|
|
|
|
|
|
|
|
|
// Копируем данные сообщения в буфер (без байтов CRC)
|
|
|
|
|
memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
|
|
|
|
|
|
|
|
|
|
// Рассчитываем CRC для полученных данных
|
|
|
|
|
uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
|
|
|
|
|
|
|
|
|
|
// Проверяем совпадение CRC
|
|
|
|
|
if (calculated_crc != received_crc) {
|
|
|
|
|
// Несовпадение CRC, игнорируем сообщение
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* 0x691
|
|
|
|
|
69 - адрес устройства
|
|
|
|
|
1 - что делать дальше с данными */
|
|
|
|
|
|
|
|
|
|
if(id_x == flash_rec->value){
|
|
|
|
|
if(msg_ch == REG_WRITE){
|
|
|
|
|
switch(msg.buf[0]) {
|
|
|
|
|
case REG_ID:
|
|
|
|
|
/* setup new 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;
|
|
|
|
|
|
|
|
|
|
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-01-18 18:28:56 +03:00
|
|
|
|
}
|
|
|
|
|
}
|
2025-04-16 22:51:39 +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;
|
|
|
|
|
|
|
|
|
|
case MOTOR_TORQUE:
|
|
|
|
|
send_motor_torque();
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case FOC_STATE:
|
|
|
|
|
send_foc_state();
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
2024-05-08 10:05:29 +00:00
|
|
|
|
}
|
|
|
|
|
|
2025-04-16 22:51:39 +03:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
volatile uint32_t ipsr_value = 0;
|
|
|
|
|
|
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
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;
|
2025-02-12 21:57:27 +08:00
|
|
|
|
|
2025-01-18 18:28:56 +03:00
|
|
|
|
} 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();
|
2024-05-08 10:05:29 +00:00
|
|
|
|
}
|
|
|
|
|
|
2025-04-16 22:51:39 +03:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void setup(){
|
|
|
|
|
/* bias for vector int */
|
|
|
|
|
// __set_MSP(*(volatile uint32_t*)0x08008000);
|
|
|
|
|
// SCB->VTOR = (volatile uint32_t)0x08008000;
|
|
|
|
|
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;
|
|
|
|
|
// Setup thermal sensor pin
|
|
|
|
|
// pinMode(TH1, INPUT_ANALOG);
|
|
|
|
|
Can.begin();
|
|
|
|
|
Can.setBaudRate(1000000);
|
|
|
|
|
TIM_TypeDef *Instance = TIM2;
|
|
|
|
|
HardwareTimer *SendTimer = new HardwareTimer(Instance);
|
|
|
|
|
// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
|
|
|
|
// SendTimer->attachInterrupt(send_data);
|
|
|
|
|
// SendTimer->resume();
|
|
|
|
|
flash_rec = load_params();
|
|
|
|
|
for(int i = 0;i < PARAM_COUNT;i++)
|
|
|
|
|
flash_buf[i] = flash_rec[i];
|
2025-01-18 18:28:56 +03:00
|
|
|
|
setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);
|
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();
|
|
|
|
|
}
|
2024-05-08 10:05:29 +00:00
|
|
|
|
|
|
|
|
|
void loop() {
|
2025-04-16 22:51:39 +03:00
|
|
|
|
foc_step(&motor, &command);
|
|
|
|
|
CAN_message_t msg;
|
|
|
|
|
GPIOC->ODR ^= GPIO_ODR_OD11;
|
|
|
|
|
delay(500);
|
|
|
|
|
while (Can.read(msg)) {
|
|
|
|
|
listen_can(msg);
|
2025-01-18 18:28:56 +03:00
|
|
|
|
}
|
2024-05-08 10:05:29 +00:00
|
|
|
|
}
|