Skip to content

Commit 55d1abc

Browse files
committed
reafctoring and readme added
1 parent 61c66cf commit 55d1abc

File tree

9 files changed

+385
-73
lines changed

9 files changed

+385
-73
lines changed

BLDCMotor.cpp

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,14 +28,14 @@ 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
@@ -55,13 +55,17 @@ void BLDCMotor::init() {
5555
pinMode(pwmC, OUTPUT);
5656
pinMode(enable_pin, OUTPUT);
5757

58-
5958
// Increase PWM frequency to 32 kHz
6059
// make silent
6160
setPwmFrequency(pwmA);
6261
setPwmFrequency(pwmB);
6362
setPwmFrequency(pwmC);
6463

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;
68+
6569
delay(500);
6670
// enable motor
6771
enable();

README.md

Lines changed: 226 additions & 63 deletions
Large diffs are not rendered by default.

examples/HMBGC_example/HMBGC_example.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ void setup() {
2626
Serial.begin(115200);
2727

2828
// check if you need internal pullups
29-
// Pullup::EXTERN - external pullup added
29+
// Pullup::EXTERN - external pullup added - dafault
3030
// Pullup::INTERN - needs internal arduino pullup
3131
encoder.pullup = Pullup::INTERN;
3232
// initialise encoder hardware

examples/angle_control/angle_control.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ void setup() {
2323
Serial.begin(115200);
2424

2525
// check if you need internal pullups
26-
// Pullup::EXTERN - external pullup added
26+
// Pullup::EXTERN - external pullup added - dafault
2727
// Pullup::INTERN - needs internal arduino pullup
2828
encoder.pullup = Pullup::EXTERN;
2929

examples/angle_control_serial/angle_control_serial.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ void setup() {
2626
Serial.begin(115200);
2727

2828
// check if you need internal pullups
29-
// Pullup::EXTERN - external pullup added
29+
// Pullup::EXTERN - external pullup added - dafault
3030
// Pullup::INTERN - needs internal arduino pullup
3131
encoder.pullup = Pullup::EXTERN;
3232

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
#include <ArduinoFOC.h>
2+
3+
// Only pins 2 and 3 are supported
4+
#define arduinoInt1 2 // Arduino UNO interrupt 0
5+
#define arduinoInt2 3 // Arduino UNO interrupt 1
6+
7+
Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192);
8+
// interrupt ruotine intialisation
9+
void doA(){encoder.handleA();}
10+
void doB(){encoder.handleB();}
11+
12+
void setup() {
13+
// debugging port
14+
Serial.begin(115200);
15+
16+
// check if you need internal pullups
17+
// Pullup::EXTERN - external pullup added - dafault
18+
// Pullup::INTERN - needs internal arduino pullup
19+
encoder.pullup = Pullup::EXTERN;
20+
21+
// initialise encoder hardware
22+
encoder.init(doA, doB);
23+
24+
Serial.println("Encoder ready");
25+
delay(1000);
26+
}
27+
28+
void loop() {
29+
// display the angle and the angular velocity to the terminal
30+
Serial.print(encoder.getAngle());
31+
Serial.print("\t")
32+
Serial.ptntln(encoder.getVelocity());
33+
}

examples/minimal_example/minimal_example.ino

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,11 @@ void setup() {
2525
encoder.init(doA, doB);
2626
// link the motor to the sensor
2727
motor.linkEncoder(&encoder);
28-
// intialise motor
29-
motor.init();
3028
// velocity control
3129
motor.controller = ControlType::velocity;
30+
31+
// intialise motor
32+
motor.init();
3233
// align encoder and start FOC
3334
motor.initFOC();
3435

@@ -44,5 +45,4 @@ void loop() {
4445
motor.loopFOC();
4546
// control loop
4647
motor.move(target_velocity);
47-
motor_monitor();
4848
}

examples/velocity_ultraslow_control_serial/velocity_ultraslow_control_serial.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ void setup() {
2727
Serial.begin(115200);
2828

2929
// check if you need internal pullups
30-
// Pullup::EXTERN - external pullup added
30+
// Pullup::EXTERN - external pullup added - dafault
3131
// Pullup::INTERN - needs internal arduino pullup
3232
encoder.pullup = Pullup::EXTERN;
3333

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#include <ArduinoFOC.h>
2+
3+
// Only pins 2 and 3 are supported
4+
#define arduinoInt1 2 // Arduino UNO interrupt 0
5+
#define arduinoInt2 3 // Arduino UNO interrupt 1
6+
7+
// BLDCMotor( int phA, int phB, int phC, int pp, int en)
8+
// - phA, phB, phC - motor A,B,C phase pwm pins
9+
// - pp - pole pair number
10+
// - enable pin - (optional input)
11+
BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8);
12+
// Encoder(int encA, int encB , int cpr, int index)
13+
// - encA, encB - encoder A and B pins
14+
// - ppr - impulses per rotation (cpr=ppr*4)
15+
// - index pin - (optional input)
16+
Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4);
17+
// interrupt ruotine intialisation
18+
void doA(){encoder.handleA();}
19+
void doB(){encoder.handleB();}
20+
21+
void setup() {
22+
// debugging port
23+
Serial.begin(115200);
24+
25+
// check if you need internal pullups
26+
// Pullup::EXTERN - external pullup added - dafault
27+
// Pullup::INTERN - needs internal arduino pullup
28+
encoder.pullup = Pullup::EXTERN;
29+
30+
// initialise encoder hardware
31+
encoder.init(doA, doB);
32+
33+
// set driver type
34+
// DriverType::unipolar
35+
// DriverType::bipolar - default
36+
motor.driver = DriverType::bipolar;
37+
38+
// power supply voltage
39+
// default 12V
40+
motor.power_supply_voltage = 12;
41+
42+
// set FOC loop to be used
43+
// ControlType::voltage
44+
// ControlType::velocity
45+
// ControlType::velocity_ultra_slow
46+
// ControlType::angle
47+
motor.controller = ControlType::voltage;
48+
49+
// link the motor to the sensor
50+
motor.linkEncoder(&encoder);
51+
52+
// intialise motor
53+
motor.init();
54+
// align encoder and start FOC
55+
motor.initFOC();
56+
57+
58+
Serial.println("Motor ready.");
59+
delay(1000);
60+
}
61+
62+
// uq voltage
63+
float voltage_Uq = 5;
64+
65+
void loop() {
66+
67+
// iterative state calculation calculating angle
68+
// and setting FOC pahse voltage
69+
// the faster you run this funciton the better
70+
// in arduino loop it should have ~1kHz
71+
// the best would be to be in ~10kHz range
72+
motor.loopFOC();
73+
74+
// iterative function setting the outter loop target
75+
// velocity, position or voltage
76+
// this funciton can be run at much lower frequency than loopFOC funciton
77+
// it can go as low as ~50Hz
78+
motor.move(voltage_Uq);
79+
80+
81+
// function intended to be used with serial plotter to monitor motor variables
82+
// significantly slowing the execution down!!!!
83+
motor_monitor();
84+
}
85+
86+
// utility function intended to be used with serial plotter to monitor motor variables
87+
// significantly slowing the execution down!!!!
88+
void motor_monitor() {
89+
switch (motor.controller) {
90+
case ControlType::velocity_ultra_slow:
91+
case ControlType::velocity:
92+
Serial.print(motor.voltage_q);
93+
Serial.print("\t");
94+
Serial.print(motor.shaft_velocity_sp);
95+
Serial.print("\t");
96+
Serial.println(motor.shaft_velocity);
97+
break;
98+
case ControlType::angle:
99+
Serial.print(motor.voltage_q);
100+
Serial.print("\t");
101+
Serial.print(motor.shaft_angle_sp);
102+
Serial.print("\t");
103+
Serial.println(motor.shaft_angle);
104+
break;
105+
case ControlType::voltage:
106+
Serial.print(motor.voltage_q);
107+
Serial.print("\t");
108+
Serial.println(motor.shaft_velocity);
109+
break;
110+
}
111+
}
112+

0 commit comments

Comments
 (0)