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 (&SPI );
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 as the position P control
60+ motor.setCustomMotionControlMethod (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+ // velocity low pass filtering time constant
67+ motor.LPF_velocity .Tf = 0 .01f ;
68+
69+ // angle loop controller
70+ motor.P_angle .P = 20 ;
71+ // angle loop velocity limit
72+ motor.velocity_limit = 20 ;
73+ motor.voltage_sensor_align = 1 .0f ;
74+
75+ // comment out if not needed
76+ motor.useMonitoring (Serial);
77+ motor.monitor_downsample = 0 ; // disable intially
78+ motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
79+
80+ // current sense init and linking
81+ current_sense.init ();
82+ motor.linkCurrentSense (¤t_sense);
83+
84+ // initialise motor
85+ motor.init ();
86+ // align encoder and start FOC
87+ motor.initFOC ();
88+
89+ // subscribe motor to the commander
90+ // command.add('T', doMotion, "motion control"); // a bit less resouce intensive
91+ command.add (' M' " motor"
92+ command.add (' C' " custom PID"
93+
94+ _delay (1000 );
95+ }
96+
97+ void loop () {
98+ // iterative setting FOC phase voltage
99+ motor.loopFOC ();
100+
101+ // iterative function setting the outter loop target
102+ motor.move ();
103+
104+ // // motor monitoring
105+ motor.monitor ();
106+
107+ // user communication
108+ command.run ();
109+ }
0 commit comments