Skip to content

Commit 620ae48

Browse files
askuricaskuric
committed
Restructuring of the code, added Enable pin
1 parent 0c5d253 commit 620ae48

File tree

4 files changed

+470
-0
lines changed

4 files changed

+470
-0
lines changed

‎MotorFOCExample/.vscode/c_cpp_properties.json

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
{
2+
"configurations": [
3+
{
4+
"name": "Win32",
5+
"includePath": [
6+
"${workspaceFolder}/**"
7+
],
8+
"defines": [
9+
"_DEBUG",
10+
"UNICODE",
11+
"_UNICODE"
12+
],
13+
"windowsSdkVersion": "10.0.18362.0",
14+
"compilerPath": "C:/Program Files (x86)/Microsoft Visual Studio/2019/BuildTools/VC/Tools/MSVC/14.24.28314/bin/Hostx64/x64/cl.exe",
15+
"cStandard": "c11",
16+
"cppStandard": "c++17",
17+
"intelliSenseMode": "msvc-x64"
18+
}
19+
],
20+
"version": 4
21+
}

‎MotorFOCExample/BLDCmotor.cpp

Lines changed: 238 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,238 @@
1+
#include "BLDCMotor.h"
2+
3+
/*
4+
BLDCMotorint(int phA,int phB,int phC, long* counter, int encA, int encB , int pp, int cpr)
5+
- phA, phB, phC - motor A,B,C phase pwm pins
6+
- *counter - encoder counter variable
7+
- encA, encB - encoder A and B pins
8+
- pp - pole pair number
9+
- cpr - counts per rotation number (cpm=ppm*4)
10+
- enable pin - (optional input)
11+
*/
12+
BLDCMotor::BLDCMotor(int phA,int phB,int phC, long* counter, int encA, int encB , int pp, int cpr)
13+
{
14+
// set enable pin to zero
15+
BLDCMotor(phA, phB, phC, counter, encA, encB, pp, cpr, 0);
16+
}
17+
BLDCMotor::BLDCMotor(int phA,int phB,int phC, long* counter, int encA, int encB , int pp, int cpr, int en)
18+
{
19+
// Pin intialization
20+
_phA = phA;
21+
_phB = phB;
22+
_phC = phC;
23+
24+
// enable pin
25+
_en = en;
26+
27+
// encoder pins
28+
_encA = encA;
29+
_encB = encB;
30+
31+
// Encoder value
32+
_encoderPosition = counter;
33+
_cpr = cpr;
34+
_pp = pp;
35+
36+
}
37+
/*
38+
initialization function
39+
*/
40+
void BLDCMotor::init(){
41+
// enable motor
42+
enableMotor();
43+
// encoder alignment
44+
delay(500);
45+
alignEncoder();
46+
delay(500);
47+
}
48+
49+
/*
50+
disable motor
51+
*/
52+
void BLDCMotor::disableMotor()
53+
{
54+
// disable the driver - if enable pin available
55+
if(_en) digitalWrite(_en, LOW);
56+
// set zero to PWM
57+
setPwm(_phA,0);
58+
setPwm(_phB,0);
59+
setPwm(_phC,0);
60+
}
61+
/*
62+
disable motor
63+
*/
64+
void BLDCMotor::enableMotor()
65+
{
66+
// set zero to PWM
67+
setPwm(_phA,0);
68+
setPwm(_phB,0);
69+
setPwm(_phC,0);
70+
// enable the driver - if enable pin available
71+
if(_en) digitalWrite(_en, HIGH);
72+
}
73+
74+
75+
76+
77+
/*
78+
Encoder alignment to electrical 0 angle
79+
*/
80+
void BLDCMotor::alignEncoder(){
81+
setPhaseVoltage(10,M_PI/2);
82+
delay(500);
83+
*_encoderPosition = 0;
84+
disableMotor();
85+
}
86+
87+
/**
88+
State calculation methods
89+
*/
90+
/*
91+
Shaft angle calculation
92+
*/
93+
float BLDCMotor::ShaftAngle(){
94+
return (*_encoderPosition)/((float)_cpr)*(2.0*M_PI);
95+
}
96+
/*
97+
Shaft velocity calculation
98+
*/
99+
float BLDCMotor::ShaftVelocity(float shaftAngle){
100+
static float dt;
101+
static float shaftAngle_1;
102+
103+
dt = (micros() - dt)*1e-6;
104+
float d_angle = -(shaftAngle - shaftAngle_1)/(dt);
105+
dt = micros();
106+
shaftAngle_1 = shaftAngle;
107+
return d_angle;
108+
}
109+
/*
110+
Electrical angle calculation
111+
*/
112+
float BLDCMotor::ElectricAngle(float shaftAngle){
113+
return normalizeAngle(shaftAngle*_pp);
114+
}
115+
/*
116+
Update motor angles and velocities
117+
*/
118+
void BLDCMotor::updateStates(){
119+
_shaftAngle = ShaftAngle();
120+
_angleElec = ElectricAngle(_shaftAngle);
121+
_shaftVel = ShaftVelocity(_shaftAngle);
122+
}
123+
124+
125+
/**
126+
FOC methods
127+
*/
128+
/*
129+
Method using FOC to set Uq to the motor at the optimal angle
130+
*/
131+
void BLDCMotor::setVoltage(float Uq){
132+
updateStates();
133+
setPhaseVoltage(Uq, _angleElec);
134+
}
135+
void BLDCMotor::setPhaseVoltage(double Uq, double angle_el){
136+
137+
// Park transform
138+
Ualpha = Uq*cos(angle_el);
139+
Ubeta = Uq*sin(angle_el);
140+
// negative Uq compensation
141+
float angle = Uq > 0 ? angle_el : normalizeAngle( angle_el + M_PI );
142+
143+
// determine the segment I, II, III
144+
if((angle >= 0)&&(angle <= _120_D2R)){
145+
// section I
146+
Ua = Ualpha + _1_SQRT3*abs(Ubeta);
147+
Ub = _2_SQRT3*abs(Ubeta);
148+
Uc = 0;
149+
150+
}else if((angle > _120_D2R)&&(angle <= (2*_120_D2R))){
151+
// section III
152+
Ua = 0;
153+
Ub = _1_SQRT3*Ubeta + abs(Ualpha);
154+
Uc = -_1_SQRT3*Ubeta + abs(Ualpha);
155+
156+
}else if((angle > (2*_120_D2R))&&(angle <= (3*_120_D2R))){
157+
// section II
158+
Ua = Ualpha + _1_SQRT3*abs(Ubeta);
159+
Ub = 0;
160+
Uc = _2_SQRT3*abs(Ubeta);
161+
}
162+
163+
// set phase voltages
164+
setPwm(_phA,Ua);
165+
setPwm(_phB,Ub);
166+
setPwm(_phC,Uc);
167+
}
168+
/*
169+
Set voltage to the pwm pin
170+
*/
171+
void BLDCMotor::setPwm(int pinPwm, float U){
172+
int U_pwm = U <= U_MAX ? 255.0*U/U_MAX : 255;
173+
analogWrite(pinPwm, U_pwm);
174+
}
175+
176+
/**
177+
Utility funcitons
178+
*/
179+
/*
180+
normalizing radian angle to [0,2PI]
181+
*/
182+
double BLDCMotor::normalizeAngle(double angle)
183+
{
184+
double a = fmod(angle, 2 * M_PI);
185+
return a >= 0 ? a : (a + 2 * M_PI);
186+
}
187+
/*
188+
Reference low pass filter
189+
*/
190+
float BLDCMotor::filterLP(float u){
191+
static float dt,yk_1;
192+
float M_Tau = 0.01;
193+
dt = (micros() - dt)*1e-6;
194+
float y_k = dt/(M_Tau+dt)*u +(1-dt/(M_Tau+dt))*yk_1;
195+
dt = micros();
196+
yk_1 = y_k;
197+
return y_k;
198+
}
199+
200+
201+
202+
203+
/**
204+
Motor control functions
205+
*/
206+
float BLDCMotor::velocityPI(float ek){
207+
static float ek_1, uk_1;
208+
static float dT;
209+
dT = (micros() - dT)*1e-6;
210+
211+
float uk = uk_1 + M_Kr*(dT/(2*M_Ti)+1)*ek + M_Kr*(dT/(2*M_Ti)-1)*ek_1;
212+
if(abs(uk) > U_MAX) uk = uk > 0 ? U_MAX : -U_MAX;
213+
214+
uk_1 = uk;
215+
ek_1 = ek;
216+
dT = micros();
217+
return uk;
218+
}
219+
220+
float BLDCMotor::positionP(float ek){
221+
float uk = M_P * ek;
222+
if(abs(uk) > W_MAX) uk = uk > 0 ? W_MAX : -W_MAX;
223+
return uk;
224+
}
225+
226+
227+
void BLDCMotor::setVelocity(float vel){
228+
updateStates();
229+
setVoltage(velocityPI(vel-_shaftVel));
230+
}
231+
232+
void BLDCMotor::setPosition(float pos){
233+
updateStates();
234+
setVoltage(velocityPI(-positionP( filterLP(pos) - _shaftAngle ) - _shaftVel));
235+
}
236+
237+
238+

