|
| 1 | +/** |
| 2 | + * Silabs MG24 6PWM closed loop velocity control example with HALL sensor based rotor position |
| 3 | + * |
| 4 | + * HARDWARE CONFIGURATION: |
| 5 | + * CPU Board: Arduino Nano Matter |
| 6 | + * Motor Driver Board: BOOSTXL-DRV8305EVM |
| 7 | + * BLDC Motor: Newark DF45M024053-A2 |
| 8 | + */ |
| 9 | + |
| 10 | +#include <SimpleFOC.h> |
| 11 | + |
| 12 | +#define HALL_SENSOR_IRQ 1 |
| 13 | +#define ENABLE_MONITOR 0 |
| 14 | + |
| 15 | +static bool allow_run = false; |
| 16 | + |
| 17 | +// BLDC motor instance |
| 18 | +BLDCMotor *motor; |
| 19 | + |
| 20 | +// BLDC driver instance |
| 21 | +BLDCDriver6PWM *driver; |
| 22 | + |
| 23 | +// Commander instance |
| 24 | +Commander *command; |
| 25 | + |
| 26 | +// Hall sensor instance |
| 27 | +HallSensor *sensor; |
| 28 | + |
| 29 | +// Interrupt routine initialization |
| 30 | +// channel A and B callbacks |
| 31 | +void doA() { sensor->handleA(); } |
| 32 | +void doB() { sensor->handleB(); } |
| 33 | +void doC() { sensor->handleC(); } |
| 34 | + |
| 35 | +void doMotor(char* cmd) { |
| 36 | + if (!command) return; |
| 37 | + command->motor(motor, cmd); |
| 38 | +} |
| 39 | + |
| 40 | +void setup() { |
| 41 | + // use monitoring with serial |
| 42 | + Serial.begin(115200); |
| 43 | + // enable more verbose output for debugging |
| 44 | + // comment out if not needed |
| 45 | + SimpleFOCDebug::enable(&Serial); |
| 46 | + |
| 47 | + // Commander |
| 48 | + command = new Commander(Serial); |
| 49 | + if (!command) return; |
| 50 | + |
| 51 | + // Driver |
| 52 | + driver = new BLDCDriver6PWM(6, 7, 8, 9, 10, 11, 12); |
| 53 | + if (!driver) return; |
| 54 | + |
| 55 | + // Driver config |
| 56 | + // power supply voltage [V] |
| 57 | + driver->voltage_power_supply = 24; |
| 58 | + // pwm frequency to be used [Hz] |
| 59 | + driver->pwm_frequency = 20000; // 20 kHz |
| 60 | + // Max DC voltage allowed - default voltage_power_supply |
| 61 | + driver->voltage_limit = 12; |
| 62 | + // dead zone percentage of the duty cycle - default 0.02 - 2% |
| 63 | + // Can set value to 0 because the DRV8305 will provide the |
| 64 | + // required dead-time. |
| 65 | + driver->dead_zone = 0; |
| 66 | + |
| 67 | + // init driver |
| 68 | + if (!driver->init()) { |
| 69 | + Serial.println("Driver init failed!"); |
| 70 | + return; |
| 71 | + } |
| 72 | + driver->enable(); |
| 73 | + |
| 74 | + // HallSensor(int encA, int encB, int encC, int pp) |
| 75 | + // - encA, encB, encC - HallSensor A, B and C pins |
| 76 | + // - pp - pole pairs |
| 77 | + sensor = new HallSensor(5, 4, 13, 8); |
| 78 | + if (!sensor) return; |
| 79 | + |
| 80 | + // initialize sensor sensor hardware |
| 81 | + sensor->init(); |
| 82 | + |
| 83 | +#if HALL_SENSOR_IRQ |
| 84 | + sensor->enableInterrupts(doA, doB, doC); |
| 85 | +#else |
| 86 | + // Note: There is a bug when initializing HallSensor in heap, attribute |
| 87 | + // `use_interrupt` gets value not `false` even `enableInterrupts` has not been |
| 88 | + // called. So we initialize this attribute value `false` after creating a |
| 89 | + // `HallSensor` instance. |
| 90 | + sensor->use_interrupt = false; |
| 91 | +#endif |
| 92 | + |
| 93 | + // Motor |
| 94 | + motor = new BLDCMotor(8); |
| 95 | + if (!motor) return; |
| 96 | + |
| 97 | + // link the motor and the driver |
| 98 | + motor->linkDriver(driver); |
| 99 | + |
| 100 | + // link the motor to the sensor |
| 101 | + motor->linkSensor(sensor); |
| 102 | + |
| 103 | + // Set below the motor's max 5600 RPM limit = 586 rad/s |
| 104 | + motor->velocity_limit = 530.0f; |
| 105 | + |
| 106 | + // set motion control loop to be used |
| 107 | + motor->controller = MotionControlType::velocity; |
| 108 | + |
| 109 | + // choose FOC modulation (optional) - SinePWM or SpaceVectorPWM |
| 110 | + motor->foc_modulation = FOCModulationType::SpaceVectorPWM; |
| 111 | + |
| 112 | + // controller configuration |
| 113 | + // velocity PI controller parameters |
| 114 | + motor->PID_velocity.P = 0.05f; |
| 115 | + motor->PID_velocity.I = 1; |
| 116 | + |
| 117 | + // velocity low pass filtering time constant |
| 118 | + motor->LPF_velocity.Tf = 0.01f; |
| 119 | + |
| 120 | +#if ENABLE_MONITOR |
| 121 | + motor->useMonitoring(Serial); |
| 122 | + motor->monitor_variables = _MON_TARGET | _MON_VEL; |
| 123 | +#endif |
| 124 | + |
| 125 | + // initialize motor |
| 126 | + if (!motor->init()) { |
| 127 | + Serial.println("Motor init failed!"); |
| 128 | + return; |
| 129 | + } |
| 130 | + |
| 131 | + // align sensor and start FOC |
| 132 | + if (!motor->initFOC()) { |
| 133 | + Serial.println("FOC init failed!"); |
| 134 | + return; |
| 135 | + } |
| 136 | + |
| 137 | + // add target command M |
| 138 | + command->add('M', doMotor, "motor"); |
| 139 | + |
| 140 | + Serial.println("Motor ready!"); |
| 141 | + Serial.println("Set target velocity [rad/s]"); |
| 142 | + |
| 143 | + allow_run = true; |
| 144 | + _delay(1000); |
| 145 | +} |
| 146 | + |
| 147 | +void loop() { |
| 148 | + if (!allow_run) return; |
| 149 | + |
| 150 | + // main FOC algorithm function |
| 151 | + // the faster you run this function the better |
| 152 | + motor->loopFOC(); |
| 153 | + |
| 154 | + // Motion control function |
| 155 | + // velocity, position or voltage (defined in motor.controller) |
| 156 | + // this function can be run at much lower frequency than loopFOC() function |
| 157 | + // You can also use motor.move() and set the motor.target in the code |
| 158 | + motor->move(); |
| 159 | + |
| 160 | +#if ENABLE_MONITOR |
| 161 | + // function intended to be used with serial plotter to monitor motor variables |
| 162 | + // significantly slowing the execution down!!!! |
| 163 | + motor->monitor(); |
| 164 | +#endif |
| 165 | + |
| 166 | + // user communication |
| 167 | + command->run(); |
| 168 | +} |
0 commit comments