Skip to content

Commit 81dfc57

Browse files
committed
Merge branch 'dev'
2 parents 979f700 + 916cb4d commit 81dfc57

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

49 files changed

+1936
-870
lines changed

README.md

Lines changed: 346 additions & 176 deletions
Large diffs are not rendered by default.

examples/HMBGC_example/HMBGC_example.ino renamed to examples/encoder_examples/HMBGC_example/HMBGC_example.ino

Lines changed: 13 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <SimpleFOC.h>
2+
// software interrupt library
23
#include <PciManager.h>
34
#include <PciListenerImp.h>
45

@@ -47,27 +48,31 @@ void setup() {
4748
// set FOC loop to be used
4849
// ControlType::voltage
4950
// ControlType::velocity
50-
// ControlType::velocity_ultra_slow
5151
// ControlType::angle
5252
motor.controller = ControlType::velocity;
5353

5454
// contoller configuration based on the controll type
5555
// velocity PI controller parameters
56-
// default K=1.0 Ti = 0.003
57-
motor.PI_velocity.K = 0.3;
58-
motor.PI_velocity.Ti = 0.003;
56+
// default P=0.5 I = 10
57+
motor.PI_velocity.P = 0.2;
58+
motor.PI_velocity.I = 20;
5959
//defualt voltage_power_supply/2
6060
motor.PI_velocity.voltage_limit = 6;
6161
// jerk control using voltage voltage ramp
6262
// default value is 300 volts per sec ~ 0.3V per millisecond
63-
motor.PI_velocity.voltage_ramp = 300;
63+
motor.PI_velocity.voltage_ramp = 1000;
64+
65+
// velocity low pass filtering
66+
// default 5ms - try different values to see what is the best.
67+
// the lower the less filtered
68+
motor.LPF_velocity.Tf = 0.01;
6469

6570
// use debugging with serial for motor init
6671
// comment out if not needed
6772
motor.useDebugging(Serial);
6873

6974
// link the motor to the sensor
70-
motor.linkEncoder(&encoder);
75+
motor.linkSensor(&encoder);
7176

7277
// intialise motor
7378
motor.init();
@@ -101,39 +106,9 @@ void loop() {
101106

102107
// function intended to be used with serial plotter to monitor motor variables
103108
// significantly slowing the execution down!!!!
104-
// motor_monitor();
109+
// motor.monitor();
105110
}
106111

107-
// utility function intended to be used with serial plotter to monitor motor variables
108-
// significantly slowing the execution down!!!!
109-
void motor_monitor() {
110-
switch (motor.controller) {
111-
case ControlType::velocity_ultra_slow:
112-
case ControlType::velocity:
113-
Serial.print(motor.voltage_q);
114-
Serial.print("\t");
115-
Serial.print(motor.shaft_velocity_sp);
116-
Serial.print("\t");
117-
Serial.println(motor.shaft_velocity);
118-
break;
119-
case ControlType::angle:
120-
Serial.print(motor.voltage_q);
121-
Serial.print("\t");
122-
Serial.print(motor.shaft_angle_sp);
123-
Serial.print("\t");
124-
Serial.println(motor.shaft_angle);
125-
break;
126-
case ControlType::voltage:
127-
Serial.print(motor.voltage_q);
128-
Serial.print("\t");
129-
Serial.print(motor.shaft_velocity);
130-
Serial.print("\t");
131-
Serial.println(motor.shaft_angle);
132-
break;
133-
}
134-
}
135-
136-
137112
// Serial communication callback function
138113
// gets the target value from the user
139114
void serialEvent() {
@@ -148,7 +123,7 @@ void serialEvent() {
148123
// end of input
149124
if (inChar == '\n') {
150125
target_velocity = inputString.toFloat();
151-
Serial.print("Tagret velocity: ");
126+
Serial.print("Target velocity: ");
152127
Serial.println(target_velocity);
153128
inputString = "";
154129
}

examples/angle_control/angle_control.ino renamed to examples/encoder_examples/angle_control/angle_control.ino

Lines changed: 30 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,7 @@
11
#include <SimpleFOC.h>
2+
// software interrupt library
3+
#include <PciManager.h>
4+
#include <PciListenerImp.h>
25

36
// Only pins 2 and 3 are supported
47
#define arduinoInt1 2 // Arduino UNO interrupt 0
@@ -19,12 +22,10 @@ Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, A0);
1922
// channel A and B callbacks
2023
void doA(){encoder.handleA();}
2124
void doB(){encoder.handleB();}
22-
// index calback interrupt code
23-
// please set the right PCINT(0,1,2)_vect parameter
24-
// PCINT0_vect - index pin in between D8 and D13
25-
// PCINT1_vect - index pin in between A0 and A5 (recommended)
26-
// PCINT2_vect - index pin in between D0 and D7
27-
ISR (PCINT1_vect) { encoder.handleIndex(); }
25+
void doIndex(){encoder.handleIndex();}
26+
27+
// If no available hadware interrupt pins use the software interrupt
28+
PciListenerImp listenerIndex(encoder.index_pin, doIndex);
2829

2930
void setup() {
3031
// debugging port
@@ -41,7 +42,11 @@ void setup() {
4142
encoder.pullup = Pullup::EXTERN;
4243

4344
// initialise encoder hardware
44-
encoder.init(doA, doB);
45+
encoder.init();
46+
// hardware interrupt enable
47+
encoder.enableInterrupts(doA, doB);
48+
// software interrupts
49+
PciManager.registerListener(&listenerIndex);
4550

4651
// power supply voltage
4752
// default 12V
@@ -50,47 +55,50 @@ void setup() {
5055
// index search velocity - default 1rad/s
5156
motor.index_search_velocity = 1;
5257
// index search PI contoller parameters
53-
// default K=0.5 Ti = 0.01
54-
motor.PI_velocity_index_search.K = 0.1;
55-
motor.PI_velocity_index_search.Ti = 0.01;
58+
// default P=1, I=10
59+
motor.PI_velocity_index_search.P = 1;
60+
motor.PI_velocity_index_search.I = 20;
5661
//motor.PI_velocity_index_search.voltage_limit = 3;
5762
// jerk control using voltage voltage ramp
5863
// default value is 100
5964
motor.PI_velocity_index_search.voltage_ramp = 100;
6065

61-
6266
// set FOC loop to be used
6367
// ControlType::voltage
6468
// ControlType::velocity
65-
// ControlType::velocity_ultra_slow
6669
// ControlType::angle
6770
motor.controller = ControlType::angle;
68-
6971

7072
// contoller configuration based on the controll type
7173
// velocity PI controller parameters
72-
// default K=1.0 Ti = 0.003
73-
motor.PI_velocity.K = 0.3;
74-
motor.PI_velocity.Ti = 0.003;
74+
// default P=0.5 I = 10
75+
motor.PI_velocity.P = 0.2;
76+
motor.PI_velocity.I = 20;
7577
//defualt voltage_power_supply/2
7678
motor.PI_velocity.voltage_limit = 6;
7779
// jerk control using voltage voltage ramp
7880
// default value is 300 volts per sec ~ 0.3V per millisecond
79-
motor.PI_velocity.voltage_ramp = 300;
81+
motor.PI_velocity.voltage_ramp = 1000;
8082

83+
// velocity low pass filtering
84+
// default 5ms - try different values to see what is the best.
85+
// the lower the less filtered
86+
motor.LPF_velocity.Tf = 0.01;
87+
8188
// angle P controller
82-
// default K=70
83-
motor.P_angle.K = 20;
89+
// default P=20
90+
motor.P_angle.P = 20;
8491
// maximal velocity of the poisiiton control
8592
// default 20
8693
motor.P_angle.velocity_limit = 4;
8794

95+
8896
// use debugging with serial for motor init
8997
// comment out if not needed
9098
motor.useDebugging(Serial);
9199

92100
// link the motor to the sensor
93-
motor.linkEncoder(&encoder);
101+
motor.linkSensor(&encoder);
94102

95103
// intialise motor
96104
motor.init();
@@ -123,36 +131,7 @@ void loop() {
123131

124132
// function intended to be used with serial plotter to monitor motor variables
125133
// significantly slowing the execution down!!!!
126-
// motor_monitor();
127-
}
128-
129-
// utility function intended to be used with serial plotter to monitor motor variables
130-
// significantly slowing the execution down!!!!
131-
void motor_monitor() {
132-
switch (motor.controller) {
133-
case ControlType::velocity_ultra_slow:
134-
case ControlType::velocity:
135-
Serial.print(motor.voltage_q);
136-
Serial.print("\t");
137-
Serial.print(motor.shaft_velocity_sp);
138-
Serial.print("\t");
139-
Serial.println(motor.shaft_velocity);
140-
break;
141-
case ControlType::angle:
142-
Serial.print(motor.voltage_q);
143-
Serial.print("\t");
144-
Serial.print(motor.shaft_angle_sp);
145-
Serial.print("\t");
146-
Serial.println(motor.shaft_angle);
147-
break;
148-
case ControlType::voltage:
149-
Serial.print(motor.voltage_q);
150-
Serial.print("\t");
151-
Serial.print(motor.shaft_angle);
152-
Serial.print("\t");
153-
Serial.println(motor.shaft_velocity);
154-
break;
155-
}
134+
// motor.monitor();
156135
}
157136

158137
// Serial communication callback function
@@ -169,7 +148,7 @@ void serialEvent() {
169148
// end of input
170149
if (inChar == '\n') {
171150
target_angle = inputString.toFloat();
172-
Serial.print("Tagret angle: ");
151+
Serial.print("Target angle: ");
173152
Serial.println(target_angle);
174153
inputString = "";
175154
}

examples/change_direction/change_direction.ino renamed to examples/encoder_examples/change_direction/change_direction.ino

Lines changed: 18 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -13,18 +13,12 @@ BLDCMotor motor = BLDCMotor(9, 5, 6, 11, 8);
1313
// - encA, encB - encoder A and B pins
1414
// - ppr - impulses per rotation (cpr=ppr*4)
1515
// - index pin - (optional input)
16-
Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, A0);
16+
Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192);
1717

1818
// Interrupt rutine intialisation
1919
// channel A and B callbacks
2020
void doA(){encoder.handleA();}
2121
void doB(){encoder.handleB();}
22-
// index calback interrupt code
23-
// please set the right PCINT(0,1,2)_vect parameter
24-
// PCINT0_vect - index pin in between D8 and D13
25-
// PCINT1_vect - index pin in between A0 and A5 (recommended)
26-
// PCINT2_vect - index pin in between D0 and D7
27-
ISR (PCINT1_vect) { encoder.handleIndex(); }
2822

2923
void setup() {
3024
// debugging port
@@ -41,7 +35,9 @@ void setup() {
4135
encoder.pullup = Pullup::EXTERN;
4236

4337
// initialise encoder hardware
44-
encoder.init(doA, doB);
38+
encoder.init();
39+
// hardware interrupt enable
40+
encoder.enableInterrupts(doA, doB);
4541

4642
// power supply voltage
4743
// default 12V
@@ -50,39 +46,42 @@ void setup() {
5046
// index search velocity - default 1rad/s
5147
motor.index_search_velocity = 1;
5248
// index search PI contoller parameters
53-
// default K=0.5 Ti = 0.01
54-
motor.PI_velocity_index_search.K = 0.1;
55-
motor.PI_velocity_index_search.Ti = 0.01;
49+
// default P=1, I=10
50+
motor.PI_velocity_index_search.P = 1;
51+
motor.PI_velocity_index_search.I = 20;
5652
//motor.PI_velocity_index_search.voltage_limit = 3;
5753
// jerk control using voltage voltage ramp
5854
// default value is 100
5955
motor.PI_velocity_index_search.voltage_ramp = 100;
60-
56+
6157
// set FOC loop to be used
6258
// ControlType::voltage
6359
// ControlType::velocity
64-
// ControlType::velocity_ultra_slow
6560
// ControlType::angle
6661
motor.controller = ControlType::velocity;
6762

6863
// contoller configuration based on the controll type
6964
// velocity PI controller parameters
70-
// default K=1.0 Ti = 0.003
71-
motor.PI_velocity.K = 0.3;
72-
motor.PI_velocity.Ti = 0.003;
65+
// default P=0.5 I = 10
66+
motor.PI_velocity.P = 0.2;
67+
motor.PI_velocity.I = 20;
7368
//defualt voltage_power_supply/2
7469
motor.PI_velocity.voltage_limit = 6;
7570
// jerk control using voltage voltage ramp
7671
// default value is 300 volts per sec ~ 0.3V per millisecond
77-
motor.PI_velocity.voltage_ramp = 300;
72+
motor.PI_velocity.voltage_ramp = 1000;
7873

74+
// velocity low pass filtering
75+
// default 5ms - try different values to see what is the best.
76+
// the lower the less filtered
77+
motor.LPF_velocity.Tf = 0.01;
7978

8079
// use debugging with serial for motor init
8180
// comment out if not needed
8281
motor.useDebugging(Serial);
8382

8483
// link the motor to the sensor
85-
motor.linkEncoder(&encoder);
84+
motor.linkSensor(&encoder);
8685
// intialise motor
8786
motor.init();
8887
// align encoder and start FOC
@@ -121,35 +120,7 @@ void loop() {
121120

122121
// function intended to be used with serial plotter to monitor motor variables
123122
// significantly slowing the execution down!!!!
124-
//motor_monitor();
123+
//motor.monitor();
125124
}
126125

127-
// utility function intended to be used with serial plotter to monitor motor variables
128-
// significantly slowing the execution down!!!!
129-
void motor_monitor() {
130-
switch (motor.controller) {
131-
case ControlType::velocity_ultra_slow:
132-
case ControlType::velocity:
133-
Serial.print(motor.voltage_q);
134-
Serial.print("\t");
135-
Serial.print(motor.shaft_velocity_sp);
136-
Serial.print("\t");
137-
Serial.println(motor.shaft_velocity);
138-
break;
139-
case ControlType::angle:
140-
Serial.print(motor.voltage_q);
141-
Serial.print("\t");
142-
Serial.print(motor.shaft_angle_sp);
143-
Serial.print("\t");
144-
Serial.println(motor.shaft_angle);
145-
break;
146-
case ControlType::voltage:
147-
Serial.print(motor.voltage_q);
148-
Serial.print("\t");
149-
Serial.print(motor.shaft_angle);
150-
Serial.print("\t");
151-
Serial.println(motor.shaft_velocity);
152-
break;
153-
}
154-
}
155126

examples/encoder_example/encoder_example.ino renamed to examples/encoder_examples/encoder_example/encoder_example.ino

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,9 @@ void setup() {
2424
encoder.pullup = Pullup::EXTERN;
2525

2626
// initialise encoder hardware
27-
encoder.init(doA, doB);
27+
encoder.init();
28+
// hardware interrupt enable
29+
encoder.enableInterrupts(doA, doB);
2830

2931
Serial.println("Encoder ready");
3032
_delay(1000);

0 commit comments

Comments
 (0)