1+ /* *
2+ * Comprehensive BLDC motor control example using encoder and the DRV8302 board
3+ *
4+ * Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
5+ * - configure PID controller constants
6+ * - change motion control loops
7+ * - monitor motor variabels
8+ * - set target values
9+ * - check all the configuration values
10+ *
11+ * check the https://docs.simplefoc.com for full list of motor commands
12+ *
13+ */
14+ #include < SimpleFOC.h>
15+
16+ // DRV8302 pins connections
17+ // don't forget to connect the common ground pin
18+ #define INH_A PA8
19+ #define INH_B PA9
20+ #define INH_C PA10
21+
22+ #define EN_GATE PB7
23+ #define M_PWM PB4
24+ #define M_OC PB3
25+ #define OC_ADJ PB6
26+ #define OC_GAIN PB5
27+
28+ #define IOUTA PA0
29+ #define IOUTB PA1
30+ #define IOUTC PA2
31+
32+ // Motor instance
33+ BLDCMotor motor = BLDCMotor(7 );
34+ BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);
35+
36+ // DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V
37+ LowsideCurrentSense cs = LowsideCurrentSense(0 .005f , 12 .22f , IOUTA, IOUTB, IOUTC);
38+
39+ // encoder instance
40+ Encoder encoder = Encoder(PB14, PB15, 2048 );
41+
42+ // Interrupt routine intialisation
43+ // channel A and B callbacks
44+ void doA (){encoder.handleA ();}
45+ void doB (){encoder.handleB ();}
46+
47+
48+ // commander interface
49+ Commander command = Commander(Serial);
50+ void onMotor (char * cmd){ command.motor (&motor, cmd); }
51+
52+ void setup () {
53+
54+ // initialize encoder sensor hardware
55+ encoder.init ();
56+ encoder.enableInterrupts (doA, doB);
57+ // link the motor to the sensor
58+ motor.linkSensor (&encoder);
59+
60+ // DRV8302 specific code
61+ // M_OC - enable overcurrent protection
62+ pinMode (M_OC,OUTPUT);
63+ digitalWrite (M_OC,LOW);
64+ // M_PWM - enable 3pwm mode
65+ pinMode (M_PWM,OUTPUT);
66+ digitalWrite (M_PWM,HIGH);
67+ // OD_ADJ - set the maximum overcurrent limit possible
68+ // Better option would be to use voltage divisor to set exact value
69+ pinMode (OC_ADJ,OUTPUT);
70+ digitalWrite (OC_ADJ,HIGH);
71+ pinMode (OC_GAIN,OUTPUT);
72+ digitalWrite (OC_GAIN,LOW);
73+
74+
75+ // driver config
76+ // power supply voltage [V]
77+ driver.voltage_power_supply = 19 ;
78+ driver.pwm_frequency = 15000 ; // suggested under 18khz
79+ driver.init ();
80+ // link the motor and the driver
81+ motor.linkDriver (&driver);
82+ // link current sense and the driver
83+ cs.linkDriver (&driver);
84+
85+ // align voltage
86+ motor.voltage_sensor_align = 0.5 ;
87+
88+ // control loop type and torque mode
89+ motor.torque_controller = TorqueControlType::voltage;
90+ motor.controller = MotionControlType::torque;
91+ motor.motion_downsample = 0.0 ;
92+
93+ // velocity loop PID
94+ motor.PID_velocity .P = 0.2 ;
95+ motor.PID_velocity .I = 5.0 ;
96+ // Low pass filtering time constant
97+ motor.LPF_velocity .Tf = 0.02 ;
98+ // angle loop PID
99+ motor.P_angle .P = 20.0 ;
100+ // Low pass filtering time constant
101+ motor.LPF_angle .Tf = 0.0 ;
102+ // current q loop PID
103+ motor.PID_current_q .P = 3.0 ;
104+ motor.PID_current_q .I = 100.0 ;
105+ // Low pass filtering time constant
106+ motor.LPF_current_q .Tf = 0.02 ;
107+ // current d loop PID
108+ motor.PID_current_d .P = 3.0 ;
109+ motor.PID_current_d .I = 100.0 ;
110+ // Low pass filtering time constant
111+ motor.LPF_current_d .Tf = 0.02 ;
112+
113+ // Limits
114+ motor.velocity_limit = 100.0 ; // 100 rad/s velocity limit
115+ motor.voltage_limit = 12.0 ; // 12 Volt limit
116+ motor.current_limit = 2.0 ; // 2 Amp current limit
117+
118+
119+ // use monitoring with serial for motor init
120+ // monitoring port
121+ Serial.begin (115200 );
122+ // comment out if not needed
123+ motor.useMonitoring (Serial);
124+ motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q
125+ motor.monitor_downsample = 0 ;
126+
127+ // initialise motor
128+ motor.init ();
129+
130+ cs.init ();
131+ // driver 8302 has inverted gains on all channels
132+ cs.gain_a *=-1 ;
133+ cs.gain_b *=-1 ;
134+ cs.gain_c *=-1 ;
135+ motor.linkCurrentSense (&cs);
136+
137+ // align encoder and start FOC
138+ motor.initFOC ();
139+
140+ // set the inital target value
141+ motor.target = 0 ;
142+
143+ // define the motor id
144+ command.add (' M' " motor"
145+
146+ Serial.println (F (" Full control example: "
147+ Serial.println (F (" Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "
148+ Serial.println (F (" Initial motion control loop is voltage loop."
149+ Serial.println (F (" Initial target voltage 2V."
150+
151+ _delay (1000 );
152+ }
153+
154+
155+ void loop () {
156+ // iterative setting FOC phase voltage
157+ motor.loopFOC ();
158+
159+ // iterative function setting the outter loop target
160+ motor.move ();
161+
162+ // monitoring the state variables
163+ motor.monitor ();
164+
165+ // user communication
166+ command.run ();
167+ }
0 commit comments