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

120 lines
2.7 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"
2025-05-16 21:06:31 +03:00
#include "config.h"
#include "process_can.h"
2025-04-16 22:51:39 +03:00
void SysTick_Handler(void) {
HAL_IncTick();
}
STM32_CAN Can(CAN2, DEF);
2025-05-16 21:06:31 +03:00
2025-04-16 22:51:39 +03:00
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
2025-05-16 21:06:31 +03:00
FLASH_RECORD* flash_rec;
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);
MotorControlInputs motor_control_inputs;
2025-05-16 21:06:31 +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-15 17:38:59 +03:00
2025-04-16 22:51:39 +03:00
volatile uint32_t ipsr_value = 0;
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();
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
}