/
BLcontroller.h
283 lines (231 loc) · 7.44 KB
/
BLcontroller.h
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
// owen change: needed to make platformio compiler happy
extern void TIMER0_isr_emulation();
#include "definitions.h"
/*
https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
// CS BITS
CS02 CS01 CS00 DESCRIPTION
0 0 0 Timer/Counter0 Disabled
0 0 1 No Prescaling
0 1 0 Clock / 8
0 1 1 Clock / 64
1 0 0 Clock / 256
1 0 1 Clock / 1024
CS12 CS11 CS10 DESCRIPTION
0 0 0 Timer/Counter1 Disabled
0 0 1 No Prescaling
0 1 0 Clock / 8
0 1 1 Clock / 64
1 0 0 Clock / 256
1 0 1 Clock / 1024
CS22 CS21 CS20 DESCRIPTION
0 0 0 Timer/Counter2 Disabled
0 0 1 No Prescaling
0 1 0 Clock / 8
0 1 1 Clock / 32
1 0 0 Clock / 64
1 0 1 Clock / 128
1 1 0 Clock / 256
1 1 1 Clock / 1024
// WAVEFORM GENERATOR BITS
WGM02 WGM01 WGM00 DESCRIPTION TOP
0 0 0 0 Normal 0xFF
1 0 0 1 PWM, Phase Corrected 0xFF
2 0 1 0 CTC OCR0A
3 0 1 1 Fast PWM 0xFF
4 1 0 0 Reserved -
5 1 0 1 Fast PWM, Phase Corr OCR0A
6 1 1 0 Reserved -
7 1 1 1 Fast PWM OCR0A
MODE WGM13 WGM12 WGM11 WGM10 DESCRIPTION TOP
0 0 0 0 0 Normal 0xFFFF
1 0 0 0 1 PWM, Phase Corrected, 8bit 0x00FF
2 0 0 1 0 PWM, Phase Corrected, 9bit 0x01FF
3 0 0 1 1 PWM, Phase Corrected, 10bit 0x03FF
4 0 1 0 0 CTC OCR1A
5 0 1 0 1 Fast PWM, 8bit 0x00FF
6 0 1 1 0 Fast PWM, 9bit 0x01FF
7 0 1 1 1 Fast PWM, 10bit 0x03FF
8 1 0 0 0 PWM, Phase and Frequency Corr ICR1
9 1 0 0 1 PWM, Phase and Frequency Corr OCR1A
10 1 0 1 0 PWM, Phase Correct ICR1
11 1 0 1 1 PWM, Phase Correct OCR1A
12 1 1 0 0 CTC ICR1
13 1 1 0 1 RESERVED
14 1 1 1 0 Fast PWM ICR1
15 1 1 1 1 Fast PWM OCR1A
MODE WGM21 WGM20 DESCRIPTION TOP
0 0 0 Normal 0xFF
1 0 1 PWM Phase Corrected
2 1 0 CTC OCR2
3 1 1 Fast PWM
x = Timer Number
7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0 bit Description
TCCRxA COMxA1 COMxA0 COMxB1 COMxB0 - - WGMx1 WGMx0 Timer/Counter Control Register x A (x=0,2)
TCCR1B ICNC1 ICES1 - WGM13 WGM12 CS12 CS11 CS10
TCCRxB FOCxA FOCxB - - WGMx2 CSx2 CSx1 CSx0 Timer/Counter Control Register x B
TIMSKx - - - - - OCIExB OCIExA TOIEx Timer/Counter Interrupt Mask Register
TIFRx - - - - - OCFxB OCFxA TOVx Timer/Counter Interrupt Flag Register
TCNTx Timer/Counter Register (stores the counter value)
OCRxA Output Compare Register x A
OCRxB Output Compare Register x B
*/
void initBlContmotor2er()
{
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
#ifdef PWM_8KHZ_FAST
TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM01)| _BV(WGM10);
TCCR0B = _BV(CS01);
TCCR1A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM10);
TCCR1B = _BV(WGM12) | _BV(CS11);
TCCR2A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM21)| _BV(WGM20);
TCCR2B = _BV(CS21);
#endif
#ifdef PWM_32KHZ_PHASE
TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00);
TCCR0B = _BV(CS00);
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
TCCR1B = _BV(CS10);
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
TCCR2B = _BV(CS20);
#endif
#ifdef PWM_4KHZ_PHASE
TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00);
TCCR0B = _BV(CS01);
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
TCCR1B = _BV(CS11);
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
TCCR2B = _BV(CS21);
#endif
// enable Timer 1 interrupt
TIMSK1 |= _BV(TOIE1);
// disable arduino standard timer interrupt
TIMSK0 &= ~_BV(TOIE1);
sei();
// Enable Timer1 Interrupt for Motor Control
OCR2A = 0; //11 APIN
OCR2B = 0; //D3
OCR1A = 0; //D9 CPIN
OCR1B = 0; //D10 BPIN
OCR0A = 0; //D6
OCR0B = 0; //D5
}
void MoveMotorPosSpeed(uint8_t motorNumber, int MotorPos, uint16_t maxPWM)
{
uint16_t posStep;
uint16_t pwm_a;
uint16_t pwm_b;
uint16_t pwm_c;
// fetch pwm from sinus table
posStep = MotorPos & 0xff;
pwm_a = pwmSinMotor[(uint8_t)posStep];
pwm_b = pwmSinMotor[(uint8_t)(posStep + 85)];
pwm_c = pwmSinMotor[(uint8_t)(posStep + 170)];
// apply power factor
pwm_a = maxPWM * pwm_a;
pwm_a = pwm_a >> 8;
pwm_a += 128;
pwm_b = maxPWM * pwm_b;
pwm_b = pwm_b >> 8;
pwm_b += 128;
pwm_c = maxPWM * pwm_c;
pwm_c = pwm_c >> 8;
pwm_c += 128;
// set motor pwm variables
if (motorNumber == 0)
{
pwm_a_motor0 = (uint8_t)pwm_a;
pwm_b_motor0 = (uint8_t)pwm_b;
pwm_c_motor0 = (uint8_t)pwm_c;
}
if (motorNumber == 1)
{
pwm_a_motor1 = (uint8_t)pwm_a;
pwm_b_motor1 = (uint8_t)pwm_b;
pwm_c_motor1 = (uint8_t)pwm_c;
}
}
void calcSinusArray()
{
for(int i=0; i<N_SIN; i++)
{
pwmSinMotor[i] = sin(2.0 * i / N_SIN * 3.14159265) * 127.0;
}
}
void initMotorStuff()
{
cli();
calcSinusArray();
sei();
}
/********************************/
/* Motor Control IRQ Routine */
/********************************/
// is called every 31.5us
// minimize interrupt code (20 instructions)
ISR( TIMER1_OVF_vect )
{
freqCounter++;
if(freqCounter==(CC_FACTOR*1000/MOTORUPDATE_FREQ))
{
freqCounter=0;
PWM_A_MOTOR0 = pwm_a_motor0;
PWM_B_MOTOR0 = pwm_b_motor0;
PWM_C_MOTOR0 = pwm_c_motor0;
PWM_A_MOTOR1 = pwm_a_motor1;
PWM_B_MOTOR1 = pwm_b_motor1;
PWM_C_MOTOR1 = pwm_c_motor1;
// update event
motorUpdate = true;
}
// care for standard timers every 1 ms
if ((freqCounter & 0x01f) == 0) {
TIMER0_isr_emulation();
}
}
/**********************************************************/
/* voltage compensation */
/* measure power supply voltage and compensate */
/* motor power accordingly */
/**********************************************************/
void voltageCompensation () {
int uBatValue;
float pwmMotorScale;
// measure uBat, 190 us
uBatValue = analogRead(ADC_VCC_PIN); // 118 us
uBatValue_f = (float)uBatValue * UBAT_ADC_SCALE * UBAT_SCALE;
utilLP_float(&voltageBat, uBatValue_f, LOWPASS_K_FLOAT(0.1)); // tau = 1 sec
if (config.motorPowerScale) {
// calculate scale factor for motor power (70us)
if (voltageBat*100 > config.cutoffVoltage) { // switch off if battery voltage below cutoff
pwmMotorScale = (config.refVoltageBat * 0.01)/voltageBat;
} else {
pwmMotorScale = 0;
}
} else {
pwmMotorScale = 1.0;
}
// 44us
if (fpvModeFreezeMotor1==true) {
maxPWMmotorMotor1Scaled = config.maxPWMfpvMotor1 * pwmMotorScale; // fpv freeze mode
} else {
maxPWMmotorMotor1Scaled = config.maxPWMmotorMotor1 * pwmMotorScale;
}
maxPWMmotorMotor1Scaled = constrain(maxPWMmotorMotor1Scaled, 0, 255);
if (fpvModeFreezeMotor2==true) {
maxPWMmotorMotor2Scaled = config.maxPWMfpvMotor2 * pwmMotorScale; // fpv freeze mode
} else {
maxPWMmotorMotor2Scaled = config.maxPWMmotorMotor2 * pwmMotorScale;
}
maxPWMmotorMotor2Scaled = constrain(maxPWMmotorMotor2Scaled, 0, 255);
}
// switch off motor power
void motorPowerOff() {
MoveMotorPosSpeed(config.motorNumberMotor1, 0, 0);
MoveMotorPosSpeed(config.motorNumberMotor2, 0, 0);
}