Skip to content

Commit 7710b6d

Browse files
askuricaskuric
committed
added phase off in 6pwm mode + atmega328 alignment of timers #71 + varaible frequency either 4kHz or 32kHz #97 328/2560
1 parent 5ac6a7d commit 7710b6d

File tree

2 files changed

+226
-110
lines changed

2 files changed

+226
-110
lines changed

‎src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp

Lines changed: 124 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -4,50 +4,65 @@
44

55
// set pwm frequency to 32KHz
66
void _pinHighFrequency(const int pin){
7+
bool high_fq = false;
8+
// set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
9+
// else set the 4kHz
10+
if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true;
711
// High PWM frequency
8-
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
12+
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-ATmega2560
913
// https://forum.arduino.cc/index.php?topic=72092.0
1014
if (pin == 13 || pin == 4 ) {
1115
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
12-
TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
16+
if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
17+
else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
1318
}
1419
else if (pin == 12 || pin == 11 )
15-
TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1
20+
if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
21+
else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
1622
else if (pin == 10 || pin == 9 )
17-
TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1
23+
if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
24+
else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
1825
else if (pin == 5 || pin == 3 || pin == 2)
19-
TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1
26+
if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
27+
else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
2028
else if (pin == 8 || pin == 7 || pin == 6)
21-
TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1
29+
if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
30+
else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
2231
else if (pin == 44 || pin == 45 || pin == 46)
23-
TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1
32+
if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
33+
else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
2434

2535
}
2636

2737

2838
// function setting the high pwm frequency to the supplied pins
2939
// - Stepper motor - 2PWM setting
30-
// - hardware speciffic
40+
// - hardware specific
41+
// supports Arduino/ATmega2560
3142
void* _configure1PWM(long pwm_frequency,const int pinA) {
43+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
44+
else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
3245
// High PWM frequency
3346
// - always max 32kHz
34-
_pinHighFrequency(pinA);
47+
_pinHighFrequency(pinA, pwm_frequency);
3548
GenericDriverParams* params = new GenericDriverParams {
3649
.pins = { pinA },
3750
.pwm_frequency = pwm_frequency
3851
};
3952
return params;
4053
}
4154

42-
4355
// function setting the high pwm frequency to the supplied pins
4456
// - Stepper motor - 2PWM setting
45-
// - hardware speciffic
46-
void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
57+
// - hardware specific
58+
// supports Arduino/ATmega2560
59+
void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
60+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
61+
else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
4762
// High PWM frequency
4863
// - always max 32kHz
49-
_pinHighFrequency(pinA);
50-
_pinHighFrequency(pinB);
64+
_pinHighFrequency(pinA, pwm_frequency);
65+
_pinHighFrequency(pinB, pwm_frequency);
5166
GenericDriverParams* params = new GenericDriverParams {
5267
.pins = { pinA, pinB },
5368
.pwm_frequency = pwm_frequency
@@ -57,31 +72,35 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
5772

5873
// function setting the high pwm frequency to the supplied pins
5974
// - BLDC motor - 3PWM setting
60-
// - hardware speciffic
75+
// - hardware specific
76+
// supports Arduino/ATmega2560
6177
void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
78+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
79+
else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
6280
// High PWM frequency
6381
// - always max 32kHz
64-
_pinHighFrequency(pinA);
65-
_pinHighFrequency(pinB);
66-
_pinHighFrequency(pinC);
82+
_pinHighFrequency(pinA, pwm_frequency);
83+
_pinHighFrequency(pinB, pwm_frequency);
84+
_pinHighFrequency(pinC, pwm_frequency);
6785
GenericDriverParams* params = new GenericDriverParams {
6886
.pins = { pinA, pinB, pinC },
6987
.pwm_frequency = pwm_frequency
7088
};
89+
// _syncAllTimers();
7190
return params;
7291
}
7392

7493
// function setting the pwm duty cycle to the hardware
7594
// - Stepper motor - 2PWM setting
76-
// - hardware speciffic
95+
// - hardware specific
7796
void _writeDutyCycle1PWM(float dc_a, void* params){
7897
// transform duty cycle from [0,1] to [0,255]
7998
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
8099
}
81100

