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

translate comment

Browse source
This commit is contained in:
lulko 2025-04-18 12:50:07 +03:00
parent 7ef7228b31
commit a2f1c2557a
3 changed files with 146 additions and 212 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -40,7 +40,7 @@ enum {
// Flash keys for unlocking flash memory // Flash keys for unlocking flash memory
#define BYTE32 0 #define BYTE32 0
#define BYTE8 1 #define BYTE8 1
#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение #define UPDATE_FLAG 0xDEADBEEF // Unique 32bit value
//FLASH SET ONE PROGRAMM WORD //FLASH SET ONE PROGRAMM WORD
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1 #define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
#define FLASH_32BYTE \ #define FLASH_32BYTE \

30
controller/fw/embed/src/flash.cpp
View file

@ -120,18 +120,18 @@ bool validata_crc(FLASH_RECORD* crc){
} }
uint16_t validate_crc16(uint8_t *data, uint32_t length) { uint16_t validate_crc16(uint8_t *data, uint32_t length) {
uint16_t crc = 0xFFFF; // Начальное значение для MODBUS uint16_t crc = 0xFFFF; // start value for CRC MODBUS
while (length--) { while (length--) {
crc ^= *data++; // XOR с очередным байтом данных crc ^= *data++; // XOR
for (uint8_t i = 0; i < 8; i++) { for (uint8_t i = 0; i < 8; i++) {
if (crc & 0x0001) { if (crc & 0x0001) {
crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse) crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse)
} else { } else {
crc >>= 1; crc >>= 1;
} }
} }
} }
return crc; // Возвращаем вычисленный CRC return crc;
} }
@ -152,11 +152,11 @@ void compact_page(){
uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) { if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) {
// Если CRC совпадает и ID параметра валидный, сохраняем последнее значение // if the crc does not match, we check further
latest[rec.data_id] = rec; latest[rec.data_id] = rec;
} }
else else
//Если не совпадает продолжить читать флэш // if
continue; continue;
} }
@ -164,7 +164,7 @@ void compact_page(){
write_ptr = SECTOR_6; // Сброс на начало write_ptr = SECTOR_6; // Сброс на начало
for (int i = 0; i < PARAM_COUNT; i++) { for (int i = 0; i < PARAM_COUNT; i++) {
if (latest[i].data_id != 0xFF) { if (latest[i].data_id != 0xFF) {
// Выравнивание перед каждой записью // alignment
if (write_ptr % 4 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4)); write_ptr += (4 - (write_ptr % 4));
} }
@ -177,27 +177,27 @@ void compact_page(){
void write_param(uint8_t param_id, uint8_t val) { void write_param(uint8_t param_id, uint8_t val) {
FLASH_RECORD param_flash = {param_id, val}; FLASH_RECORD param_flash = {param_id, val};
// __disable_irq(); // Запрещаем прерывания на время всей операции // __disable_irq(); // Interrupt off
param_flash.crc = validate_crc16((uint8_t*)&param_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти param_flash.crc = validate_crc16((uint8_t*)&param_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти
// Проверка выравнивания ДО проверки границ сектора кратного 4 // check alignment
if (write_ptr % 4 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4)); write_ptr += (4 - (write_ptr % 4));
} }
// Проверка переполнения с учётом выравнивания // check buffer overflow
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) { if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
compact_page(); // После compact_page write_ptr обновляется compact_page(); // after compact_page update
// Повторно выравниваем после функции. То есть сколько не хватает для кратности // alignment
if (write_ptr % 4 != 0) { if (write_ptr % 4 != 0) {
write_ptr += (4 - (write_ptr % 4)); write_ptr += (4 - (write_ptr % 4));
} }
} }
flash_write(write_ptr, &param_flash); //внутри функции итак автоматические инкрементируется указатель write_ptr на размер структуры flash_write(write_ptr, &param_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure
// __enable_irq(); // Разрешаем прерывания // __enable_irq(); // Interrupt on
} }
FLASH_RECORD* load_params(){ FLASH_RECORD* load_params(){
@ -206,7 +206,7 @@ FLASH_RECORD* load_params(){
FLASH_RECORD res; FLASH_RECORD res;
for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
flash_read(addr,&res); flash_read(addr,&res);
/* провекра CRC */ /* check CRC */
uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC
if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT)
continue; continue;

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

@ -15,14 +15,12 @@
#include "reg_cah.h" #include "reg_cah.h"
#include "flash.h" #include "flash.h"
void SysTick_Handler(void) { void SysTick_Handler(void) {
HAL_IncTick(); HAL_IncTick();
} }
STM32_CAN Can(CAN2, DEF); STM32_CAN Can(CAN2, DEF);
/* for FLASH */ /* FLASH Related Definitions */
uint32_t flash_flag; uint32_t flash_flag;
uint8_t flag_can = 0; uint8_t flag_can = 0;
uint32_t flash_error; uint32_t flash_error;
@ -35,7 +33,7 @@ volatile uint8_t msg_ch;
volatile uint8_t crc_h; volatile uint8_t crc_h;
volatile uint8_t crc_l; volatile uint8_t crc_l;
volatile float kt = 0.1; //for torgue calculation volatile float kt = 0.1; // Torque calculation constant
static FLASH_RECORD* flash_rec; static FLASH_RECORD* flash_rec;
static FLASH_RECORD flash_buf[PARAM_COUNT]; static FLASH_RECORD flash_buf[PARAM_COUNT];
@ -70,8 +68,8 @@ void doMotor(char *cmd) {
delayMicroseconds(2); delayMicroseconds(2);
} }
// Empty interrupt handler to prevent default handling
void CAN2_RX0_IRQHandler() { void CAN2_RX0_IRQHandler() {
// Пустая функция, но прерывание не приведет к Default Handler
} }
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
@ -79,32 +77,35 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
Commander *commander, CommandCallback callback) { Commander *commander, CommandCallback callback) {
encoder->init(&spi); encoder->init(&spi);
// Driver configuration
driver->pwm_frequency = 20000; driver->pwm_frequency = 20000;
driver->voltage_power_supply = 24; driver->voltage_power_supply = 24;
driver->voltage_limit = 24; driver->voltage_limit = 24;
driver->init(); driver->init();
// Current sense initialization
current_sense->linkDriver(driver); current_sense->linkDriver(driver);
current_sense->init(); current_sense->init();
// Motor configuration
motor->linkSensor(encoder); motor->linkSensor(encoder);
motor->linkDriver(driver); motor->linkDriver(driver);
motor->linkCurrentSense(current_sense); motor->linkCurrentSense(current_sense);
motor->useMonitoring(Serial); motor->useMonitoring(Serial);
motor->monitor_downsample = 5000; // default 0 motor->monitor_downsample = 5000; // Default monitoring interval
motor->controller = MotionControlType::angle; motor->controller = MotionControlType::angle;
motor->torque_controller = TorqueControlType::voltage; motor->torque_controller = TorqueControlType::voltage;
motor->foc_modulation = FOCModulationType::SpaceVectorPWM; motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
// PID start // PID Configuration
motor->PID_velocity.P = 0.75; motor->PID_velocity.P = 0.75;
motor->PID_velocity.I = 20; motor->PID_velocity.I = 20;
motor->LPF_velocity.Tf = 0.005; motor->LPF_velocity.Tf = 0.005;
motor->P_angle.P = 0.5; motor->P_angle.P = 0.5;
motor->LPF_angle.Tf = 0.001; motor->LPF_angle.Tf = 0.001;
// PID end
motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm) // Motor limits
motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm)
motor->voltage_limit = 24; motor->voltage_limit = 24;
motor->current_limit = 0.5; motor->current_limit = 0.5;
@ -114,41 +115,39 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
} }
void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) { void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) {
// Создаем сообщение // Create CAN message
CAN_message_t msg; CAN_message_t msg;
msg.id = id; msg.id = id;
msg.len = 8; // или как в протоколе msg.len = 8; // Protocol-defined message length
msg.buf[0] = message_type; msg.buf[0] = message_type;
memcpy(&msg.buf[1], data, data_length); memcpy(&msg.buf[1], data, data_length);
// Формируем массив для CRC, включающий ID и все данные // Prepare CRC calculation buffer (ID + data)
size_t crc_data_size = sizeof(msg.id) + data_length; size_t crc_data_size = sizeof(msg.id) + data_length;
uint8_t crc_data[crc_data_size]; uint8_t crc_data[crc_data_size];
// Копируем ID // Copy message ID
memcpy(crc_data, &msg.id, sizeof(msg.id)); memcpy(crc_data, &msg.id, sizeof(msg.id));
// Копируем все байты data // Copy all data bytes
memcpy(crc_data + sizeof(msg.id), data, data_length); memcpy(crc_data + sizeof(msg.id), data, data_length);
// Расчет CRC // Calculate CRC
uint16_t crc_value = validate_crc16(crc_data, crc_data_size); uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
// Вставляем CRC в буфер // Insert CRC into buffer
msg.buf[6] = crc_value & 0xFF; msg.buf[6] = crc_value & 0xFF;
msg.buf[7] = (crc_value >> 8) & 0xFF; msg.buf[7] = (crc_value >> 8) & 0xFF;
// Отправляем // Send message
Can.write(msg); Can.write(msg);
__NOP(); __NOP();
} }
void send_velocity() { void send_velocity() {
float current_velocity = motor.shaftVelocity(); float current_velocity = motor.shaftVelocity();
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Null check
// Обработка ошибки: запись в лог, сигнализация и т.д. // Error handling: logging, alerts, etc.
return; return;
} }
uint8_t value = flash_rec[vel].value; uint8_t value = flash_rec[vel].value;
@ -158,13 +157,11 @@ void send_velocity() {
void send_angle() { void send_angle() {
float current_angle = motor.shaftAngle(); float current_angle = motor.shaftAngle();
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Null check
// Обработка ошибки: запись в лог, сигнализация и т.д. // Error handling: logging, alerts, etc.
return; return;
} }
// uint8_t value = flash_rec[angl].value;
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,sizeof(current_angle));
} }
@ -179,23 +176,22 @@ void send_motor_enabled() {
} }
void send_foc_state() { void send_foc_state() {
/* data for reading of firmware */ /* Firmware data reading */
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Null check
// Обработка ошибки: запись в лог, сигнализация и т.д. // Error handling: logging, alerts, etc.
return; return;
} }
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,sizeof(value));
} }
void send_id() { void send_id() {
/* data for reading of firmware */ /* Firmware data reading */
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Null check
// Обработка ошибки: запись в лог, сигнализация и т.д. // Error handling: logging, alerts, etc.
return; return;
} }
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
@ -204,8 +200,8 @@ void send_id() {
} }
void send_motor_torque() { void send_motor_torque() {
float i_q = motor.current.q; // Ток по оси q (А) float i_q = motor.current.q; // Q-axis current (A)
float torque = kt * i_q; // Расчет момента float torque = kt * i_q; // Torque calculation
torque *= 100; torque *= 100;
flash_rec = load_params(); flash_rec = load_params();
CAN_TX_msg.id = flash_rec->value; CAN_TX_msg.id = flash_rec->value;
@ -215,10 +211,9 @@ void send_motor_torque() {
Can.write(CAN_TX_msg); Can.write(CAN_TX_msg);
} }
void send_pid(uint8_t param_pid){ void send_pid(uint8_t param_pid){
flash_rec = load_params(); flash_rec = load_params();
if (flash_rec == nullptr) { // Проверка на NULL if (flash_rec == nullptr) { // Null check
return; return;
} }
uint8_t id = flash_rec[addr_id].value; uint8_t id = flash_rec[addr_id].value;
@ -229,10 +224,8 @@ void send_pid(uint8_t param_pid){
l++; l++;
if(l >= 2) if(l >= 2)
data_send = (float)d; data_send = (float)d;
else if(l == 1) else if(l == 1)
data_send = (float)(d * 10); data_send = (float)(d * 10);
else else
data_send = (float)(d * 100); data_send = (float)(d * 100);
if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp; if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
@ -243,180 +236,122 @@ void send_pid(uint8_t param_pid){
void setup_id(uint8_t my_id) { void setup_id(uint8_t my_id) {
write_param(addr_id,my_id); write_param(addr_id,my_id);
// send_id();
} }
void setup_angle(float target_angle) { void setup_angle(float target_angle) {
// float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы motor.enable(); // Enable motor if disabled
motor.enable(); // Включаем мотор если он отключен
motor.controller = MotionControlType::angle; motor.controller = MotionControlType::angle;
motor.move(target_angle); motor.move(target_angle);
} }
void setup_pid_angle(uint8_t param_pid, float data){ void setup_pid_angle(uint8_t param_pid, float data){
switch (param_pid) switch (param_pid) {
{ case pid_p:
case pid_p: motor.P_angle.P = data;
motor.P_angle.P = data; break;
break; case pid_i:
motor.P_angle.I = data;
case pid_i: break;
motor.P_angle.I = data; case pid_d:
break; motor.P_angle.D = data;
break;
case pid_d: default:
motor.P_angle.D = data; break;
break;
default:
break;
} }
uint8_t check = uint8_t(data); uint8_t check = uint8_t(data);
uint8_t data_save = 0; uint8_t data_save = 0;
if(check != 0) if(check != 0)
if(check /= 10) if(check /= 10)
data_save = check; data_save = check;
else else
data_save = (uint8_t)(data * 10); data_save = (uint8_t)(data * 10);
else else
data_save = (uint8_t)(data * 100); data_save = (uint8_t)(data * 100);
write_param(param_pid,data_save); write_param(param_pid,data_save);
} }
void listen_can(const CAN_message_t &msg) { void listen_can(const CAN_message_t &msg) {
msg_id = msg.id; msg_id = msg.id;
msg_ch = msg_id & 0xF; // Extract message channel
id_x = (msg_id >> 4) & 0x7FF; // Extract device address
msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать /* CRC Calculation */
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); uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки uint8_t data[10] = {0}; // Message buffer for CRC verification
// Копируем ID сообщения в буфер данных для расчета CRC 2 байта // Copy message ID (2 bytes)
memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id)); memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
// Copy message data (excluding CRC bytes)
// Копируем данные сообщения в буфер (без байтов CRC)
memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2); memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
// Рассчитываем CRC для полученных данных // Calculate CRC
uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2); uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
// Проверяем совпадение CRC // Verify CRC match
if (calculated_crc != received_crc) { if (calculated_crc != received_crc) {
// Несовпадение CRC, игнорируем сообщение return; // Ignore message on CRC mismatch
return;
} }
/* Message Structure: 0x691
/* 0x691 69 - Device address
69 - адрес устройства 1 - Action code */
1 - что делать дальше с данными */
if(id_x == flash_rec->value){ if(id_x == flash_rec->value){
if(msg_ch == REG_WRITE){ if(msg_ch == REG_WRITE){
switch(msg.buf[0]) { switch(msg.buf[0]) {
case REG_ID: case REG_ID:
/* setup new id */ setup_id(msg.buf[1]);
setup_id(msg.buf[1]); break;
break; case REG_LED_BLINK:
for (int i = 0; i < 10; i++) {
case REG_LED_BLINK: GPIOC->ODR ^= GPIO_ODR_OD10;
for (int i = 0; i < 10; i++) { delay(100);
GPIOC->ODR ^= GPIO_ODR_OD10; }
delay(100); break;
} case MOTOR_ANGLE:
break; memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
sizeof(motor_control_inputs.target_angle));
case MOTOR_ANGLE: setup_angle(motor_control_inputs.target_angle);
memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], break;
sizeof(motor_control_inputs.target_angle)); case REG_MOTOR_POSPID_Kp:
setup_angle(motor_control_inputs.target_angle); setup_pid_angle(pid_p,msg.buf[1]);
break; break;
case REG_MOTOR_POSPID_Ki:
case REG_MOTOR_POSPID_Kp: setup_pid_angle(pid_i,msg.buf[1]);
setup_pid_angle(pid_p,msg.buf[1]); break;
break; case REG_MOTOR_POSPID_Kd:
setup_pid_angle(pid_d,msg.buf[1]);
case REG_MOTOR_POSPID_Ki: break;
setup_pid_angle(pid_i,msg.buf[1]); case MOTOR_ENABLED:
break; if (msg.buf[1] == 1) {
motor.enable();
case REG_MOTOR_POSPID_Kd: motor_control_inputs.motor_enabled = 1;
setup_pid_angle(pid_d,msg.buf[1]); } else {
break; motor.disable();
motor_control_inputs.motor_enabled = 0;
case MOTOR_ENABLED: }
if (msg.buf[1] == 1) { default:
motor.enable(); break;
motor_control_inputs.motor_enabled = 1;
} else {
motor.disable();
motor_control_inputs.motor_enabled = 0;
} }
default:
break;
} }
} else if (msg_ch == REG_READ) {
else if (msg_ch == REG_READ) { switch (msg.buf[0]) {
switch (msg.buf[0]) { case REG_ID: send_id(); break;
case REG_ID: case MOTOR_VELOCITY: send_velocity(); break;
send_id(); case MOTOR_ANGLE: send_angle(); break;
break; case MOTOR_ENABLED: send_motor_enabled(); break;
case MOTOR_VELOCITY: case MOTOR_TORQUE: send_motor_torque(); break;
send_velocity(); case FOC_STATE: send_foc_state(); break;
break; case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break;
case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break;
case MOTOR_ANGLE: case REG_MOTOR_POSPID_Kd: send_pid(pid_d); break;
send_angle(); default: break;
break; }
case MOTOR_ENABLED:
send_motor_enabled();
break;
case MOTOR_TORQUE:
send_motor_torque();
break;
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;
} }
} }
} }
}
volatile uint32_t ipsr_value = 0; volatile uint32_t ipsr_value = 0;
void foc_step(BLDCMotor *motor, Commander *commander) { void foc_step(BLDCMotor *motor, Commander *commander) {
if (motor_control_inputs.target_velocity != 0 || if (motor_control_inputs.target_velocity != 0 ||
motor->controller == MotionControlType::velocity) { motor->controller == MotionControlType::velocity) {
@ -424,7 +359,6 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
motor->controller = MotionControlType::velocity; motor->controller = MotionControlType::velocity;
} }
motor->target = motor_control_inputs.target_velocity; motor->target = motor_control_inputs.target_velocity;
} else { } else {
if (motor->controller != MotionControlType::angle) { if (motor->controller != MotionControlType::angle) {
motor->controller = MotionControlType::angle; motor->controller = MotionControlType::angle;
@ -438,45 +372,45 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
commander->run(); commander->run();
} }
void setup(){ void setup(){
/* bias for vector int */ // Vector table initialization (commented out)
// __set_MSP(*(volatile uint32_t*)0x08008000); // __set_MSP(*(volatile uint32_t*)0x08008000);
// SCB->VTOR = (volatile uint32_t)0x08008000; // SCB->VTOR = (volatile uint32_t)0x08008000;
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.setTx(HARDWARE_SERIAL_TX_PIN); Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.begin(115200); Serial.setTx(HARDWARE_SERIAL_TX_PIN);
Serial.begin(115200);
pinMode(PC11, OUTPUT); pinMode(PC11, OUTPUT);
pinMode(PC10,OUTPUT); pinMode(PC10,OUTPUT);
GPIOC->ODR &= ~GPIO_ODR_OD10; GPIOC->ODR &= ~GPIO_ODR_OD10;
// Setup thermal sensor pin
// pinMode(TH1, INPUT_ANALOG); Can.begin();
Can.begin(); Can.setBaudRate(1000000);
Can.setBaudRate(1000000);
TIM_TypeDef *Instance = TIM2; // Load parameters from flash
HardwareTimer *SendTimer = new HardwareTimer(Instance);
// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
// SendTimer->attachInterrupt(send_data);
// SendTimer->resume();
flash_rec = load_params(); flash_rec = load_params();
for(int i = 0;i < PARAM_COUNT;i++) for(int i = 0;i < PARAM_COUNT;i++)
flash_buf[i] = flash_rec[i]; flash_buf[i] = flash_rec[i];
// Initialize FOC system
setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor); setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
// Default motor configuration
GPIOC->ODR |= GPIO_ODR_OD11; GPIOC->ODR |= GPIO_ODR_OD11;
motor.torque_controller = TorqueControlType::foc_current; motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::torque; motor.controller = MotionControlType::torque;
__enable_irq(); __enable_irq();
} }
void loop() { void loop() {
foc_step(&motor, &command); foc_step(&motor, &command);
CAN_message_t msg; CAN_message_t msg;
GPIOC->ODR ^= GPIO_ODR_OD11; GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED
delay(500); delay(500);
// Process incoming CAN messages
while (Can.read(msg)) { while (Can.read(msg)) {
listen_can(msg); listen_can(msg);
} }
} }