-
Notifications
You must be signed in to change notification settings - Fork 0
/
pwm.cpp
355 lines (318 loc) · 10.4 KB
/
pwm.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
/*****************************************************************************
* pwm.cpp
*
* PWM booster manager.
*
* ---------------------------------------------------------------------------
* ardccino - Arduino dual PWM/DCC controller
* (C) 2013-2015 Gerardo García Peña <killabytenow@gmail.com>
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation; either version 3 of the License, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*****************************************************************************/
#include "config.h"
#include "pwm.h"
#include "interrupts.h"
static int get_timer(uint8_t pin)
{
#ifndef SIMULATOR
uint8_t timer = pgm_read_byte_near(digital_pin_to_timer_PGM + pin);
switch(timer) {
case TIMER1A:
case TIMER1B:
timer = (1 << 1);
break;
case TIMER2:
case TIMER2A:
case TIMER2B:
timer = (1 << 2);
break;
#if defined (__AVR_ATmega2560__)
case TIMER3A:
case TIMER3B:
case TIMER3C:
timer = (1 << 3);
break;
#endif
case NOT_ON_TIMER:
cli.fatal(P("get_timer(): Pin %d cannot be used for PWM output."), pin);
default:
cli.fatal(P("get_timer(): Pin %d uses unknown timer (%d)"), pin, timer);
}
return timer;
#else
return 1 << (pin % 2);
#endif
}
static void toggle_timer_pin(uint8_t pin, uint8_t enable)
{
#ifndef SIMULATOR
uint8_t timer = pgm_read_byte_near(digital_pin_to_timer_PGM + pin);
enable = enable ? 0xff : 0;
#define TOGGLE_TIMER_REG(R,S) R = ((R) & ~(3 << ((S)*2))) | (enable & (2 << ((S)*2)))
#define TOGGLE_TIMER_REG_A(R) TOGGLE_TIMER_REG(R,3)
#define TOGGLE_TIMER_REG_B(R) TOGGLE_TIMER_REG(R,2)
#define TOGGLE_TIMER_REG_C(R) TOGGLE_TIMER_REG(R,1)
switch(timer) {
case TIMER1A: TOGGLE_TIMER_REG_A(TCCR1A); break; // OC1A
case TIMER1B: TOGGLE_TIMER_REG_B(TCCR1A); break; // OC1B
#if defined(__AVR_ATmega8__)
case TIMER2: TOGGLE_TIMER_REG_A(TCCR2); break; // OC2
#endif
case TIMER2A: TOGGLE_TIMER_REG_A(TCCR2A); break; // OC2A
case TIMER2B: TOGGLE_TIMER_REG_B(TCCR2A); break; // OC2B
#if defined (__AVR_ATmega2560__)
case TIMER3A: TOGGLE_TIMER_REG_A(TCCR3A); break; // OC3A
case TIMER3B: TOGGLE_TIMER_REG_B(TCCR3A); break; // OC3B
case TIMER3C: TOGGLE_TIMER_REG_C(TCCR3A); break; // OC3C
#endif
case NOT_ON_TIMER:
cli.fatal(P("toggle_timer_pin(): Pin %d cannot be used for PWM output."), pin);
default:
cli.fatal(P("toggle_timer_pin(): Pin %d uses unknown timer (%d)"), pin, timer);
}
#undef TOGGLE_TIMER_REG_A
#undef TOGGLE_TIMER_REG_B
#undef TOGGLE_TIMER_REG_C
#undef TOGGLE_TIMER_REG
#else
enable = pin = enable; // shit! I hate warnings!
#endif
}
static void set_timer_register(uint8_t pin, uint8_t pwm)
{
#ifndef SIMULATOR
uint8_t timer = pgm_read_byte_near(digital_pin_to_timer_PGM + pin);
#ifndef SIMULATOR
switch(timer) {
case TIMER1A: OCR1A = pwm; break;
case TIMER1B: OCR1B = pwm; break;
#if defined(__AVR_ATmega8__)
case TIMER2: OCR2 = pwm; break;
#endif
case TIMER2A: OCR2A = pwm; break;
case TIMER2B: OCR2B = pwm; break;
#if defined (__AVR_ATmega2560__)
case TIMER3A: OCR3A = pwm; break;
case TIMER3B: OCR3B = pwm; break;
case TIMER3C: OCR3C = pwm; break;
#endif
default: cli.fatal(P("Cannot write PWM output to pin %d"), pin);
}
#endif
#else
pwm = pin = pwm; // shit! I hate warnings!
#endif
}
static void booster_activate(uint8_t b)
{
toggle_timer_pin(BoosterMngr::boosters[b].pwmSignalPin, 1);
}
static void booster_deactivate(uint8_t b)
{
toggle_timer_pin(BoosterMngr::boosters[b].pwmSignalPin, 0);
digitalWrite(BoosterMngr::boosters[b].dirSignalPin, 0);
digitalWrite(BoosterMngr::boosters[b].pwmSignalPin, 0);
}
void PwmMngr::init(void)
{
uint8_t timers = 0;
// discover which timers must be configured
for(int b = 0; b < BoosterMngr::nboosters; b++)
timers |= get_timer(BoosterMngr::boosters[b].pwmSignalPin);
// CONFIGURE TIMERS
disable_interrupts();
#ifndef SIMULATOR
if(timers & (1 << 1)) {
// timer 1
TCCR1A = (TCCR1A & 0b00001100)
// 0b00______ Normal port operation, OC1A disconnected
// 0b__00____ Normal port operation, OC1B disconnected
// 0b____**__ Reserved
| 0b00000001; // 0b______01 Fast PWM 8-bit (!WGM13, WGM12, !WGM11, WGM10)
TCCR1B = (TCCR1B & 0b11100000)
// 0b***_____ ICNC1, ICES1, Reserved
| 0b00001000 // 0b___01___ Fast PWM 8-bit (!WGM13, WGM12, !WGM11, WGM10)
| 0x04; // 0b_____100 clock prescaler 0x04 (16e6/256 = 62.5 khz)
TIMSK1 &= 0b11011000; // disable all timer 1 interrupts
// 0b**_**___ Reserved
// 0b__0_____ input capture interrupt enable
// 0b_____0__ OCIE1B: output compare B match interrupt enable
// 0b______0_ OCIE1A: output compare A match interrupt enable
// 0b_______0 TOIE1: overflow interrupt enable
OCR1A = OCR1B = 0;
}
if(timers & (1 << 2)) {
// timer 2
TCCR2A = (TCCR2A & 0b00001100)
// 0b00______ Normal port operation, OC2A disconnected
// 0b__00____ Normal port operation, OC2B disconnected
// 0b____**__ reserved
| 0b00000011; // 0b00000011 Fast PWM (!WGM22, WGM21, WGM20)
TCCR2B = (TCCR2B & 0b11110000)
// 0b****____ FOC2A, FOC2B, Reserved, Reserved
| 0b00000000 // 0b____0___ Fast PWM (!WGM22, WGM21, WGM20)
| 0x6; // 0b_____110 clock prescaler 0x6 (16e6/256 = 62.5 khz)
TIMSK2 &= 0b11111000; // disable all timer 2 interrupts
OCR2A = OCR2B = 0;
}
#if defined (__AVR_ATmega2560__)
if(timers & (1 << 3)) {
// timer 1
TCCR3A = (TCCR3A & 0b00000000)
// 0b00______ Normal port operation, OC3A disconnected
// 0b__00____ Normal port operation, OC3B disconnected
// 0b____00__ Normal port operation, OC3C disconnected
| 0b00000001; // 0b______01 Fast PWM 8-bit (!WGM33, WGM32, !WGM31, WGM30)
TCCR3B = (TCCR3B & 0b11100000)
// 0b***_____ ICNC3, ICES3, Reserved
| 0b00001000 // 0b___01___ Fast PWM 8-bit (!WGM33, WGM32, !WGM31, WGM30)
| 0x04; // 0b_____100 clock prescaler 0x04 (16e6/256 = 62.5 khz)
TIMSK3 &= 0b11010000; // disable all timer 1 interrupts
// 0b**_*____ Reserved
// 0b__0_____ input capture interrupt enable
// 0b____0___ OCIE3C: output compare B match interrupt enable
// 0b_____0__ OCIE3B: output compare B match interrupt enable
// 0b______0_ OCIE3A: output compare A match interrupt enable
// 0b_______0 TOIE3: overflow interrupt enable
OCR3A = OCR3B = OCR3C = 0;
}
#endif
#endif
enable_interrupts();
//-------------------------------------------
// RESET PWM STATUS
for(int b = 0; b < BoosterMngr::nboosters; b++) {
booster_activate(b);
BoosterMngr::boosters[b].reset();
}
}
void PwmMngr::fini(void)
{
for(int b = 0; b < BoosterMngr::nboosters; b++)
booster_deactivate(b);
// disconnect all OC1A:B/OC2A:B pins and go to normal operations mode
disable_interrupts();
#ifndef SIMULATOR
// timer 1
TCCR1A = TCCR1A & 0b00001100; TCCR1B = TCCR1B & 0b11100000;
OCR1A = OCR1B = 0;
TIMSK1 &= 0b11011000;
// timer 2
TCCR2A = TCCR2A & 0b00001100; TCCR2B = TCCR2B & 0b11110000;
OCR2A = OCR2B = 0;
TIMSK2 &= 0b11111000;
#if defined (__AVR_ATmega2560__)
// timer 3
TCCR3A = TCCR3A & 0b00000000; TCCR3B = TCCR3B & 0b11100000;
OCR3A = OCR3B = OCR3C = 0;
TIMSK3 &= 0b11010000;
#endif
enable_interrupts();
#endif
}
void PwmMngr::on(uint8_t b)
{
booster_activate(b);
BoosterMngr::boosters[b].enabled = true;
}
void PwmMngr::off(uint8_t b)
{
BoosterMngr::boosters[b].enabled = false;
BoosterMngr::boosters[b].curr_power = 0;
booster_deactivate(b);
}
void PwmMngr::booster_refresh(Booster *b)
{
if(b->trgt_power == b->curr_power)
return;
if(b->inertial) {
// INERTIAL MODE
int d, trgt_power, acc_inc;
// If target of different sign than current val then brake to zero...
// elsewhere go directly to the target speed
trgt_power = b->curr_power && (b->trgt_power ^ b->curr_power) & ((int) 0x8000)
? 0
: b->trgt_power;
// calculate acceleration increment
acc_inc = trgt_power > b->curr_power
? b->inc_accel
: -b->inc_accel;
if(abs(b->curr_accel) < b->max_accel)
b->curr_accel += acc_inc;
// calculate final speed
if(abs(trgt_power - b->curr_power) < abs(b->curr_accel)) {
d = (trgt_power - b->curr_power) >> 1;
if(!d)
d = trgt_power - b->curr_power;
b->curr_accel = d;
} else {
d = b->curr_accel;
}
b->curr_power += d;
if(b->trgt_power == b->curr_power)
b->curr_accel = 0;
} else {
// DIRECT MODE
b->curr_power = b->trgt_power;
b->curr_accel = 0;
}
//cli.debug(P("booster#%s power %d accel %d"), b->name, b->curr_power, b->curr_accel);
// UPDATE BOOSTER OUTPUT
digitalWrite(b->dirSignalPin, b->enabled && b->curr_power > 0);
unsigned char pwmvalue = b->min_power > abs(b->curr_power)
? b->min_power
: abs(b->curr_power);
set_timer_register(b->pwmSignalPin, pwmvalue);
}
void PwmMngr::refresh(void)
{
for(int b = 0; b < BoosterMngr::nboosters; b++)
booster_refresh(BoosterMngr::boosters + b);
}
void PwmMngr::accelerate(Booster *b, int v)
{
b->trgt_power = v > 0
? (b->trgt_power + v < b->max_power
? b->trgt_power + v
: b->max_power)
: (b->trgt_power + v > -b->max_power
? b->trgt_power + v
: -b->max_power);
}
void PwmMngr::accelerate(int b, int v)
{
if(b < 0 || b > BoosterMngr::nboosters)
cli.fatal(P("pwmAccelerate: booster out of bounds."));
accelerate(BoosterMngr::boosters + b, v);
}
void PwmMngr::speed(int b, int s)
{
if(b < 0 || b > BoosterMngr::nboosters)
cli.fatal(P("pwmSpeed: pwm booster out of bounds."));
if(s < -255 || s > 255)
cli.fatal(P("pwmSpeed: speed out of bounds."));
BoosterMngr::boosters[b].trgt_power = s;
}
void PwmMngr::stop(int b)
{
speed(b, 0);
}
void PwmMngr::switch_dir(int b)
{
if(b < 0 || b > BoosterMngr::nboosters)
cli.fatal(P("switch_dir: pwm out of bounds."));
BoosterMngr::boosters[b].trgt_power = 0 - BoosterMngr::boosters[b].trgt_power;
}