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