82101
// function setting the pwm duty cycle to the hardware
83102
// - Stepper motor - 2PWM setting
84-
// - hardware speciffic
103+
// - hardware specific
85104
void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){
86105
// transform duty cycle from [0,1] to [0,255]
87106
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
@@ -90,7 +109,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){
90109

91110
// function setting the pwm duty cycle to the hardware
92111
// - BLDC motor - 3PWM setting
93-
// - hardware speciffic
112+
// - hardware specific
94113
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){
95114
// transform duty cycle from [0,1] to [0,255]
96115
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
@@ -100,14 +119,16 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){
100119

101120
// function setting the high pwm frequency to the supplied pins
102121
// - Stepper motor - 4PWM setting
103-
// - hardware speciffic
122+
// - hardware specific
104123
void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
124+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
125+
else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
105126
// High PWM frequency
106127
// - always max 32kHz
107-
_pinHighFrequency(pin1A);
108-
_pinHighFrequency(pin1B);
109-
_pinHighFrequency(pin2A);
110-
_pinHighFrequency(pin2B);
128+
_pinHighFrequency(pin1A,pwm_frequency);
129+
_pinHighFrequency(pin1B,pwm_frequency);
130+
_pinHighFrequency(pin2A,pwm_frequency);
131+
_pinHighFrequency(pin2B,pwm_frequency);
111132
GenericDriverParams* params = new GenericDriverParams {
112133
.pins = { pin1A, pin1B, pin2A, pin2B },
113134
.pwm_frequency = pwm_frequency
@@ -117,7 +138,7 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const
117138

118139
// function setting the pwm duty cycle to the hardware
119140
// - Stepper motor - 4PWM setting
120-
// - hardware speciffic
141+
// - hardware specific
121142
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params) {
122143
// transform duty cycle from [0,1] to [0,255]
123144
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
@@ -128,45 +149,79 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo
128149

129150

130151
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
131-
// supports Arudino/ATmega2560
152+
// supports Arduino/ATmega2560
132153
// https://ww1.microchip.com/downloads/en/devicedoc/atmel-2549-8-bit-avr-microcontroller-atmega640-1280-1281-2560-2561_datasheet.pdf
133154
// https://docs.arduino.cc/hacking/hardware/PinMapping2560
134-
int _configureComplementaryPair(int pinH, int pinL) {
155+
int _configureComplementaryPair(const int pinH,const int pinL, long frequency) {
156+
bool high_fq = false;
157+
// set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
158+
// else set the 4kHz
159+
if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN)) high_fq=true;
160+
// High PWM frequency
161+
// https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-ATmega2560
162+
// https://forum.arduino.cc/index.php?topic=72092.0
163+
if (pin == 13 || pin == 4 ) {
164+
TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
165+
if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
166+
else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
167+
}
168+
else if (pin == 12 || pin == 11 )
169+
if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
170+
else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
171+
else if (pin == 10 || pin == 9 )
172+
if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
173+
else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
174+
else if (pin == 5 || pin == 3 || pin == 2)
175+
if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
176+
else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
177+
else if (pin == 8 || pin == 7 || pin == 6)
178+
if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
179+
else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
180+
else if (pin == 44 || pin == 45 || pin == 46)
181+
if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
182+
else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
183+
135184
if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){
136185
// configure the pwm phase-corrected mode
137186
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
138187
// configure complementary pwm on low side
139188
if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
140189
else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
141-
// set prescaler to 1 - 32kHz freq
142-
TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
190+
// set frequency
191+
if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
192+
else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
143193
}else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){
144-
// set prescaler to 1 - 32kHz freq
145-
TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
194+
// set frequency
195+
if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
196+
else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
146197
// configure complementary pwm on low side
147198
if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
148199
else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
149200
}else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){
150-
// set prescaler to 1 - 32kHz freq
151-
TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
201+
// set frequency
202+
if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
203+
else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
152204
// configure complementary pwm on low side
153205
if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
154206
else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
155207
}else if((pinH == 5 && pinL == 2 ) || (pinH == 2 && pinL == 5 ) ){
156-
// set prescaler to 1 - 32kHz freq
157-
TCCR3B = ((TCCR3B & 0b11111000) | 0x01);
208+
// set frequency
209+
if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
210+
else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
158211
// configure complementary pwm on low side
159212
if(pinH == 5 ) TCCR3A = 0b10110000 | (TCCR3A & 0b00001111) ;
160213
else TCCR3A = 0b11100000 | (TCCR3A & 0b00001111) ;
161214
}else if((pinH == 6 && pinL == 7 ) || (pinH == 7 && pinL == 6 ) ){
162-
// set prescaler to 1 - 32kHz freq
163-
TCCR4B = ((TCCR4B & 0b11111000) | 0x01);
215+
// set frequency
216+
if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
217+
else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
164218
// configure complementary pwm on low side
165219
if(pinH == 6 ) TCCR4A = 0b10110000 | (TCCR4A & 0b00001111) ;
166220
else TCCR4A = 0b11100000 | (TCCR4A & 0b00001111) ;
167221
}else if((pinH == 46 && pinL == 45 ) || (pinH == 45 && pinL == 46 ) ){
168-
// set prescaler to 1 - 32kHz freq
169-
TCCR5B = ((TCCR5B & 0b11111000) | 0x01);
222+
// set frequency
223+
if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
224+
else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
170225
// configure complementary pwm on low side
171226
if(pinH == 46 ) TCCR5A = 0b10110000 | (TCCR5A & 0b00001111) ;
172227
else TCCR5A = 0b11100000 | (TCCR5A & 0b00001111) ;
@@ -177,48 +232,59 @@ int _configureComplementaryPair(int pinH, int pinL) {
177232
}
178233

179234
// Configuring PWM frequency, resolution and alignment
180-
// - BLDC driver - 6PWM setting
235+
// - BLDC driver - setting
181236
// - hardware specific
182-
// supports Arudino/ATmega328
237+
// supports Arduino/ATmega2560
183238
void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
239+
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
240+
else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
184241
// High PWM frequency
185242
// - always max 32kHz
186243
int ret_flag = 0;
187-
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
188-
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
189-
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
244+
ret_flag += _configureComplementaryPair(pinA_h, pinA_l, pwm_frequency);
245+
ret_flag += _configureComplementaryPair(pinB_h, pinB_l, pwm_frequency);
246+
ret_flag += _configureComplementaryPair(pinC_h, pinC_l, pwm_frequency);
190247
if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED;
191248
GenericDriverParams* params = new GenericDriverParams {
192249
.pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l },
193250
.pwm_frequency = pwm_frequency,
194251
.dead_zone = dead_zone
195252
};
253+
// _syncAllTimers();
196254
return params;
197255
}
198256

