Skip to content

Commit 00b3a11

Browse files
askuricaskuric
committed
merge from dev
2 parents 06cdef7 + b3f463e commit 00b3a11

File tree

25 files changed

+811
-304
lines changed

25 files changed

+811
-304
lines changed

‎.github/workflows/ccpp.yml

Lines changed: 0 additions & 13 deletions
This file was deleted.

‎ArduinoFOC.h

Lines changed: 4 additions & 143 deletions
Original file line numberDiff line numberDiff line change
@@ -1,146 +1,7 @@
1-
#ifndef BLDCMotor_h
2-
#define BLDCMotor_h
1+
#ifndef ArduinoFOC_h
2+
#define ArduinoFOC_h
33

4-
#include "Arduino.h"
4+
#include "BLDCMotor.h"
55
#include "Encoder.h"
66

7-
// default configuration values
8-
// power supply voltage
9-
#define DEF_POWER_SUPPLY 12.0
10-
// velocity PI controller params
11-
#define DEF_PI_VEL_K 1.0
12-
#define DEF_PI_VEL_TI 0.003
13-
// ultra slow velocity PI params
14-
#define DEF_PI_VEL_US_K 120.0
15-
#define DEF_PI_VEL_US_TI 100.0
16-
// angle P params
17-
#define DEF_P_ANGLE_K 20
18-
// angle velocity limit default
19-
#define DEF_P_ANGLE_VEL_LIM 20
20-
21-
// sign funciton
22-
#define sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
23-
// utility defines
24-
#define _2_SQRT3 1.15470053838
25-
#define _1_SQRT3 0.57735026919
26-
#define _SQRT3_2 0.86602540378
27-
#define _SQRT2 1.41421356237
28-
#define _120_D2R 2.09439510239
29-
30-
// controller type configuration enum
31-
enum ControlType{
32-
voltage,
33-
velocity,
34-
velocity_ultra_slow,
35-
angle
36-
};
37-
38-
// driver type configuration enum
39-
enum DriverType{
40-
bipolar, // L6234
41-
unipolar // HMBGC
42-
};
43-
44-
// P/PI controller strucutre
45-
struct PI_s{
46-
float K;
47-
float Ti;
48-
long timestamp;
49-
float uk_1, ek_1;
50-
float u_limit;
51-
float velocity_limit;
52-
};
53-
54-
/**
55-
BLDC motor class
56-
*/
57-
class BLDCMotor
58-
{
59-
public:
60-
BLDCMotor(int phA,int phB,int phC,int pp, int en = 0);
61-
// change driver state
62-
void init();
63-
void disable();
64-
void enable();
65-
// connect encoder
66-
void linkEncoder(Encoder* enc);
67-
68-
// initilise FOC
69-
void initFOC();
70-
// iterative method updating motor angles and velocity measurement
71-
void loopFOC();
72-
// iterative control loop defined by controller
73-
void move(float target);
74-
75-
76-
// hardware variables
77-
int pwmA;
78-
int pwmB;
79-
int pwmC;
80-
int enable_pin;
81-
int pole_pairs;
82-
83-
// state variables
84-
float elctric_angle;
85-
float shaft_velocity;
86-
float shaft_angle;
87-
float shaft_velocity_sp;
88-
float shaft_angle_sp;
89-
float voltage_q;
90-
91-
// Power supply woltage
92-
float power_supply_voltage;
93-
94-
// configuraion structures
95-
DriverType driver;
96-
ControlType controller;
97-
PI_s PI_velocity;
98-
PI_s PI_velocity_ultra_slow;
99-
PI_s P_angle;
100-
101-
// encoder link
102-
Encoder* encoder;
103-
104-
105-
private:
106-
//Encoder alignment to electrical 0 angle
107-
void alignEncoder();
108-
/** State calculation methods */
109-
//Shaft angle calculation
110-
float shaftAngle();
111-
//Shaft velocity calculation
112-
float shaftVelocity();
113-
114-
//Electrical angle calculation
115-
float electricAngle(float shaftAngle);
116-
//Set phase voltaget to pwm output
117-
void setPwm(int pinPwm, float U);
118-
119-
/** FOC methods */
120-
//Method using FOC to set Uq to the motor at the optimal angle
121-
void setPhaseVoltage(double Uq, double angle_el);
122-
void setPhaseVoltageUnipolar(double Uq, double angle_el);
123-
void setPhaseVoltageBipolar(double Uq, double angle_el);
124-
125-
/** Utility funcitons */
126-
//normalizing radian angle to [0,2PI]
127-
double normalizeAngle(double angle);
128-
//Reference low pass filter
129-
float filterLP(float u);
130-
131-
/** Motor control functions */
132-
float velocityPI(float ek);
133-
float velocityUltraSlowPI(float ek);
134-
float positionP(float ek);
135-
136-
float Ua,Ub,Uc;
137-
float Ualpha,Ubeta;
138-
};
139-
140-
141-
/*
142-
High PWM frequency
143-
*/
144-
void setPwmFrequency(int pin);
145-
146-
#endif
7+
#endif

