B g431 b esc1

platformio.ini

@@ -19,3 +19,5 @@ lib_deps =
         SPI @ 1.1.0
         Wire @ 1.0.0
         Simple FOC @ 2.3.2
+
+build_flags = -D HAL_ADC_MODULE_ONLY -D HAL_OPAMP_MODULE_ENABLED

src/main.cpp

@@ -1,88 +1,117 @@
 /**
- *  Hall sensor example code
- *
- * This is a code intended to test the hall sensors connections
- * and to demonstrate the hall sensor setup.
+ * B-G431B-ESC1 position motion control example with hall sensor
  *
  */
 
 #include <SimpleFOC.h>
 
 #define POLE_PAIRS 7
 
-// Motor instance
+// BLDC motor & driver instance
 BLDCMotor motor = BLDCMotor(POLE_PAIRS);
 BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
+LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
 
-// Hall sensor instance
-// HallSensor(int hallA, int hallB , int hallC, int pp)
-//  - hallA, hallB, hallC    - HallSensor A, B and C pins
-//  - pp                     - pole pairs
+// hall sensor instance
 HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3, POLE_PAIRS);
 
 // Interrupt routine initialisation
-// channel A, B and C callbacks
 void doA(){sensor.handleA();}
 void doB(){sensor.handleB();}
 void doC(){sensor.handleC();}
 
-//target variable
-float target_velocity = 0;
-
+// angle set point variable
+float target_angle = 0;
 // instantiate the commander
 Commander command = Commander(Serial);
-void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
-void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
+void doTarget(char* cmd) {
+        float angle_degrees;
+        command.scalar(&angle_degrees, cmd);
+        target_angle = angle_degrees * (PI / 180.0);
+}
 
 void setup() {
         Serial.begin(115200);
-        SimpleFOCDebug::enable(&Serial);
         delay(2000);
 
-        // initialise hall sensor hardware
+        // initialize hall sensor hardware
         sensor.init();
-        // hardware interrupt enable
         sensor.enableInterrupts(doA, doB, doC);
         // link the motor to the sensor
         motor.linkSensor(&sensor);
 
         // driver config
         // power supply voltage [V]
         driver.voltage_power_supply = 12;
-        // limit the maximal dc voltage the driver can set
-        // as a protection measure for the low-resistance motors
-        // this value is fixed on startup
-        driver.voltage_limit = 6;
         driver.init();
         // link the motor and the driver
         motor.linkDriver(&driver);
-
-        // limiting motor movements
-        // limit the voltage to be set to the motor
-        // start very low for high resistance motors
-        // current = voltage / resistance, so try to be well under 1Amp
-        motor.voltage_limit = 3;   // [V]
-        motor.velocity_limit = 10; // [rad/s]
-
-        // open loop control config
-        motor.controller = MotionControlType::velocity_openloop;
-
-        // init motor hardware
+        // link current sense and the driver
+        currentSense.linkDriver(&driver);
+
+        // current sensing
+        currentSense.init();
+        // no need for aligning
+        currentSense.skip_align = true;
+        motor.linkCurrentSense(&currentSense);
+
+        // aligning voltage [V]
+        motor.voltage_sensor_align = 3;
+        // index search velocity [rad/s]
+        motor.velocity_index_search = 3;
+
+        // set motion control loop to be used
+        motor.controller = MotionControlType::angle;
+
+        // velocity PI controller parameters
+        motor.PID_velocity.P = 0.2f;
+        motor.PID_velocity.I = 2;
+        motor.PID_velocity.D = 0;
+        // default voltage_power_supply
+        motor.voltage_limit = 6;
+        // jerk control using voltage voltage ramp
+        // default value is 300 volts per sec  ~ 0.3V per millisecond
+        motor.PID_velocity.output_ramp = 1000;
+
+        // velocity low pass filtering time constant
+        motor.LPF_velocity.Tf = 0.01f;
+
+        // angle P controller
+        motor.P_angle.P = 20;
+        // maximal velocity of the position control
+        motor.velocity_limit = 4;
+
+        // use monitoring with serial
+        motor.useMonitoring(Serial);
+
+        // initialize motor
         motor.init();
+        // align sensor and start FOC
+        motor.initFOC();
 
         // add target command T
-        command.add('T', doTarget, "target velocity");
-        command.add('L', doLimit, "voltage limit");
+        command.add('T', doTarget, "target angle [deg]");
 
-        Serial.println("Motor ready!");
-        Serial.println("Set target velocity [rad/s]");
+        Serial.println(F("Motor ready."));
+        Serial.println(F("Set the target angle using serial terminal:"));
 }
 
 void loop() {
-        // open loop velocity movement
-        // using motor.voltage_limit and motor.velocity_limit
-        // to turn the motor "backwards", just set a negative target_velocity
-        motor.move(target_velocity);
+        // main FOC algorithm function
+        // the faster you run this function the better
+        // Arduino UNO loop  ~1kHz
+        // Bluepill loop ~10kHz
+        motor.loopFOC();
+
+        // Motion control function
+        // velocity, position or voltage (defined in motor.controller)
+        // this function can be run at much lower frequency than loopFOC() function
+        // You can also use motor.move() and set the motor.target in the code
+        motor.move(target_angle);
+
+        // function intended to be used with serial plotter to monitor motor variables
+        // significantly slowing the execution down!!!!
+        // motor.monitor();
 
         // user communication
         command.run();