Fix register for sensor fwd2 readout
[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 "uart.h"
9
10 /*
11  * I2C Register Map (8 Bit)
12  * 0x00 Register select
13  * 0x01 Distance left MSB
14  * 0x02 Distance left LSB
15  * 0x03 Distance right MSB
16  * 0x04 Distance right LSB
17  * 0x05 Distance forward1 MSB
18  * 0x06 Distance forward1 LSB
19  * 0x07 Distance backward MSB
20  * 0x08 Distance backward LSB
21  * 0x09 Voltage MSB
22  * 0x0A Voltage LSB
23  * 0x0B Distance forward2 MSB
24  * 0x0C Distance forward2 LSB
25  *
26  * 0x15 Distance forward1 MSB (read only)
27  * 0x16 Distance forward1 LSB (read only)
28  * 0x17 Distance backward MSB (read only)
29  * 0x18 Distance backward LSB (read only)
30  * 0x19 Distance forward2 MSB (read only)
31  * 0x1A Distance forward2 LSB (read only)
32  *
33  * 0xff Bootloader
34  */
35
36
37 #define TWI_ACK         TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
38 #define TWI_RESET       TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
39 #define TWI_NAK         TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
40
41 static volatile uint8_t ireg=0;
42 static volatile uint8_t bootloader=0;
43 static volatile uint16_t dist_left=0;
44 static volatile uint16_t dist_right=0;
45 static volatile uint16_t dist_forward1=0;
46 static volatile uint16_t dist_forward2=0;
47 static volatile uint16_t dist_backward=0;
48 static volatile uint8_t start_dist_fwd1=0;
49 static volatile uint8_t start_dist_fwd2=0;
50 static volatile uint8_t start_dist_bwd=0;
51 static volatile uint16_t voltage=0;
52 static volatile uint8_t pind_pre=0;
53
54 ISR(TWI_vect)
55 {
56         static int16_t tmp16=0;
57
58         switch (TWSR & 0xF8)
59         {  
60                 case 0x60: // start write
61                         TWI_ACK;
62                         ireg = 0;
63                         break;
64                 case 0x80: // write
65                         switch(ireg) {
66                                 case 0x00: // register select
67                                         ireg = TWDR;
68
69                                         if (ireg == 0x05) start_dist_fwd1=1;
70                                         if (ireg == 0x07) start_dist_bwd=1;
71                                         if (ireg == 0x0b) start_dist_fwd2=1;
72
73                                         ireg--; // because we do ireg++ below
74                                         TWI_ACK;
75                                         break;
76                                 case 0xff: // bootloader
77                                         bootloader = TWDR;
78                                 default:
79                                         TWI_NAK;
80                         }
81                         ireg++;
82                         break;
83                 case 0xA8: // start read
84                 case 0xB8: // read
85                         switch(ireg) {
86                                 case 0x01: // Distance left MSB
87                                         tmp16 = dist_left;
88                                         TWDR = tmp16>>8;
89                                         TWI_ACK;
90                                         break;
91                                 case 0x02: // Distance right LSB
92                                         TWDR = tmp16;
93                                         TWI_ACK;
94                                         break;
95                                 case 0x03: // Distance right MSB
96                                         tmp16 = dist_right;
97                                         TWDR = tmp16>>8;
98                                         TWI_ACK;
99                                         break;
100                                 case 0x04: // Distance right LSB
101                                         TWDR = tmp16;
102                                         TWI_ACK;
103                                         break;
104                                 case 0x05: // Distance forward1 MSB
105                                         tmp16 = dist_forward1;
106                                         TWDR = tmp16>>8;
107                                         TWI_ACK;
108                                         break;
109                                 case 0x06: // Distance forward1 LSB
110                                         TWDR = tmp16;
111                                         TWI_ACK;
112                                         break;
113                                 case 0x07: // Distance backward MSB
114                                         tmp16 = dist_backward;
115                                         TWDR = tmp16>>8;
116                                         TWI_ACK;
117                                         break;
118                                 case 0x08: // Distance backward LSB
119                                         TWDR = tmp16;
120                                         TWI_ACK;
121                                         break;
122                                 case 0x09: // Voltage MSB
123                                         tmp16 = voltage;
124                                         TWDR = tmp16>>8;
125                                         TWI_ACK;
126                                         break;
127                                 case 0x0A: // Voltage LSB
128                                         TWDR = tmp16;
129                                         TWI_ACK;
130                                         break;
131                                 case 0x0B: // Distance forward2 MSB
132                                         tmp16 = dist_forward2;
133                                         TWDR = tmp16>>8;
134                                         TWI_ACK;
135                                         break;
136                                 case 0x0C: // Distance forward2 LSB
137                                         TWDR = tmp16;
138                                         TWI_ACK;
139                                         break;
140                                 case 0x15: // Distance forward1 MSB
141                                         tmp16 = dist_forward1;
142                                         TWDR = tmp16>>8;
143                                         TWI_ACK;
144                                         break;
145                                 case 0x16: // Distance forward1 LSB
146                                         TWDR = tmp16;
147                                         TWI_ACK;
148                                         break;
149                                 case 0x17: // Distance backward MSB
150                                         tmp16 = dist_backward;
151                                         TWDR = tmp16>>8;
152                                         TWI_ACK;
153                                         break;
154                                 case 0x18: // Distance backward LSB
155                                         TWDR = tmp16;
156                                         TWI_ACK;
157                                         break;
158                                 case 0x19: // Distance forward2 MSB
159                                         tmp16 = dist_forward2;
160                                         TWDR = tmp16>>8;
161                                         TWI_ACK;
162                                         break;
163                                 case 0x1A: // Distance forward2 LSB
164                                         TWDR = tmp16;
165                                         TWI_ACK;
166                                         break;
167                                 default:
168                                         TWDR = 0;
169                                         TWI_NAK;
170                         }
171                         ireg++;
172                         break;
173                 default:
174                         TWI_RESET;
175         }
176 }
177
178
179 uint16_t ReadChannel(uint8_t mux) {
180         uint8_t i;
181         uint16_t result;
182
183         ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler 
184         // setzen auf 8 (1) und ADC aktivieren (1)
185
186         ADMUX = mux;                      // Kanal waehlen
187         ADMUX |= (1<<REFS0);
188
189         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
190            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
191         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
192         while ( ADCSRA & (1<<ADSC)  ) {
193                      // auf Abschluss der Konvertierung warten 
194         }
195         result = ADCW;  // ADCW muss einmal gelesen werden,
196         // sonst wird Ergebnis der nächsten Wandlung
197         // nicht übernommen.
198
199         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
200         result = 0;
201         for( i=0; i<5; i++ )
202         {
203                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
204                 while ( ADCSRA & (1<<ADSC) ) {
205                            // auf Abschluss der Konvertierung warten
206                 }
207                 result += ADCW;             // Wandlungsergebnisse aufaddieren
208         }
209
210         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
211
212         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
213         
214         return result;
215 }
216
217
218 static unsigned short get_distance(uint8_t i) {
219         return ReadChannel(i);
220 }
221
222
223 static unsigned short get_voltage(void) {
224         return ReadChannel(2)*1.46;
225 }
226
227
228 ISR(INT0_vect) {
229         static uint16_t t_start=0;
230         uint16_t t_now = TCNT1;
231         uint16_t t_diff;
232
233         if (bit_is_set(PIND, 2)) { // high level
234                 // start timer
235                 t_start = t_now;
236         } else {
237                 t_diff = t_now - t_start;
238                 dist_forward1 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
239                 // disable this interrupt
240                 EIMSK &= ~(1 << INT0);
241         }
242 }
243
244
245 ISR(INT1_vect) {
246         static uint16_t t_start=0;
247         uint16_t t_now = TCNT1;
248         uint16_t t_diff;
249
250         if (bit_is_set(PIND, 3)) { // high level
251                 // start timer
252                 t_start = t_now;
253         } else {
254                 t_diff = t_now - t_start;
255                 dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
256                 // disable this interrupt
257                 EIMSK &= ~(1 << INT1);
258         }
259 }
260
261
262 ISR(PCINT2_vect) {
263         uint8_t pind_cur = PIND;
264
265         if ((pind_cur ^ pind_pre) & (1<<4)) { // PCINT20
266                 static uint16_t t_start=0;
267                 uint16_t t_now = TCNT1;
268                 uint16_t t_diff;
269
270                 if (bit_is_set(pind_cur, 4)) { // high level
271                         // start timer
272                         t_start = t_now;
273                 } else {
274                         t_diff = t_now - t_start;
275                         dist_forward2 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
276                         // disable this interrupt
277                         PCMSK2 &= ~(1 << PCINT20);
278                 }
279         }
280
281         pind_pre = pind_cur;
282 }
283
284
285 int main(void) {
286         bootloader = 0x00;
287         setup_uart(9600);
288         uart_setup_stdout();
289
290         // I2C
291         TWAR = 0x52;
292         TWI_RESET;
293
294         // Timer 1: Normal mode, Top: 0xffff, Prescaler: F_CPU/256=62500Hz
295         TCCR1A = 0x0;
296         TCCR1B = (1 << CS12);
297
298         // External Interrupts
299         EICRA = (1 << ISC10) | (1 << ISC00);
300         PCICR = (1 << PCIE2);
301
302         printf("\r\nStart\r\n");
303
304         set_sleep_mode(SLEEP_MODE_IDLE);
305         sei();
306         while(1) {
307                 switch(ireg) {
308                         case 0x01: // ir left
309                                 dist_left = get_distance(0);
310                                 break;
311                         case 0x03: // ir right
312                                 dist_right = get_distance(1);
313                                 break;
314                         case 0x09: // voltage
315                                 voltage = get_voltage();
316                                 break;
317                         case 0xff: // Magic reg that starts the bootloader
318                                 if (bootloader == 0xa5) {
319                                         cli();
320                                         {
321                                                 void (*start)(void) = (void*)0x1800;
322                                                 start();
323                                         }
324                                 }
325                                 break;
326                 }
327
328                 if (start_dist_fwd1) {
329                         start_dist_fwd1 = 0;
330                         dist_forward1 = 0;
331
332                         DDRD |= (1 << 2);
333                         PORTD |= (1 << 2);
334                         _delay_us(10);
335                         PORTD &= ~(1 << 2);
336                         DDRD &= ~(1 << 2);
337                         // wait for interrupt
338                         EIFR &= (1 << INTF0); // clear old interrupt before enabling
339                         EIMSK |= (1 << INT0);
340                 }
341                 if (start_dist_bwd) {
342                         start_dist_bwd = 0;
343                         dist_backward = 0;
344
345                         DDRD |= (1 << 3);
346                         PORTD |= (1 << 3);
347                         _delay_us(10);
348                         PORTD &= ~(1 << 3);
349                         DDRD &= ~(1 << 3);
350                         // wait for interrupt
351                         EIFR &= (1 << INTF1); // clear old interrupt before enabling
352                         EIMSK |= (1 << INT1);
353                 }
354                 if (start_dist_fwd2) {
355                         start_dist_fwd2 = 0;
356                         dist_forward2 = 0;
357
358                         // PD4 = PCINT20
359                         DDRD |= (1 << 4);
360                         PORTD |= (1 << 4);
361                         _delay_us(10);
362                         PORTD &= ~(1 << 4);
363                         DDRD &= ~(1 << 4);
364                         // wait for interrupt
365                         pind_pre = PIND;
366                         PCMSK2 |= (1 << PCINT20);
367                 }
368
369                 sleep_mode();
370         }
371
372         return 0;
373 }