199257
// function setting the
200-
void _setPwmPair(int pinH, int pinL, float val, int dead_time)
258+
void _setPwmPair(int pinH, int pinL, float val, int dead_time, PhaseState ps)
201259
{
202260
int pwm_h = _constrain(val-dead_time/2,0,255);
203261
int pwm_l = _constrain(val+dead_time/2,0,255);
262+
// determine the phase state and set the pwm accordingly
263+
// deactivate phases if needed
264+
if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){
265+
digitalWrite(pinH, LOW);
266+
}else{
267+
analogWrite(pinH, pwm_h);
268+
}
269+
if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){
270+
digitalWrite(pinL, LOW);
271+
}else{
272+
if(pwm_l == 255 || pwm_l == 0)
273+
digitalWrite(pinL, pwm_l ? LOW : HIGH);
274+
else
275+
analogWrite(pinL, pwm_l);
276+
}
204277

205-
analogWrite(pinH, pwm_h);
206-
if(pwm_l == 255 || pwm_l == 0)
207-
digitalWrite(pinL, pwm_l ? LOW : HIGH);
208-
else
209-
analogWrite(pinL, pwm_l);
210278
}
211279

212280
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
213281
// - BLDC driver - 6PWM setting
214282
// - hardware specific
215-
// supports Arudino/ATmega328
283+
// supports Arduino/ATmega328
216284
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){
217-
_setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
218-
_setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
219-
_setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
220-
221-
_UNUSED(phase_state);
285+
_setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[0]);
286+
_setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[1]);
287+
_setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[2]);
222288
}
223289

224290
#endif

0 commit comments

Comments
 (0)