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

Add flag for CAN

Browse source
This commit is contained in:
lulko 2025-05-04 22:53:42 +03:00
parent a2f1c2557a
commit b36103201c
1 changed files with 35 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -12,6 +12,7 @@
#include "wiring_analog.h" #include "wiring_analog.h"
#include "wiring_constants.h" #include "wiring_constants.h"
// clang-format on // clang-format on
#include "reg_cah.h" #include "reg_cah.h"
#include "flash.h" #include "flash.h"
@ -33,6 +34,10 @@ 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;
/* bool for test CAN */
volatile bool CAN_GET = false;
volatile float kt = 0.1; // Torque calculation constant volatile float kt = 0.1; // Torque calculation constant
static FLASH_RECORD* flash_rec; static FLASH_RECORD* flash_rec;
@ -68,9 +73,7 @@ void doMotor(char *cmd) {
delayMicroseconds(2); delayMicroseconds(2);
} }
// Empty interrupt handler to prevent default handling
void CAN2_RX0_IRQHandler() {
}
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
DRV8313Driver *driver, LowsideCurrentSense *current_sense, DRV8313Driver *driver, LowsideCurrentSense *current_sense,
@ -142,12 +145,12 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s
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 check if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc. // Error handling: logging, alerts, etc.
return; return;
} }
uint8_t value = flash_rec[vel].value; uint8_t value = flash_rec[vel].value;
@ -372,9 +375,10 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
commander->run(); commander->run();
} }
void setup(){
void setup(){
// Vector table initialization (commented out) // Vector table initialization (commented out)
// __set_MSP(*(volatile uint32_t*)0x08008000); // __set_MSP(*(volatile uintф32_t*)0x08008000);
// SCB->VTOR = (volatile uint32_t)0x08008000; // SCB->VTOR = (volatile uint32_t)0x08008000;
Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setRx(HARDWARE_SERIAL_RX_PIN);
@ -384,10 +388,11 @@ void setup(){
pinMode(PC11, OUTPUT); pinMode(PC11, OUTPUT);
pinMode(PC10,OUTPUT); pinMode(PC10,OUTPUT);
GPIOC->ODR &= ~GPIO_ODR_OD10; GPIOC->ODR &= ~GPIO_ODR_OD10;
// Can.enableMBInterrupts();
Can.begin(); Can.begin();
Can.setBaudRate(1000000); Can.setBaudRate(1000000);
// Настройка прерываний CAN
CAN2->IER |= CAN_IER_FMPIE0;
// Load parameters from flash // Load parameters from flash
flash_rec = load_params(); flash_rec = load_params();
for(int i = 0;i < PARAM_COUNT;i++) for(int i = 0;i < PARAM_COUNT;i++)
@ -396,21 +401,36 @@ void setup(){
// Initialize FOC system // Initialize FOC system
setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor); setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
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);
// Default motor configuration // 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() {
__enable_irq();
foc_step(&motor, &command); foc_step(&motor, &command);
CAN_message_t msg; CAN_message_t msg;
GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED
delay(500); delay(500);
// Process incoming CAN messages // // Process incoming CAN messages
while (Can.read(msg)) { while (Can.read(msg)) {
CAN_GET = true;
}
/* If receive data from CAN */
if(CAN_GET) {
listen_can(msg); listen_can(msg);
} }
} }