Skip to content

Commit 62d22d4

Browse files
askuricaskuric
committed
README.md
1 parent 7322b4e commit 62d22d4

File tree

4 files changed

+201
-69
lines changed

4 files changed

+201
-69
lines changed

‎src/BLDCMotor.cpp

Lines changed: 16 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -70,9 +70,7 @@ void BLDCMotor::init() {
7070
if(monitor_port) monitor_port->println("MOT: PWM config.");
7171
// Increase PWM frequency to 32 kHz
7272
// make silent
73-
_setPwmFrequency(pwmA);
74-
_setPwmFrequency(pwmB);
75-
_setPwmFrequency(pwmC);
73+
_setPwmFrequency(pwmA, pwmB, pwmC);
7674

7775
// sanity check for the voltage limit configuration
7876
if(PI_velocity.voltage_limit > voltage_power_supply) PI_velocity.voltage_limit = voltage_power_supply;
@@ -92,17 +90,13 @@ void BLDCMotor::disable()
9290
// disable the driver - if enable_pin pin available
9391
if (hasEnable()) digitalWrite(enable_pin, LOW);
9492
// set zero to PWM
95-
setPwm(pwmA, 0);
96-
setPwm(pwmB, 0);
97-
setPwm(pwmC, 0);
93+
setPwm(0, 0, 0);
9894
}
9995
// enable motor driver
10096
void BLDCMotor::enable()
10197
{
10298
// set zero to PWM
103-
setPwm(pwmA, 0);
104-
setPwm(pwmB, 0);
105-
setPwm(pwmC, 0);
99+
setPwm(0, 0, 0);
106100
// enable_pin the driver - if enable_pin pin available
107101
if (hasEnable()) digitalWrite(enable_pin, HIGH);
108102

@@ -142,10 +136,11 @@ int BLDCMotor::alignSensor() {
142136
// Encoder alignment the absolute zero angle
143137
// - to the index
144138
int BLDCMotor::absoluteZeroAlign() {
145-
// if no absolute zero return
139+
140+
if(monitor_port) monitor_port->println("MOT: Absolute zero align.");
141+
// if no absolute zero return
146142
if(!sensor->hasAbsoluteZero()) return 0;
147143

148-
if(monitor_port) monitor_port->println("MOT: Absolute zero align.");
149144

150145
if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching...");
151146
// search the absolute zero with small velocity
@@ -348,28 +343,21 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
348343
}
349344

350345
// set the voltages in hardware
351-
setPwm(pwmA, Ua);
352-
setPwm(pwmB, Ub);
353-
setPwm(pwmC, Uc);
346+
setPwm(Ua, Ub, Uc);
354347
}
355348

356349

357350

358351

359352
// Set voltage to the pwm pin
360-
// - function a bit optimized to get better performance
361-
void BLDCMotor::setPwm(int pinPwm, float U) {
362-
// max value
363-
float U_max = voltage_power_supply;
364-
365-
// sets the voltage [0,12V(U_max)] to pwm [0,255]
366-
// - U_max you can set in header file - default 12V
367-
int U_pwm = 255.0 * U / U_max;
368-
369-
// limit the values between 0 and 255
370-
U_pwm = (U_pwm < 0) ? 0 : (U_pwm >= 255) ? 255 : U_pwm;
371-
372-
analogWrite(pinPwm, U_pwm);
353+
void BLDCMotor::setPwm(float Ua, float Ub, float Uc) {
354+
// calculate duty cycle
355+
// limited in [0,1]
356+
float dc_a = (Ua < 0) ? 0 : (Ua >= voltage_power_supply) ? 1 : Ua / voltage_power_supply;
357+
float dc_b = (Ub < 0) ? 0 : (Ub >= voltage_power_supply) ? 1 : Ub / voltage_power_supply;
358+
float dc_c = (Uc < 0) ? 0 : (Uc >= voltage_power_supply) ? 1 : Uc / voltage_power_supply;
359+
// hardware specific writing
360+
_writeDutyCycle(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC );
373361
}
374362

375363
/**
@@ -596,6 +584,4 @@ int BLDCMotor::command(String user_command) {
596584
}
597585
// return 0 if error and 1 if ok
598586
return errorFlag;
599-
}
600-
601-
587+
}

‎src/BLDCMotor.h

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -245,8 +245,15 @@ class BLDCMotor
245245

246246
/** Electrical angle calculation */
247247
float electricAngle(float shaftAngle);
248-
/** Set phase voltaget to pwm output */
249-
void setPwm(int pinPwm, float U);
248+
249+
/**
250+
* Set phase voltages to the harware
251+
*
252+
* @param Ua phase A voltage
253+
* @param Ub phase B voltage
254+
* @param Uc phase C voltage
255+
*/
256+
void setPwm(float Ua, float Ub, float Uc);
250257

251258
// Utility functions
252259
/** normalizing radian angle to [0,2PI] */

‎src/FOCutils.cpp

Lines changed: 148 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,39 +1,156 @@
11
#include "FOCutils.h"
22

3+
#if defined(ESP_H) // if ESP32 board
4+
// empty motor slot
5+
#define _EMPTY_SLOT -20
36

4-
void _setPwmFrequency(int pin) {
5-
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other atmega328p chips
6-
// High PWM frequency
7-
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
8-
if (pin == 5 || pin == 6 || pin == 9 || pin == 10) {
9-
if (pin == 5 || pin == 6) {
10-
// configure the pwm phase-corrected mode
11-
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
12-
// set prescaler to 1
13-
TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
14-
} else {
15-
// set prescaler to 1
16-
TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
7+
// structure containing motor slot configuration
8+
// this library supports up to 4 motors
9+
typedef struct {
10+
int pinA;
11+
mcpwm_dev_t* mcpwm_num;
12+
mcpwm_unit_t mcpwm_unit;
13+
mcpwm_operator_t mcpwm_operator;
14+
mcpwm_io_signals_t mcpwm_a;
15+
mcpwm_io_signals_t mcpwm_b;
16+
mcpwm_io_signals_t mcpwm_c;
17+
18+
} motor_slots_t;
19+
20+
// define motor slots array
21+
motor_slots_t esp32_motor_slots[4] = {
22+
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A
23+
{_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B
24+
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A
25+
{_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B
26+
};
27+
28+
#endif
29+
30+
31+
// function setting the high pwm frequency to the supplied pins
32+
// - hardware speciffic
33+
// supports Arudino/ATmega328, STM32 and ESP32
34+
void _setPwmFrequency(const int pinA, const int pinB, const int pinC) {
35+
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips
36+
// High PWM frequency
37+
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
38+
if (pinA == 5 || pinA == 6 || pinB == 5 || pinB == 6 || pinC == 5 || pinC == 6 ) {
39+
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
40+
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
41+
}
42+
if (pinA == 9 || pinA == 10 || pinB == 9 || pinB == 10 || pinC == 9 || pinC == 10 )
43+
TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1
44+
if (pinA == 3 || pinA == 11 || pinB == 3 || pinB == 11 || pinC == 3 || pinC == 11 )
45+
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1
46+
47+
#elif defined(_STM32_DEF_) // if stm chips
48+
49+
analogWrite(pinA, 0);
50+
analogWriteFrequency(50000); // set 50kHz
51+
analogWrite(pinB, 0);
52+
analogWriteFrequency(50000); // set 50kHz
53+
analogWrite(pinC, 0);
54+
analogWriteFrequency(50000); // set 50kHz
55+
56+
#elif defined(ESP_H) // if esp32 boards
57+
58+
motor_slots_t m_slot = {};
59+
60+
// determine which motor are we connecting
61+
// and set the appropriate configuration parameters
62+
for(int i = 0; i < 4; i++){
63+
if(esp32_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
64+
esp32_motor_slots[i].pinA = pinA;
65+
m_slot = esp32_motor_slots[i];
66+
break;
1767
}
1868
}
19-
else if (pin == 3 || pin == 11) {
20-
// set prescaler to 1
21-
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
69+
70+
// configure pins
71+
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
72+
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
73+
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
74+
75+
mcpwm_config_t pwm_config;
76+
pwm_config.frequency = 4000000; //frequency = 20000Hz
77+
pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 50.0%
78+
pwm_config.cmpr_b = 0; //duty cycle of PWMxB = 50.0%
79+
pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
80+
pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
81+
mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
82+
mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings
83+
mcpwm_init(m_slot.mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings
84+
85+
_delay(100);
86+
87+
mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_0);
88+
mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_1);
89+
mcpwm_stop(m_slot.mcpwm_unit, MCPWM_TIMER_2);
90+
m_slot.mcpwm_num->clk_cfg.prescale = 0;
91+
92+
m_slot.mcpwm_num->timer[0].period.prescale = 4;
93+
m_slot.mcpwm_num->timer[1].period.prescale = 4;
94+
m_slot.mcpwm_num->timer[2].period.prescale = 4;
95+
_delay(1);
96+
m_slot.mcpwm_num->timer[0].period.period = 2048;
97+
m_slot.mcpwm_num->timer[1].period.period = 2048;
98+
m_slot.mcpwm_num->timer[2].period.period = 2048;
99+
_delay(1);
100+
m_slot.mcpwm_num->timer[0].period.upmethod = 0;
101+
m_slot.mcpwm_num->timer[1].period.upmethod = 0;
102+
m_slot.mcpwm_num->timer[2].period.upmethod = 0;
103+
_delay(1);
104+
mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_0);
105+
mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_1);
106+
mcpwm_start(m_slot.mcpwm_unit, MCPWM_TIMER_2);
107+
108+
mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0);
109+
mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0);
110+
mcpwm_sync_enable(m_slot.mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0);
111+
_delay(1);
112+
m_slot.mcpwm_num->timer[0].sync.out_sel = 1;
113+
_delay(1);
114+
m_slot.mcpwm_num->timer[0].sync.out_sel = 0;
115+
#endif
116+
}
117+
118+
// function setting the pwm duty cycle to the hardware
119+
//- hardware speciffic
120+
//
121+
// Arduino and STM32 devices use analogWrite()
122+
// ESP32 uses MCPWM
123+
void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){
124+
#if defined(ESP_H) // if ESP32 boards
125+
// determine which motor slot is the motor connected to
126+
for(int i = 0; i < 4; i++){
127+
if(esp32_motor_slots[i].pinA == pinA){ // if motor slot found
128+
// se the PWM on the slot timers
129+
// transform duty cycle from [0,1] to [0,2047]
130+
esp32_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047;
131+
esp32_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047;
132+
esp32_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047;
133+
break;
134+
}
22135
}
23-
#elif defined(_STM32_DEF_) // if stm chips
24-
analogWrite(pin,0);
25-
analogWriteFrequency(50000); // la valeur par défaut est 20000 Hz
136+
#else // Arduino & STM32 devices
137+
// transform duty cycle from [0,1] to [0,255]
138+
analogWrite(pinA, 255*dc_a);
139+
analogWrite(pinB, 255*dc_b);
140+
analogWrite(pinC, 255*dc_c);
26141
#endif
27142
}
28143

144+
29145
// function buffering delay()
30146
// arduino uno function doesn't work well with interrupts
31147
void _delay(unsigned long ms){
32148
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
33149
// if arduino uno and other atmega328p chips
34-
// return the value based on the prescaler
35-
unsigned long t = _micros();
36-
while(_round((_micros() - t)/1000) < ms){};
150+
// use while instad of delay,
151+
// due to wrong measurement based on changed timer0
152+
unsigned long t = _micros() + ms*1000;
153+
while( _micros() < t ){};
37154
#else
38155
// regular micros
39156
delay(ms);
@@ -56,9 +173,6 @@ unsigned long _micros(){
56173
}
57174

58175

59-
// lookup table for sine calculation in between 0 and 90 degrees
60-
//float sine_array[200] = {0.0000,0.0079,0.0158,0.0237,0.0316,0.0395,0.0473,0.0552,0.0631,0.0710,0.0789,0.0867,0.0946,0.1024,0.1103,0.1181,0.1260,0.1338,0.1416,0.1494,0.1572,0.1650,0.1728,0.1806,0.1883,0.1961,0.2038,0.2115,0.2192,0.2269,0.2346,0.2423,0.2499,0.2575,0.2652,0.2728,0.2804,0.2879,0.2955,0.3030,0.3105,0.3180,0.3255,0.3329,0.3404,0.3478,0.3552,0.3625,0.3699,0.3772,0.3845,0.3918,0.3990,0.4063,0.4135,0.4206,0.4278,0.4349,0.4420,0.4491,0.4561,0.4631,0.4701,0.4770,0.4840,0.4909,0.4977,0.5046,0.5113,0.5181,0.5249,0.5316,0.5382,0.5449,0.5515,0.5580,0.5646,0.5711,0.5775,0.5839,0.5903,0.5967,0.6030,0.6093,0.6155,0.6217,0.6279,0.6340,0.6401,0.6461,0.6521,0.6581,0.6640,0.6699,0.6758,0.6815,0.6873,0.6930,0.6987,0.7043,0.7099,0.7154,0.7209,0.7264,0.7318,0.7371,0.7424,0.7477,0.7529,0.7581,0.7632,0.7683,0.7733,0.7783,0.7832,0.7881,0.7930,0.7977,0.8025,0.8072,0.8118,0.8164,0.8209,0.8254,0.8298,0.8342,0.8385,0.8428,0.8470,0.8512,0.8553,0.8594,0.8634,0.8673,0.8712,0.8751,0.8789,0.8826,0.8863,0.8899,0.8935,0.8970,0.9005,0.9039,0.9072,0.9105,0.9138,0.9169,0.9201,0.9231,0.9261,0.9291,0.9320,0.9348,0.9376,0.9403,0.9429,0.9455,0.9481,0.9506,0.9530,0.9554,0.9577,0.9599,0.9621,0.9642,0.9663,0.9683,0.9702,0.9721,0.9739,0.9757,0.9774,0.9790,0.9806,0.9821,0.9836,0.9850,0.9863,0.9876,0.9888,0.9899,0.9910,0.9920,0.9930,0.9939,0.9947,0.9955,0.9962,0.9969,0.9975,0.9980,0.9985,0.9989,0.9992,0.9995,0.9997,0.9999,1.0000,1.0000};
61-
62176
// int array instead of float array
63177
// 2x storage save (int 2Byte float 4 Byte )
64178
// sin*10000
@@ -72,24 +186,24 @@ int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,110
72186
float _sin(float a){
73187
if(a < _PI_2){
74188
//return sine_array[(int)(199.0*( a / (_PI/2.0)))];
75-
//return sine_array[(int)(126.6873* a)]; // float array optimised
76-
return 0.0001*sine_array[_round(126.6873* a)]; // int array optimised
189+
//return sine_array[(int)(126.6873* a)]; // float array optimized
190+
return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized
77191
}else if(a < _PI){
78192
// return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))];
79-
//return sine_array[398 - (int)(126.6873*a)]; // float array optimised
80-
return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimised
193+
//return sine_array[398 - (int)(126.6873*a)]; // float array optimized
194+
return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized
81195
}else if(a < _3PI_2){
82196
// return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))];
83-
//return -sine_array[-398 + (int)(126.6873*a)]; // float array optimised
84-
return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimised
197+
//return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized
198+
return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized
85199
} else {
86200
// return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))];
87-
//return -sine_array[796 - (int)(126.6873*a)]; // float array optimised
88-
return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimised
201+
//return -sine_array[796 - (int)(126.6873*a)]; // float array optimized
202+
return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized
89203
}
90204
}
91205

92-
// function approfimating cosine calculaiton by using fixed size array
206+
// function approximating cosine calculation by using fixed size array
93207
// ~55us (float array)
94208
// ~56us (int array)
95209
// precision +-0.005

‎src/FOCutils.h

Lines changed: 28 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,15 @@
33

44
#include "Arduino.h"
55

6+
7+
#if defined(ESP_H) // if esp32 boards
8+
9+
#include "driver/mcpwm.h"
10+
#include "soc/mcpwm_reg.h"
11+
#include "soc/mcpwm_struct.h"
12+
13+
#endif
14+
615
// sign function
716
#define sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
817
#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
@@ -24,15 +33,17 @@
2433
* High PWM frequency setting function
2534
* - hardware specific
2635
*
27-
* @param pin pin number to configure
36+
* @param pinA pinA number to configure
37+
* @param pinB pinB number to configure
38+
* @param pinC pinC number to configure
2839
*/
29-
void _setPwmFrequency(int pin);
40+
void _setPwmFrequency(int pinA, int pinB, int pinC);
3041

3142
/**
3243
* Function implementing delay() function in milliseconds
3344
* - blocking function
3445
* - hardware specific
35-
*
46+
3647
* @param ms number of milliseconds to wait
3748
*/
3849
void _delay(unsigned long ms);
@@ -43,6 +54,20 @@ void _delay(unsigned long ms);
4354
*/
4455
unsigned long _micros();
4556

57+
/**
58+
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
59+
* hardware specific
60+
*
61+
* @param dc_a duty cycle phase A [0, 1]
62+
* @param dc_b duty cycle phase B [0, 1]
63+
* @param dc_c duty cycle phase C [0, 1]
64+
* @param pinA phase A hardware pin number
65+
* @param pinB phase B hardware pin number
66+
* @param pinC phase C hardware pin number
67+
*/
68+
void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC );
69+
70+
4671
/**
4772
* Function approximating the sine calculation by using fixed size array
4873
* - execution time ~40us (Arduino UNO)

0 commit comments

Comments
 (0)