‎MotorFOCExample/BLDCmotor.h

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
2+
#ifndef BLDCMotor_h
3+
#define BLDCMotor_h
4+
5+
#include "Arduino.h"
6+
7+
#define _2_SQRT3 1.15470053838
8+
#define _1_SQRT3 0.57735026919
9+
#define _120_D2R 2.09439510239
10+
11+
#define U_MAX 12
12+
#define W_MAX 20
13+
14+
15+
#define M_Kr 0.25
16+
#define M_Ti 0.01
17+
#define M_P 70
18+
19+
20+
21+
/**
22+
BLDC motor class
23+
*/
24+
class BLDCMotor
25+
{
26+
public:
27+
BLDCMotor(int phA,int phB,int phC, long* counter, int encA, int encB , int pp, int cpr);
28+
BLDCMotor(int phA,int phB,int phC, long* counter, int encA, int encB , int pp, int cpr, int en);
29+
void init();
30+
void disableMotor();
31+
void enableMotor();
32+
// Set phase voltages using FOC
33+
void setVoltage(float Uq);
34+
// Set referent velocity PI
35+
void setVelocity(float vel);
36+
// Set referent position P+PI
37+
void setPosition(float pos);
38+
39+
long* _encoderPosition;
40+
41+
int _phA;
42+
int _phB;
43+
int _phC;
44+
int _en;
45+
int _encoder;
46+
int _encA;
47+
int _encB;
48+
49+
float _angleElec;
50+
float _shaftVel;
51+
float _shaftAngle;
52+
53+
int _pp;
54+
int _cpr;
55+
56+
57+
private:
58+
//Encoder alignment to electrical 0 angle
59+
void alignEncoder();
60+
/** State calculation methods */
61+
//Shaft angle calculation
62+
float ShaftAngle();
63+
//Shaft velocity calculation
64+
float ShaftVelocity(float shaftAngle);
65+
//Electrical angle calculation
66+
float ElectricAngle(float shaftAngle);
67+
//Update motor angles and velocities
68+
void updateStates();
69+
//Set phase voltaget to pwm output
70+
void setPwm(int pinPwm, float U);
71+
72+
/** FOC methods */
73+
//Method using FOC to set Uq to the motor at the optimal angle
74+
void setPhaseVoltage(double Uq, double angle_el);
75+
76+
/** Utility funcitons */
77+
//normalizing radian angle to [0,2PI]
78+
double normalizeAngle(double angle);
79+
//Reference low pass filter
80+
float filterLP(float u);
81+
82+
/** Motor control functions */
83+
float velocityPI(float ek);
84+
float positionP(float ek);
85+
86+
float Ua,Ub,Uc;
87+
float Ualpha,Ubeta;
88+
};
89+
90+
91+
#endif

0 commit comments

Comments
 (0)