120 lines
2.7 KiB
C++
120 lines
2.7 KiB
C++
#include "Arduino.h"
|
|
#include "stm32f446xx.h"
|
|
#include <SimpleFOC.h>
|
|
#include <STM32_CAN.h>
|
|
#include <AS5045.h>
|
|
#include <DRV8313.h>
|
|
#include <cstring>
|
|
#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
|
|
|
|
#include "reg_cah.h"
|
|
#include "flash.h"
|
|
#include "config.h"
|
|
#include "process_can.h"
|
|
|
|
|
|
void SysTick_Handler(void) {
|
|
HAL_IncTick();
|
|
}
|
|
|
|
STM32_CAN Can(CAN2, DEF);
|
|
/* for FLASH */
|
|
uint32_t flash_flag;
|
|
uint8_t flag_can = 0;
|
|
uint32_t flash_error;
|
|
FLASH_EraseInitTypeDef pEraseInit;
|
|
uint32_t SectorError;
|
|
|
|
|
|
/* bool for test CAN */
|
|
volatile bool CAN_GET = false;
|
|
|
|
volatile float kt = 0.1; // Torque calculation constant
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
void setup(){
|
|
SCB->VTOR = (volatile uint32_t)0x08008004;
|
|
|
|
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;
|
|
// Can.enableMBInterrupts();
|
|
Can.begin();
|
|
Can.setBaudRate(1000000);
|
|
// Настройка прерываний CAN
|
|
CAN2->IER |= CAN_IER_FMPIE0;
|
|
flash_rec = load_params(); //for update write_ptr
|
|
if(flash_rec[firmw].value == FIRMWARE_FLAG) NVIC_SystemReset(); //if in flash go to the bootloader
|
|
|
|
// Initialize FOC system
|
|
setup_foc(&encoder, &motor, &driver, ¤t_sense,flash_rec);
|
|
|
|
CAN2->IER |= CAN_IER_FMPIE0 | // Сообщение в FIFO0
|
|
CAN_IER_FFIE0 | // FIFO0 full
|
|
CAN_IER_FOVIE0; // FIFO0 overflow
|
|
|
|
|
|
// Default motor configuration
|
|
GPIOC->ODR |= GPIO_ODR_OD11; //set LED
|
|
motor.torque_controller = TorqueControlType::foc_current;
|
|
motor.controller = MotionControlType::torque;
|
|
__enable_irq();
|
|
|
|
}
|
|
|
|
|
|
|
|
void loop() {
|
|
__enable_irq();
|
|
send_angle();
|
|
send_velocity();
|
|
foc_step(&motor);
|
|
CAN_message_t msg;
|
|
|
|
|
|
|
|
// Process incoming CAN messages
|
|
while (Can.read(msg)) {
|
|
listen_can(msg);
|
|
CAN_GET = true;
|
|
}
|
|
/* If receive data from CAN */
|
|
if(CAN_GET) {
|
|
|
|
CAN_GET = false;
|
|
}
|
|
}
|
|
|