diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index d446a0d..05567b7 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -4,7 +4,12 @@ #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 STM32_CAN Can(CAN2, DEF); @@ -13,109 +18,177 @@ static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; SPIClass spi; -MagneticSensorAS5045 sensor(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK); +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.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2, - CURRENT_SENSOR_3); +LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1, + CURRENT_SENSOR_2, CURRENT_SENSOR_3); Commander command(Serial); +struct MotorControlInputs { + float target_angle; + float target_velocity; + bool motor_enabled; +}; + +MotorControlInputs motor_control_inputs; + void doMotor(char *cmd) { - command.motor(&motor, cmd); - digitalWrite(PC10, !digitalRead(PC10)); - delayMicroseconds(2); + command.motor(&motor, cmd); + // digitalWrite(PC10, !digitalRead(PC10)); + // delayMicroseconds(2); } -void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver, - LowsideCurrentSense *current_sense, Commander *commander, - CommandCallback callback) { - sensor->init(&spi); - motor->linkSensor(sensor); - driver->pwm_frequency = 20000; - driver->voltage_power_supply = 20; - driver->voltage_limit = 40; - driver->init(); - motor->linkDriver(driver); - current_sense->linkDriver(driver); - current_sense->init(); - motor->linkCurrentSense(current_sense); - SimpleFOCDebug::enable(nullptr); - commander->add('M', callback, "motor"); - motor->useMonitoring(Serial); - motor->monitor_start_char = 'M'; - motor->monitor_end_char = 'M'; - commander->verbose = VerboseMode::machine_readable; - motor->controller = MotionControlType::angle; - motor->torque_controller = TorqueControlType::voltage; - motor->foc_modulation = FOCModulationType::SinePWM; - motor->PID_velocity.P = 0.3; - motor->PID_velocity.I = 15; - motor->PID_velocity.D = 0.001; - motor->PID_velocity.output_ramp = 1000; - motor->LPF_velocity.Tf = 0.005; - motor->LPF_angle.Tf = 0.005; - motor->P_angle.P = 15; - motor->velocity_limit = 40; - motor->voltage_limit = 40; - motor->sensor_direction = Direction::CCW; - current_sense->skip_align = true; - motor->init(); - motor->initFOC(); +void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, + DRV8313Driver *driver, LowsideCurrentSense *current_sense, + Commander *commander, CommandCallback callback) { + encoder->init(&spi); + motor->linkSensor(encoder); + driver->pwm_frequency = 20000; + driver->voltage_power_supply = 20; + driver->voltage_limit = 40; + driver->init(); + motor->linkDriver(driver); + current_sense->linkDriver(driver); + current_sense->init(); + motor->linkCurrentSense(current_sense); + SimpleFOCDebug::enable(nullptr); + commander->add('M', callback, "motor"); + motor->useMonitoring(Serial); + motor->monitor_start_char = 'M'; + motor->monitor_end_char = 'M'; + // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; + motor->monitor_downsample = 10; // default 10 + commander->verbose = VerboseMode::user_friendly; + motor->controller = MotionControlType::angle; + motor->torque_controller = TorqueControlType::voltage; + motor->foc_modulation = FOCModulationType::SinePWM; + motor->PID_velocity.P = 0.3; + motor->PID_velocity.I = 15; + motor->PID_velocity.D = 0.001; + motor->PID_velocity.output_ramp = 1000; + motor->LPF_velocity.Tf = 0.005; + motor->LPF_angle.Tf = 0.005; + motor->P_angle.P = 15; + motor->velocity_limit = 40; + motor->voltage_limit = 40; + motor->current_limit = 0.5; + motor->sensor_direction = Direction::CCW; + current_sense->skip_align = true; + motor->init(); + motor->initFOC(); } +void send_velocity() { + float current_velocity = motor.shaftVelocity(); + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'V'; + CAN_TX_msg.len = 5; + memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity)); + Can.write(CAN_TX_msg); +} + +void send_angle() { + float current_angle = motor.shaftAngle(); + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'A'; + CAN_TX_msg.len = 5; + memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); + Can.write(CAN_TX_msg); +} + +void send_motor_enabled() { + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'E'; + memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled, + sizeof(motor_control_inputs.motor_enabled)); + Can.write(CAN_TX_msg); +} void send_data() { - // TODO(vanyabeat) add can functionability for ROS packets - CAN_TX_msg.id = 1; - CAN_TX_msg.buf[0] = 'A'; - CAN_TX_msg.len = 5; - Can.write(CAN_TX_msg); - digitalWrite(PC11, !digitalRead(PC11)); + // TODO(vanyabeat) add can functionability for ROS packets + send_velocity(); + send_angle(); + send_motor_enabled(); + // read_temperature(); + digitalWrite(PC11, !digitalRead(PC11)); } -void read_can_message() { - float angle; - memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle)); - motor.target = angle; - digitalWrite(PC10, !digitalRead(PC10)); +void read_can_step() { + char flag = CAN_inMsg.buf[0]; + if (flag == 'V') { + memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.target_velocity)); + } else if (flag == 'A') { + memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.target_angle)); + } else if (flag == 'E') { + bool enable_flag = CAN_inMsg.buf[1]; + if (enable_flag == 1) { + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.motor_enabled)); + motor.enable(); + } else if (enable_flag == 0) { + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.motor_enabled)); + motor.disable(); + } + } + // motor.target = angle; + digitalWrite(PC10, !digitalRead(PC10)); } -void run_foc(BLDCMotor *motor, Commander *commander) { - motor->loopFOC(); - motor->move(); - motor->monitor(); - commander->run(); +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() { - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - // setup led pin - pinMode(PC11, OUTPUT); - pinMode(PC10, OUTPUT); - Can.begin(); - Can.setBaudRate(1000000); - TIM_TypeDef *Instance = TIM2; - HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz - SendTimer->attachInterrupt(send_data); - SendTimer->resume(); - setup_foc(&sensor, &motor, &driver, ¤t_sense, &command, doMotor); - _delay(1000); + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); + Serial.begin(115200); + // setup led pin + pinMode(PC11, OUTPUT); + pinMode(PC10, OUTPUT); + // Setup thermal sensor pin + // pinMode(TH1, INPUT_ANALOG); + Can.begin(); + Can.setBaudRate(1000000); + TIM_TypeDef *Instance = TIM2; + HardwareTimer *SendTimer = new HardwareTimer(Instance); + SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz + SendTimer->attachInterrupt(send_data); + SendTimer->resume(); + setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); + _delay(1000); } -float current_angle = 0; +float current_angle = 0.0; void loop() { - run_foc(&motor, &command); - current_angle = sensor.getAngle(); - memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); - while (Can.read(CAN_inMsg)) { - read_can_message(); - } + foc_step(&motor, &command); + // current_angle = encoder.getAngle(); + // memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); + while (Can.read(CAN_inMsg)) { + read_can_step(); + } }