1+
2+ #include < Arduino.h>
3+ #include < SimpleFOC.h>
4+ #include " current_sense/hardware_specific/stm32/stm32_mcu.h"
5+
6+
7+ // BLDC motor & driver instance
8+ BLDCMotor motor = BLDCMotor(7 );
9+ BLDCDriver3PWM driver = BLDCDriver3PWM(D6, D10, D5, D8);
10+
11+ // encoder instance
12+ MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, D4);
13+
14+ // inline current sensor instance
15+ // INA240A1 (gain 20V/V) and 5mOhm shunt resistor
16+ LowsideCurrentSense current_sense = LowsideCurrentSense(0.005 , 20 .0f , A0, _NC, A3);
17+
18+ // commander communication instance
19+ Commander command = Commander(Serial);
20+ // void doMotion(char* cmd){ command.motion(&motor, cmd); }
21+ void doMotor (char * cmd){ command.motor (&motor, cmd); }
22+
23+
24+ // custom PID controller instance for the custom control method
25+ // P controller with gain of 1.0f, no integral or derivative gain
26+ PIDController custom_PID = PIDController(1 .0f , 0 , 0 );
27+ // custom motion control method
28+ float positionPControl (FOCMotor* motor, float target){
29+ // simple proportional position control
30+ float error = target - motor->shaft_angle ;
31+ // set the PID output limit to the motor current limit
32+ custom_PID.limit = motor->current_limit ;
33+ return custom_PID (error); // return current command based on the error
34+ }
35+
36+ // optional add the PID to command to be able to tune it in runtime
37+ void doPID (char * cmd){ command.pid (&custom_PID, cmd); }
38+
39+ void setup () {
40+ // use monitoring with serial
41+ Serial.begin (115200 );
42+ // enable more verbose output for debugging
43+ // comment out if not needed
44+ SimpleFOCDebug::enable (&Serial);
45+
46+ // initialize sensor hardware
47+ sensor.init ();
48+ motor.linkSensor (&sensor);
49+
50+ // driver config
51+ // power supply voltage [V]
52+ driver.voltage_power_supply = 20 ;
53+ driver.init ();
54+ // link driver
55+ motor.linkDriver (&driver);
56+ // link current sense and the driver
57+ current_sense.linkDriver (&driver);
58+
59+ // set the custom control method
60+ motor.linkCustomMotionControl (positionPControl);
61+ // set control loop type to be used
62+ motor.controller = MotionControlType::custom;
63+ // set the torque control type to voltage control (default is voltage control)
64+ motor.torque_controller = TorqueControlType::foc_current;
65+
66+ // comment out if not needed
67+ motor.useMonitoring (Serial);
68+ motor.monitor_downsample = 0 ; // disable intially
69+ motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
70+
71+ // subscribe motor to the commander
72+ // command.add('T', doMotion, "motion control"); // a bit less resouce intensive
73+ command.add (' M' " motor"
74+ command.add (' C' " custom PID"
75+
76+ // current sense init and linking
77+ current_sense.init ();
78+ motor.linkCurrentSense (¤t_sense);
79+
80+ // initialise motor
81+ motor.init ();
82+ // align encoder and start FOC
83+ motor.initFOC ();
84+
85+ _delay (1000 );
86+ }
87+
88+ void loop () {
89+ // iterative setting FOC phase voltage
90+ motor.loopFOC ();
91+
92+ // iterative function setting the outter loop target
93+ motor.move ();
94+
95+ // // motor monitoring
96+ motor.monitor ();
97+
98+ // user communication
99+ command.run ();
100+ }
0 commit comments