‎ArduinoFOC.cpp renamed to ‎BLDCMotor.cpp

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
#include "ArduinoFOC.h"
1+
#include "BLDCMotor.h"
22

33

44
/*
@@ -28,20 +28,23 @@ BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en)
2828
PI_velocity.K = DEF_PI_VEL_K;
2929
PI_velocity.Ti = DEF_PI_VEL_TI;
3030
PI_velocity.timestamp = micros();
31-
PI_velocity.u_limit = DEF_POWER_SUPPLY;
31+
PI_velocity.u_limit = -1;
3232

3333
// Ultra slow velocity
3434
// PI contoroller
3535
PI_velocity_ultra_slow.K = DEF_PI_VEL_US_K;
3636
PI_velocity_ultra_slow.Ti = DEF_PI_VEL_US_TI;
3737
PI_velocity_ultra_slow.timestamp = micros();
38-
PI_velocity_ultra_slow.u_limit = DEF_POWER_SUPPLY;
38+
PI_velocity_ultra_slow.u_limit = -1;
3939

4040
// position loop config
4141
// P controller constant
4242
P_angle.K = DEF_P_ANGLE_K;
4343
// maximum angular velocity to be used for positioning
4444
P_angle.velocity_limit = DEF_P_ANGLE_VEL_LIM;
45+
46+
// driver deafault type
47+
driver = DriverType::bipolar;
4548
}
4649

4750
// init hardware pins
@@ -52,16 +55,22 @@ void BLDCMotor::init() {
5255
pinMode(pwmC, OUTPUT);
5356
pinMode(enable_pin, OUTPUT);
5457

55-
5658
// Increase PWM frequency to 32 kHz
5759
// make silent
5860
setPwmFrequency(pwmA);
5961
setPwmFrequency(pwmB);
6062
setPwmFrequency(pwmC);
6163

62-
driver = DriverType::bipolar;
64+
// check if u_limit configuration has been done.
65+
// if not set it to the power_supply_voltage
66+
if(PI_velocity.u_limit == -1) PI_velocity.u_limit = power_supply_voltage;
67+
if(PI_velocity_ultra_slow.u_limit == -1) PI_velocity_ultra_slow.u_limit = power_supply_voltage;
6368

6469
delay(500);
70+
// enable motor
71+
enable();
72+
delay(500);
73+
6574
}
6675

6776
/*

‎BLDCMotor.h

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
#ifndef BLDCMotor_h
2+
#define BLDCMotor_h
3+
4+
#include "Arduino.h"
5+
#include "Encoder.h"
6+
7+
// default configuration values
8+
// power supply voltage
9+
#define DEF_POWER_SUPPLY 12.0
10+
// velocity PI controller params
11+
#define DEF_PI_VEL_K 1.0
12+
#define DEF_PI_VEL_TI 0.003
13+
// ultra slow velocity PI params
14+
#define DEF_PI_VEL_US_K 120.0
15+
#define DEF_PI_VEL_US_TI 100.0
16+
// angle P params
17+
#define DEF_P_ANGLE_K 20
18+
// angle velocity limit default
19+
#define DEF_P_ANGLE_VEL_LIM 20
20+
21+
// sign funciton
22+
#define sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
23+
// utility defines
24+
#define _2_SQRT3 1.15470053838
25+
#define _1_SQRT3 0.57735026919
26+
#define _SQRT3_2 0.86602540378
27+
#define _SQRT2 1.41421356237
28+
#define _120_D2R 2.09439510239
29+
30+
// controller type configuration enum
31+
enum ControlType{
32+
voltage,
33+
velocity,
34+
velocity_ultra_slow,
35+
angle
36+
};
37+
38+
// driver type configuration enum
39+
enum DriverType{
40+
bipolar, // L6234
41+
unipolar // HMBGC
42+
};
43+
44+
// P/PI controller strucutre
45+
struct PI_s{
46+
float K;
47+
float Ti;
48+
long timestamp;
49+
float uk_1, ek_1;
50+
float u_limit;
51+
float velocity_limit;
52+
};
53+
54+
/**
55+
BLDC motor class
56+
*/
57+
class BLDCMotor
58+
{
59+
public:
60+
BLDCMotor(int phA,int phB,int phC,int pp, int en = 0);
61+
// change driver state
62+
void init();
63+
void disable();
64+
void enable();
65+
// connect encoder
66+
void linkEncoder(Encoder* enc);
67+
68+
// initilise FOC
69+
void initFOC();
70+
// iterative method updating motor angles and velocity measurement
71+
void loopFOC();
72+
// iterative control loop defined by controller
73+
void move(float target);
74+
75+
76+
// hardware variables
77+
int pwmA;
78+
int pwmB;
79+
int pwmC;
80+
int enable_pin;
81+
int pole_pairs;
82+
83+
// state variables
84+
float elctric_angle;
85+
float shaft_velocity;
86+
float shaft_angle;
87+
float shaft_velocity_sp;
88+
float shaft_angle_sp;
89+
float voltage_q;
90+
91+
// Power supply woltage
92+
float power_supply_voltage;
93+
94+
// configuraion structures
95+
DriverType driver;
96+
ControlType controller;
97+
PI_s PI_velocity;
98+
PI_s PI_velocity_ultra_slow;
99+
PI_s P_angle;
100+
101+
// encoder link
102+
Encoder* encoder;
103+
104+
105+
private:
106+
//Encoder alignment to electrical 0 angle
107+
void alignEncoder();
108+
/** State calculation methods */
109+
//Shaft angle calculation
110+
float shaftAngle();
111+
//Shaft velocity calculation
112+
float shaftVelocity();
113+
114+
//Electrical angle calculation
115+
float electricAngle(float shaftAngle);
116+
//Set phase voltaget to pwm output
117+
void setPwm(int pinPwm, float U);
118+
119+
/** FOC methods */
120+
//Method using FOC to set Uq to the motor at the optimal angle
121+
void setPhaseVoltage(double Uq, double angle_el);
122+
void setPhaseVoltageUnipolar(double Uq, double angle_el);
123+
void setPhaseVoltageBipolar(double Uq, double angle_el);
124+
125+
/** Utility funcitons */
126+
//normalizing radian angle to [0,2PI]
127+
double normalizeAngle(double angle);
128+
//Reference low pass filter
129+
float filterLP(float u);
130+
131+
/** Motor control functions */
132+
float velocityPI(float ek);
133+
float velocityUltraSlowPI(float ek);
134+
float positionP(float ek);
135+
136+
float Ua,Ub,Uc;
137+
float Ualpha,Ubeta;
138+
};
139+
140+
141+
/*
142+
High PWM frequency
143+
*/
144+
void setPwmFrequency(int pin);
145+
146+
#endif

0 commit comments

Comments
 (0)