// clang-format off #include "Arduino.h" #include "stm32f446xx.h" #include #include #include #include #include #include #include #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" void SysTick_Handler(void) { HAL_IncTick(); } STM32_CAN Can(CAN2, DEF); /* FLASH Related Definitions */ uint32_t flash_flag; uint8_t flag_can = 0; uint32_t flash_error; FLASH_EraseInitTypeDef pEraseInit; uint32_t SectorError; 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; /* bool for test CAN */ volatile bool CAN_GET = false; volatile float kt = 0.1; // Torque calculation constant static FLASH_RECORD* flash_rec; static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; 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); struct MotorControlInputs { float target_angle = 0.0; float target_velocity = 0.0; bool motor_enabled = false; bool foc_state = false; }; MotorControlInputs motor_control_inputs; void doMotor(char *cmd) { command.motor(&motor, cmd); digitalWrite(PC10, !digitalRead(PC10)); delayMicroseconds(2); } void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, DRV8313Driver *driver, LowsideCurrentSense *current_sense, Commander *commander, CommandCallback callback) { encoder->init(&spi); // Driver configuration driver->pwm_frequency = 20000; driver->voltage_power_supply = 24; driver->voltage_limit = 24; driver->init(); // Current sense initialization current_sense->linkDriver(driver); current_sense->init(); // Motor configuration motor->linkSensor(encoder); motor->linkDriver(driver); motor->linkCurrentSense(current_sense); motor->useMonitoring(Serial); motor->monitor_downsample = 5000; // Default monitoring interval motor->controller = MotionControlType::angle; motor->torque_controller = TorqueControlType::voltage; motor->foc_modulation = FOCModulationType::SpaceVectorPWM; // PID Configuration motor->PID_velocity.P = 0.75; motor->PID_velocity.I = 20; motor->LPF_velocity.Tf = 0.005; motor->P_angle.P = 0.5; motor->LPF_angle.Tf = 0.001; // Motor limits motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm) motor->voltage_limit = 24; motor->current_limit = 0.5; motor->sensor_direction = Direction::CCW; motor->init(); motor->initFOC(); } template void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) { // Create CAN message CAN_message_t msg_l; msg_l.id = id; // msg_l.len = 8; // Protocol-defined message length memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t)); memcpy(&msg_l.buf[1], data, sizeof(T)); // Prepare CRC calculation buffer (ID + data) uint8_t crc_data[CAN_MSG_MAX_LEN]; // Copy message ID memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T)); // Copy all data bytes memcpy(crc_data + 1, msg_l.buf, 6); // Calculate CRC uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN); // Insert CRC into buffer msg_l.buf[6] = crc_value & 0xFF; msg_l.buf[7] = (crc_value >> 8) & 0xFF; // Send message Can.write(msg_l); } void send_velocity() { float current_velocity = motor.shaftVelocity(); if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } float value = flash_rec[vel].value; uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'V',&value); } void send_angle() { float current_angle = motor.shaftAngle(); if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'A',¤t_angle); } void send_motor_enabled() { int motor = 1; if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'M',&motor); } void send_foc_state() { /* Firmware data reading */ if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t value = flash_rec[foc_id].value; uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'F',&value); } void send_id() { /* Firmware data reading */ if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'I',&id); __NOP(); } // void send_motor_torque() { // float i_q = motor.current.q; // Q-axis current (A) // float torque = kt * i_q; // Torque calculation // torque *= 100; // 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 send_pid(uint8_t param_pid){ if (flash_rec == nullptr) { // Null check return; } uint8_t id = flash_rec[addr_id].value; conv_float_to_int.i = flash_rec[param_pid].value; uint32_t data = conv_float_to_int.i; switch(param_pid){ case pid_p: param_pid = REG_MOTOR_POSPID_Kp; break; case pid_i: param_pid = REG_MOTOR_POSPID_Ki; break; case pid_d: param_pid = REG_MOTOR_POSPID_Kd; break; } send_can_with_id_crc(id,param_pid,&data); } void setup_id(uint8_t my_id) { write_param(addr_id,my_id); } void setup_angle(float target_angle) { motor.enable(); // Enable motor if disabled motor.controller = MotionControlType::angle; motor.move(target_angle); } void setup_pid_angle(uint8_t param_pid, float data){ switch (param_pid) { case pid_p: motor.P_angle.P = data; break; case pid_i: motor.P_angle.I = data; break; case pid_d: motor.P_angle.D = data; break; default: break; } write_param(param_pid,data); } void listen_can(const CAN_message_t &msg) { msg_id = msg.id; msg_ch = msg_id & 0xF; // Extract message channel id_x = (msg_id >> 4) & 0x7FF; // Extract device address /* CRC Calculation */ uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8); uint8_t data[10] = {0}; // Message buffer for CRC verification // Copy message ID (2 bytes) memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id)); // Copy message data (excluding CRC bytes) memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2); // Calculate CRC uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2); // Verify CRC match if (calculated_crc != received_crc) { return; // Ignore message on CRC mismatch } flash_rec = load_params(); /* Message Structure: 0x691 69 - Device address 1 - Action code */ if(id_x == flash_rec[addr_id].value){ if(msg_ch == REG_WRITE){ switch(msg.buf[0]) { case REG_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_ANGLE: memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(motor_control_inputs.target_angle)); setup_angle(motor_control_inputs.target_angle); break; case REG_MOTOR_POSPID_Kp: memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.P; write_param(pid_p,conv_float_to_int.i); break; case REG_MOTOR_POSPID_Ki: memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.I; write_param(pid_i,conv_float_to_int.i); break; case REG_MOTOR_POSPID_Kd: memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.D; write_param(pid_d,conv_float_to_int.i); 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; } } 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; 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; 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; } 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(); } void setup(){ // Vector table initialization (commented out) // __set_MSP(*(volatile uintф32_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; // Can.enableMBInterrupts(); Can.begin(); Can.setBaudRate(1000000); // Настройка прерываний CAN CAN2->IER |= CAN_IER_FMPIE0; flash_rec = load_params(); /* For test */ // float value = 3.46; //addr // conv_float_to_int.f = value; // write_param(pid_p,conv_float_to_int.i); //for update address in firmware // // Load parameters from flash // flash_rec = load_params(); // Initialize FOC system setup_foc(&encoder, &motor, &driver, ¤t_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 GPIOC->ODR |= GPIO_ODR_OD11; motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; __enable_irq(); } void loop() { __enable_irq(); foc_step(&motor, &command); CAN_message_t msg; GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED delay(500); // // Process incoming CAN messages while (Can.read(msg)) { CAN_GET = true; } /* If receive data from CAN */ if(CAN_GET) { listen_can(msg); } }