avr nano: Fix adc sample rate
[ros_wild_thumper.git] / avr / nano / main.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <limits.h>
4 #include <avr/io.h>
5 #include <avr/interrupt.h>
6 #include <avr/sleep.h>
7 #include <util/delay.h>
8 #include <util/twi.h>
9 #include <avr/eeprom.h>
10 #include <avr/wdt.h>
11 #include <avr/pgmspace.h>
12 #include "uart.h"
13
14 /*
15  * I2C Register Map (8 Bit)
16  * 0x00 Register select
17  * 0x01 Distance left MSB
18  * 0x02 Distance left LSB
19  * 0x03 Distance right MSB
20  * 0x04 Distance right LSB
21  * 0x05 Distance forward1 MSB
22  * 0x06 Distance forward1 LSB
23  * 0x07 Distance backward MSB
24  * 0x08 Distance backward LSB
25  * 0x09 Voltage MSB
26  * 0x0A Voltage LSB
27  * 0x0B Distance forward2 MSB
28  * 0x0C Distance forward2 LSB
29  * 0x0D Current MSB
30  * 0x0E Current LSB
31  *
32  * 0x15 Distance forward1 MSB (read only)
33  * 0x16 Distance forward1 LSB (read only)
34  * 0x17 Distance backward MSB (read only)
35  * 0x18 Distance backward LSB (read only)
36  * 0x19 Distance forward2 MSB (read only)
37  * 0x1A Distance forward2 LSB (read only)
38  *
39  * 0xff Bootloader
40  */
41
42
43 /**
44  * Nano Pinout
45  * A0 PC0/ADC0
46  * A1 PC1/ADC1
47  * A2 PC2/ADC2
48  * A3 PC3/ADC3
49  * A4 PC4/ADC4
50  * A5 PC5/ADC5
51  * A6 ADC6
52  * A7 ADC7
53  * D0 PD0/RX
54  * D1 PD1/TX
55  * D2 PD2/IND0
56  * D3 PD3/IND1
57  * D4 PD4/T0
58  * D5 PD5/T1
59  * D6 PD6/AIN0
60  * D7 PD7/AIN1
61  * D8 PB0/ICP
62  * D9 PB1/OC1
63  * D10 PB2/SS
64  * D11 PB3/MOSI
65  * D12 PB4/MISO
66  * D13 PB5/SCK
67  */
68
69
70 #define TWI_ACK   TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE)
71 #define TWI_NAK   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
72 #define TWI_RESET TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWSTO) | (1<<TWEN) | (1<<TWIE);
73
74 static volatile uint8_t ireg=0;
75 static volatile uint8_t bootloader=0;
76 static volatile uint16_t dist_left=0;
77 static volatile uint16_t dist_right=0;
78 static volatile uint16_t dist_forward1=0;
79 static volatile uint16_t dist_forward2=0;
80 static volatile uint16_t dist_backward=0;
81 static volatile uint8_t start_dist_fwd1=0;
82 static volatile uint8_t start_dist_fwd2=0;
83 static volatile uint8_t start_dist_bwd=0;
84 static volatile uint16_t voltage=0;
85 static volatile uint16_t current=0;
86 static volatile uint8_t pind_pre=0;
87
88 ISR(TWI_vect)
89 {
90         static int16_t tmp16=0;
91
92         switch(TW_STATUS)
93         {  
94                 case TW_SR_SLA_ACK: // start write
95                         TWI_ACK;
96                         ireg = 0;
97                         break;
98                 case TW_SR_DATA_ACK: // write
99                         switch(ireg) {
100                                 case 0x00: // register select
101                                         ireg = TWDR;
102
103                                         if (ireg == 0x05) start_dist_fwd1=1;
104                                         if (ireg == 0x07) start_dist_bwd=1;
105                                         if (ireg == 0x0b) start_dist_fwd2=1;
106
107                                         ireg--; // because we do ireg++ below
108                                         TWI_ACK;
109                                         break;
110                                 case 0xff: // bootloader
111                                         bootloader = TWDR;
112                                 default:
113                                         TWI_NAK;
114                         }
115                         if (ireg < 0xff) ireg++;
116                         break;
117                 case TW_ST_SLA_ACK: // start read
118                 case TW_ST_DATA_ACK: // read
119                         switch(ireg) {
120                                 case 0x01: // Distance left MSB
121                                         tmp16 = dist_left;
122                                         TWDR = tmp16>>8;
123                                         TWI_ACK;
124                                         break;
125                                 case 0x02: // Distance right LSB
126                                         TWDR = tmp16;
127                                         TWI_ACK;
128                                         break;
129                                 case 0x03: // Distance right MSB
130                                         tmp16 = dist_right;
131                                         TWDR = tmp16>>8;
132                                         TWI_ACK;
133                                         break;
134                                 case 0x04: // Distance right LSB
135                                         TWDR = tmp16;
136                                         TWI_ACK;
137                                         break;
138                                 case 0x05: // Distance forward1 MSB
139                                         tmp16 = dist_forward1;
140                                         TWDR = tmp16>>8;
141                                         TWI_ACK;
142                                         break;
143                                 case 0x06: // Distance forward1 LSB
144                                         TWDR = tmp16;
145                                         TWI_ACK;
146                                         break;
147                                 case 0x07: // Distance backward MSB
148                                         tmp16 = dist_backward;
149                                         TWDR = tmp16>>8;
150                                         TWI_ACK;
151                                         break;
152                                 case 0x08: // Distance backward LSB
153                                         TWDR = tmp16;
154                                         TWI_ACK;
155                                         break;
156                                 case 0x09: // Voltage MSB
157                                         tmp16 = voltage;
158                                         TWDR = tmp16>>8;
159                                         TWI_ACK;
160                                         break;
161                                 case 0x0A: // Voltage LSB
162                                         TWDR = tmp16;
163                                         TWI_ACK;
164                                         break;
165                                 case 0x0B: // Distance forward2 MSB
166                                         tmp16 = dist_forward2;
167                                         TWDR = tmp16>>8;
168                                         TWI_ACK;
169                                         break;
170                                 case 0x0C: // Distance forward2 LSB
171                                         TWDR = tmp16;
172                                         TWI_ACK;
173                                         break;
174                                 case 0x0D: // Current MSB
175                                         tmp16 = current;
176                                         TWDR = tmp16>>8;
177                                         TWI_ACK;
178                                         break;
179                                 case 0x0E: // Current LSB
180                                         TWDR = tmp16;
181                                         TWI_ACK;
182                                         break;
183                                 case 0x15: // Distance forward1 MSB
184                                         tmp16 = dist_forward1;
185                                         TWDR = tmp16>>8;
186                                         TWI_ACK;
187                                         break;
188                                 case 0x16: // Distance forward1 LSB
189                                         TWDR = tmp16;
190                                         TWI_ACK;
191                                         break;
192                                 case 0x17: // Distance backward MSB
193                                         tmp16 = dist_backward;
194                                         TWDR = tmp16>>8;
195                                         TWI_ACK;
196                                         break;
197                                 case 0x18: // Distance backward LSB
198                                         TWDR = tmp16;
199                                         TWI_ACK;
200                                         break;
201                                 case 0x19: // Distance forward2 MSB
202                                         tmp16 = dist_forward2;
203                                         TWDR = tmp16>>8;
204                                         TWI_ACK;
205                                         break;
206                                 case 0x1A: // Distance forward2 LSB
207                                         TWDR = tmp16;
208                                         TWI_ACK;
209                                         break;
210                                 default:
211                                         TWDR = 0;
212                                         TWI_NAK;
213                         }
214                         ireg++;
215                         break;
216                 case TW_SR_STOP:
217                         TWI_ACK;
218                         break;
219                 case TW_NO_INFO:
220                         break;
221                 default:
222                         TWI_RESET;
223         }
224 }
225
226
227 uint16_t ReadChannel(uint8_t mux) {
228         uint8_t i;
229         uint16_t result;
230
231         ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler
232         // setzen auf 128 (1) und ADC aktivieren (1)
233         // 16MHz/128=125kHz
234
235         ADMUX = mux;                      // Kanal waehlen
236         ADMUX |= (1<<REFS0);
237
238         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
239            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
240         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
241         while ( ADCSRA & (1<<ADSC)  ) {
242                      // auf Abschluss der Konvertierung warten 
243         }
244         result = ADCW;  // ADCW muss einmal gelesen werden,
245         // sonst wird Ergebnis der nächsten Wandlung
246         // nicht übernommen.
247
248         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
249         result = 0;
250         for( i=0; i<5; i++ )
251         {
252                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
253                 while ( ADCSRA & (1<<ADSC) ) {
254                            // auf Abschluss der Konvertierung warten
255                 }
256                 result += ADCW;             // Wandlungsergebnisse aufaddieren
257         }
258
259         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
260
261         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
262         
263         return result;
264 }
265
266
267 static unsigned short get_distance(uint8_t i) {
268         return ReadChannel(i);
269 }
270
271
272 static unsigned short get_voltage(void) {
273         return ReadChannel(2)*1.46;
274 }
275
276
277 /*
278  * zero offset: 0.5V
279  * sensitivity: 133mV/A
280  */
281 static unsigned short get_current(void) {
282         double volt = ReadChannel(3)*5.0171; // mV
283         return (volt-517.78)/0.12656; // mA
284 }
285
286
287 ISR(INT0_vect) {
288         static uint16_t t_start=0;
289         uint16_t t_now = TCNT1;
290         uint16_t t_diff;
291
292         if (bit_is_set(PIND, 2)) { // high level
293                 // start timer
294                 t_start = t_now;
295         } else {
296                 t_diff = t_now - t_start;
297                 dist_forward1 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
298                 // disable this interrupt
299                 EIMSK &= ~(1 << INT0);
300         }
301 }
302
303
304 ISR(INT1_vect) {
305         static uint16_t t_start=0;
306         uint16_t t_now = TCNT1;
307         uint16_t t_diff;
308
309         if (bit_is_set(PIND, 3)) { // high level
310                 // start timer
311                 t_start = t_now;
312         } else {
313                 t_diff = t_now - t_start;
314                 dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
315                 // disable this interrupt
316                 EIMSK &= ~(1 << INT1);
317         }
318 }
319
320
321 ISR(PCINT2_vect) {
322         uint8_t pind_cur = PIND;
323
324         if ((pind_cur ^ pind_pre) & (1<<4)) { // PCINT20
325                 static uint16_t t_start=0;
326                 uint16_t t_now = TCNT1;
327                 uint16_t t_diff;
328
329                 if (bit_is_set(pind_cur, 4)) { // high level
330                         // start timer
331                         t_start = t_now;
332                 } else {
333                         t_diff = t_now - t_start;
334                         dist_forward2 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
335                         // disable this interrupt
336                         PCMSK2 &= ~(1 << PCINT20);
337                 }
338         }
339
340         pind_pre = pind_cur;
341 }
342
343
344 int main(void) {
345         bootloader = 0x00;
346         setup_uart(9600);
347         uart_setup_stdout();
348
349         // I2C
350         TWAR = 0x52;
351         TWI_ACK;
352
353         // Timer 1: Normal mode, Top: 0xffff, Prescaler: F_CPU/256=62500Hz
354         TCCR1A = 0x0;
355         TCCR1B = (1 << CS12);
356
357         // External Interrupts
358         EICRA = (1 << ISC10) | (1 << ISC00);
359         PCICR = (1 << PCIE2);
360
361         printf_P(PSTR("\r\nStart\r\n"));
362
363         set_sleep_mode(SLEEP_MODE_IDLE);
364         sei();
365         while(1) {
366                 switch(ireg) {
367                         case 0x01: // ir left
368                                 dist_left = get_distance(0);
369                                 break;
370                         case 0x03: // ir right
371                                 dist_right = get_distance(1);
372                                 break;
373                         case 0x09: // voltage
374                                 voltage = get_voltage();
375                                 break;
376                         case 0x0d: // current
377                                 current = get_current();
378                                 break;
379                         case 0xff: // Magic reg that starts the bootloader
380                                 if (bootloader == 0xa5) {
381                                         cli();
382                                         // write mark to first area in eeprom
383                                         eeprom_write_byte((uint8_t*)0, 123);
384                                         eeprom_busy_wait();
385                                         // Use watchdog to restart
386                                         wdt_enable(WDTO_15MS);
387                                 }
388                                 break;
389                 }
390
391                 if (start_dist_fwd1) {
392                         start_dist_fwd1 = 0;
393                         dist_forward1 = 0;
394
395                         DDRD |= (1 << 2);
396                         PORTD |= (1 << 2);
397                         _delay_us(10);
398                         PORTD &= ~(1 << 2);
399                         DDRD &= ~(1 << 2);
400                         // wait for interrupt
401                         EIFR &= (1 << INTF0); // clear old interrupt before enabling
402                         EIMSK |= (1 << INT0);
403                 }
404                 if (start_dist_bwd) {
405                         start_dist_bwd = 0;
406                         dist_backward = 0;
407
408                         DDRD |= (1 << 3);
409                         PORTD |= (1 << 3);
410                         _delay_us(10);
411                         PORTD &= ~(1 << 3);
412                         DDRD &= ~(1 << 3);
413                         // wait for interrupt
414                         EIFR &= (1 << INTF1); // clear old interrupt before enabling
415                         EIMSK |= (1 << INT1);
416                 }
417                 if (start_dist_fwd2) {
418                         start_dist_fwd2 = 0;
419                         dist_forward2 = 0;
420
421                         // PD4 = PCINT20
422                         DDRD |= (1 << 4);
423                         PORTD |= (1 << 4);
424                         _delay_us(10);
425                         PORTD &= ~(1 << 4);
426                         DDRD &= ~(1 << 4);
427                         // wait for interrupt
428                         pind_pre = PIND;
429                         PCMSK2 |= (1 << PCINT20);
430                 }
431
432                 sleep_mode();
433         }
434
435         return 0;
436 }