-
Notifications
You must be signed in to change notification settings - Fork 16
/
main.c
338 lines (285 loc) · 10.7 KB
/
main.c
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
// Copyright (c) 2015 Shaun Crampton.
// 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, see <http:https://www.gnu.org/licenses/>.
#include "fixp.h"
#include <stdlib.h>
#include <stdbool.h>
#include <math.h> // Make sure we pick up the optimized AVR math lib.
#include <stdint.h>
#include <stdfix.h>
#include <avr/interrupt.h>
#include "pins.h"
#include "adc.h"
#define TARGET_LOOP_HZ (500)
#define TICKS_PER_LOOP (F_CPU / TARGET_LOOP_HZ)
#define TICK_SECONDS (ACC_LITERAL(1.0) / TARGET_LOOP_HZ)
// Calculate the scaling factor from gyro ADC reading to radians. The gyro is
// ratiometric with Vs but it doesn't use the full range.
#define RADIANS_PER_DEGREE 0.0174532925
#define V_PER_ADC_UNIT (FRACT_LITERAL(0.0048828125l))
#define GYRO_MAX_DEG_PER_SEC (ACC_LITERAL(150.0)) // From gyro datasheet
#define GYRO_V_PER_DEG_PER_SEC (ACC_LITERAL(0.01125)) // Calibrated value from datasheet.
#define GYRO_DEG_PER_ADC_UNIT (ACC_LITERAL(0.434027777)) // V_PER_ADC_UNIT / GYRO_V_PER_DEG_PER_SEC
#define GYRO_RAD_PER_ADC_UNIT (ACC_LITERAL(0.007575213759)) // (GYRO_DEG_PER_ADC_UNIT * RADIANS_PER_DEGREE)
// Calculate the scaling factor from ADC readings to g.
//
// The accelerometer is ratiometric with Vs but it doesn't use the full range
// from GND to Vs. At Vs = 5V, it reads about 1V per g.
#define ACCEL_V_PER_G (ACC_LITERAL(1.0))
#define ACCEL_G_PER_ADC_UNIT (V_PER_ADC_UNIT / ACCEL_V_PER_G)
// EEPROM addresses
#define GYRO_OFFSET_ADDR 0
#define GYRO_OFFSET_SIZE sizeof(accum sat)
#define ACCEL_FACT_ADDR (GYRO_OFFSET_ADDR + GYRO_OFFSET_SIZE)
#define ACCEL_FACT_SIZE sizeof(accum sat)
// Default gyro offset used if no calibration has been done. Unique to each
// gyro.
#define GYRO_OFFSET (ACC_LITERAL(3.982) * GYRO_RAD_PER_ADC_UNIT)
// Allowances for mechanical differences in motors
#define MOTOR_A_FACTOR 1
#define MOTOR_B_FACTOR 1
// Values read from the trim pots.
static uint16_t d_tilt_pot = 512;
static uint16_t x_offset_pot = 512;
static uint16_t gyro_offset_pot = 512;
static accum sat gyro_offset = GYRO_OFFSET;
static accum sat accel_fact = ACC_LITERAL(1);
static bool reset_complete = false;
static accum sat tilt_rads_estimate = ACC_LITERAL(0);
static accum sat tilt_int_rads = ACC_LITERAL(0);
// High water mark for timer values at the end of our ISR.
static long int max_timer = 0;
static accum sat speed = ACC_LITERAL(0);
#define NUM_G_SQ 3
static accum sat total_gs_sq[NUM_G_SQ];
static int gs_idx = 0;
// Counter, used to schedule work that we only do once in a while.
static long int loop_count = 0;
// true if an error has occurred like a timer overflow.
static bool error = false;
#define ABS(x) ((x) < ACC_LITERAL(0) ? -(x) : (x))
// Filter orders.
#define NZEROS 2
#define NPOLES 2
/**
* Low pass filter. Created via
* http:https://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html
*/
static accum sat filterx(accum sat in)
{
static accum sat xv[NZEROS+1], yv[NPOLES+1];
xv[0] = xv[1]; xv[1] = xv[2];
xv[2] = in / ACC_LITERAL(1.058546241e+03);
yv[0] = yv[1]; yv[1] = yv[2];
yv[2] = (xv[0] + xv[2]) + ACC_LITERAL(2) * xv[1]
+ ( -ACC_LITERAL(0.9149758348) * yv[0]) + ( ACC_LITERAL(1.9111970674) * yv[1]);
return yv[2];
}
//static accum sat filtery(accum sat in)
//{
// static accum sat xv[NZEROS+1], yv[NPOLES+1];
// xv[0] = xv[1]; xv[1] = xv[2];
// xv[2] = in / 1.058546241e+03k;
// yv[0] = yv[1]; yv[1] = yv[2];
// yv[2] = (xv[0] + xv[2]) + 2 * xv[1]
// + ( -0.9149758348k * yv[0]) + ( 1.9111970674k * yv[1]);
// return yv[2];
//}
/**
* (Timer 1 == OCR1A) interrupt handler. Called each time timer 1 hits the top
* value we set in OCR1A at start of day.
*
* The timer is automatically reset to 0 after it hits OCR1A.
*/
ISR(TIMER1_COMPA_vect)
{
// First step: read the input values from the ADC. Each ADC conversion takes
// 0.12msec and we have a budget of 1msec per loop. First the gyro...
START_ADC(GYRO_APIN);
uint16_t raw_gyro_value = wait_for_adc_result();
// Then the X pin.
START_ADC(X_APIN);
// While we're waiting for the X value, convert the gyro value to rads.
accum sat gyro_rads_per_sec = GYRO_RAD_PER_ADC_UNIT *
(ACC_LITERAL(512) - (accum sat)raw_gyro_value);
gyro_rads_per_sec -= gyro_offset;
uint16_t raw_x_value = wait_for_adc_result();
// Then the Y pin.
START_ADC(Y_APIN);
// While we're waiting, convert the X value...
accum sat x_gs = ((accum sat)ACCEL_G_PER_ADC_UNIT) *
(raw_x_value - ACC_LITERAL(512));
uint16_t raw_y_value = wait_for_adc_result();
accum sat y_gs = ((accum sat)ACCEL_G_PER_ADC_UNIT) *
(raw_y_value - ACC_LITERAL(512));
// Start the ADC in the background while we do the rest of the processing.
if (loop_count == TARGET_LOOP_HZ / 3) {
START_ADC(TILT_POT_APIN);
} else if (loop_count == (TARGET_LOOP_HZ * 2 / 3)) {
START_ADC(X_POT_APIN);
} else if (loop_count == TARGET_LOOP_HZ) {
START_ADC(GYRO_POT_APIN);
}
// Filter the inputs from the accelerometer to try to reduce the impact of
// vibration and acceleration from any movements the bot is making.
accum sat x_filt_gs = filterx(x_gs) * accel_fact;
// accum sat y_filt_gs = filtery(y_gs) * accel_fact;
// Our constant rotation to apply to the accelerometer. Sets the 0-tilt
// angle.
accum sat accel_rotation = ((accum sat)(x_offset_pot - 512) *
ACC_LITERAL(0.001));
// Estimate the tilt from the accelerometer alone. We'll factor in a fraction
// of this below.
accum sat accel_tilt_estimate = x_filt_gs + accel_rotation;
accum sat gs_sq = (x_gs * x_gs + y_gs * y_gs);
total_gs_sq[gs_idx++] = ABS(ACC_LITERAL(1.0) - gs_sq * accel_fact * accel_fact);
if (gs_idx == NUM_G_SQ)
{
gs_idx = 0;
}
if ((y_gs < ACC_LITERAL(0.1)) && ((x_filt_gs > -ACC_LITERAL(0.6)) || (x_filt_gs < ACC_LITERAL(0.6)))) {
// We fell over! Shut off the motors.
speed = 0;
reset_complete = false;
} else if (!reset_complete) {
// We've never been upright, wait until we're righted by the user.
if (-ACC_LITERAL(0.02) < x_gs && x_gs < ACC_LITERAL(0.02)) {
tilt_rads_estimate = accel_tilt_estimate;
speed = 0;
tilt_int_rads = 0;
reset_complete = true;
}
} else {
// Normal operation!
#define g_factor (ACC_LITERAL(0.0005))
tilt_rads_estimate = (ACC_LITERAL(1.0) - g_factor)
* (tilt_rads_estimate + gyro_rads_per_sec * (accum sat)TICK_SECONDS)
+ g_factor * accel_tilt_estimate;
tilt_int_rads += tilt_rads_estimate * (accum sat)TICK_SECONDS;
#define D_TILT_FACT ACC_LITERAL(16.0)
#define TILT_FACT ACC_LITERAL(400.0)
#define TILT_INT_FACT ACC_LITERAL(100.0)
#define MAX_TILT_INT (0.01)
if (tilt_int_rads > (accum sat)MAX_TILT_INT) {
tilt_int_rads = (accum sat)MAX_TILT_INT;
}
if (tilt_int_rads < -(accum sat)MAX_TILT_INT) {
tilt_int_rads = -(accum sat)MAX_TILT_INT;
}
speed = tilt_rads_estimate * TILT_FACT +
tilt_int_rads * TILT_INT_FACT +
gyro_rads_per_sec * D_TILT_FACT;
if (TCNT1 > max_timer) {
max_timer = TCNT1;
}
}
// Set the motor directions
if (speed < 0) {
WRITE_PIN(DIR_A_PORT, DIR_A_PIN, PIN_OFF);
WRITE_PIN(DIR_B_PORT, DIR_B_PIN, PIN_OFF);
} else {
WRITE_PIN(DIR_A_PORT, DIR_A_PIN, PIN_ON);
WRITE_PIN(DIR_B_PORT, DIR_B_PIN, PIN_ON);
}
// Set the motor speeds.
accum sat abs_speed = ABS(speed);
if (abs_speed > 0 && abs_speed < 10) {
abs_speed *= 10;
} else if (abs_speed < 20) {
abs_speed *= 5;
abs_speed += 90;
} else {
abs_speed += 170;
if (abs_speed > 0xff) {
abs_speed = 0xff;
}
}
uint8_t motor_a_speed = (uint8_t)abs_speed;
uint8_t motor_b_speed = (uint8_t)abs_speed;
PWM_A = motor_a_speed;
PWM_B = motor_b_speed;
if (loop_count == TARGET_LOOP_HZ / 3) {
d_tilt_pot = wait_for_adc_result();
} else if (loop_count == (TARGET_LOOP_HZ * 2 / 3)) {
x_offset_pot = wait_for_adc_result();
} else if (loop_count == TARGET_LOOP_HZ) {
gyro_offset_pot = wait_for_adc_result();
loop_count = 0;
if (error) {
// There's been an error, turn on our status LED permanently.
WRITE_PIN(LED_PORT, LED_PIN, PIN_ON);
} else {
// No error, toggle the status LED once per second as a heartbeat.
TOGGLE_PIN(LED_PORT, LED_PIN, PIN_ON);
}
}
if (bit_is_set(TIFR1, OCF1A))
{
// This loop took too long and the timer wrapped. That means that our
// timing will be incorrect.
error = true;
}
loop_count++;
}
void setup() {
// Set control pins to be outputs.
SET_DIRECTION(PWM_A_PORT, PWM_A_PIN, DIR_OUT);
SET_DIRECTION(PWM_B_PORT, PWM_B_PIN, DIR_OUT);
SET_DIRECTION(DIR_A_PORT, DIR_A_PIN, DIR_OUT);
SET_DIRECTION(DIR_B_PORT, DIR_B_PIN, DIR_OUT);
// Turn on internal pull-up on switch input. Note: turning on the pullup
// uses the PORTxn register, just like writing to an output pin.
SET_DIRECTION(SWITCH_PORT, SWITCH_PIN, DIR_IN);
WRITE_PIN(SWITCH_PORT, SWITCH_PIN, PIN_ON);
// Configure Timer2 for motor PWM outputs.
// Enable PWM on each of the outputs. Set phase-correct mode.
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
TCCR2B = _BV(CS21) | _BV(CS20); // CLK/32 prescaler
// Turn off motors for now.
PWM_A = 0;
PWM_B = 0;
// Configure the Serial port. BlueSMIRF defaults to 115200bps.
// TODO Configure serial
// Read calibration data from EEPROM.
// TODO EEPROM
// We use Timer 1 as our accurate timebase.
// Set it to CTC mode so that we can configure its TOP value in OCR1A.
TCCR1A = 0;
TCCR1B = _BV(WGM12) | _BV(CS10);
TIMSK1 = _BV(OCIE1A); // Enable (Timer 1 == OCR1A) interrupt.
// Have to set TOP value *after* setting up the timer or it is lost.
#if TICKS_PER_LOOP > UINT16_MAX
#error TICKS_PER_LOOP too big
#endif
OCR1A = TICKS_PER_LOOP;
// Configure ADC.
// Use AVcc for reference.
ADMUX |= _BV(REFS0);
// Enable ADC; set CLK/128 prescaler.
ADCSRA |= _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
// Disable digital input on the analog pins.
DIDR0 |= _BV(ADC5D) | _BV(ADC4D) | _BV(ADC3D) |
_BV(ADC2D) | _BV(ADC1D) | _BV(ADC0D);
// Enable interrupts
sei();
}
void loop() {
__asm__("NOP");
}
int main() {
setup();
while (1) {
loop();
}
return 0;
}