Skip to content

Commit 049e864

Browse files
askuricaskuric
committed
first upload
0 parents  commit 049e864

File tree

7 files changed

+464
-0
lines changed

7 files changed

+464
-0
lines changed

‎ArduinoCode/BLDCmotor.cpp

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

‎ArduinoCode/BLDCmotor.h

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

‎ArduinoCode/encoder.cpp

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
#include "encoder.h"
2+
#include "Arduino.h"
3+
4+
// countiner
5+
long counter[2]={0};
6+
7+
/**
8+
Encoder methods
9+
*/
10+
11+
int A1_=0;
12+
int B1_=0;
13+
int A2_=0;
14+
int B2_=0;
15+
/*
16+
A channel
17+
*/
18+
void doEncoder1A(){
19+
int A = digitalRead(A0);
20+
if( A!= A1_ ){
21+
if(A1_ == B1_){
22+
counter[ENCODER_1] += 1;
23+
}else{
24+
counter[ENCODER_1] -= 1;
25+
}
26+
A1_ = A;
27+
}
28+
}
29+
30+
/*
31+
B channel
32+
*/
33+
void doEncoder1B(){
34+
int B = digitalRead(A1);
35+
if( B!= B1_ ){
36+
if( A1_ != B1_ ){
37+
counter[ENCODER_1] += 1;
38+
}else{
39+
counter[ENCODER_1] -= 1;
40+
}
41+
B1_ = B;
42+
}
43+
}
44+
45+
/*
46+
A channel
47+
*/
48+
void doEncoder2A(){
49+
int A = digitalRead(A2);
50+
if( A!= A2_ ){
51+
if(A2_ == B2_){
52+
counter[ENCODER_2] += 1;
53+
}else{
54+
counter[ENCODER_2] -= 1;
55+
}
56+
A2_ = A;
57+
}
58+
}
59+
60+
/*
61+
B channel
62+
*/
63+
void doEncoder2B(){
64+
int B = digitalRead(A3);
65+
if( B!= B2_ ){
66+
if( A2_ != B2_ ){
67+
counter[ENCODER_2] += 1;
68+
}else{
69+
counter[ENCODER_2] -= 1;
70+
}
71+
B2_ = B;
72+
}
73+
}
74+
75+

‎ArduinoCode/encoder.h

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
#ifndef Encoder_H
2+
#define Encoder_H
3+
4+
extern long counter[2];
5+
6+
void doEncoder1A();
7+
void doEncoder1B();
8+
void doEncoder2A();
9+
void doEncoder2B();
10+
11+
#define ENCODER_1 0
12+
#define ENCODER_2 1
13+
14+
#endif

0 commit comments

Comments
 (0)