added initial avr/nano program
[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 "uart.h"
8
9 /*
10  * I2C Register Map (8 Bit)
11  * 0x00 Register select
12  * 0x01 Distance left MSB
13  * 0x02 Distance left LSB
14  * 0x03 Distance right MSB
15  * 0x04 Distance right LSB
16  *
17  * 0xff Bootloader
18  */
19
20
21 #define TWI_ACK         TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
22 #define TWI_RESET       TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
23 #define TWI_NAK         TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
24
25 static volatile uint8_t ireg=0;
26 static volatile uint8_t bootloader=0;
27 static volatile uint16_t dist_left=0;
28 static volatile uint16_t dist_right=0;
29
30 ISR(TWI_vect)
31 {
32         static int16_t tmp16=0;
33
34         switch (TWSR & 0xF8)
35         {  
36                 case 0x60: // start write
37                         TWI_ACK;
38                         ireg = 0;
39                         break;
40                 case 0x80: // write
41                         switch(ireg) {
42                                 case 0x00: // register select
43                                         ireg = TWDR;
44                                         ireg--; // because we do ireg++ below
45                                         TWI_ACK;
46                                         break;
47                                 case 0xff: // bootloader
48                                         bootloader = TWDR;
49                                 default:
50                                         TWI_NAK;
51                         }
52                         ireg++;
53                         break;
54                 case 0xA8: // start read
55                 case 0xB8: // read
56                         switch(ireg) {
57                                 case 0x01: // Distance left MSB
58                                         tmp16 = dist_left;
59                                         TWDR = tmp16>>8;
60                                         TWI_ACK;
61                                         break;
62                                 case 0x02: // Distance right LSB
63                                         TWDR = tmp16;
64                                         TWI_ACK;
65                                         break;
66                                 case 0x03: // Distance right MSB
67                                         tmp16 = dist_right;
68                                         TWDR = tmp16>>8;
69                                         TWI_ACK;
70                                         break;
71                                 case 0x04: // Distance right LSB
72                                         TWDR = tmp16;
73                                         TWI_ACK;
74                                         break;
75                                 default:
76                                         TWDR = 0;
77                                         TWI_NAK;
78                         }
79                         ireg++;
80                         break;
81                 default:
82                         TWI_RESET;
83         }
84 }
85
86
87 uint16_t ReadChannel(uint8_t mux) {
88         uint8_t i;
89         uint16_t result;
90
91         ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler 
92         // setzen auf 8 (1) und ADC aktivieren (1)
93
94         ADMUX = mux;                      // Kanal waehlen
95         ADMUX |= (1<<REFS1) | (1<<REFS0); // interne Referenzspannung nutzen 
96
97         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
98            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
99         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
100         while ( ADCSRA & (1<<ADSC)  ) {
101                      // auf Abschluss der Konvertierung warten 
102         }
103         result = ADCW;  // ADCW muss einmal gelesen werden,
104         // sonst wird Ergebnis der nächsten Wandlung
105         // nicht Ã¼bernommen.
106
107         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
108         result = 0;
109         for( i=0; i<5; i++ )
110         {
111                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
112                 while ( ADCSRA & (1<<ADSC) ) {
113                            // auf Abschluss der Konvertierung warten
114                 }
115                 result += ADCW;             // Wandlungsergebnisse aufaddieren
116         }
117
118         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
119
120         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
121         
122         return result;
123 }
124
125
126 static unsigned short get_distance(uint8_t i) {
127         return ReadChannel(i);
128 }
129
130
131 int main(void) {
132         bootloader = 0x00;
133         setup_uart(9600);
134         uart_setup_stdout();
135
136         // I2C
137         TWAR = 0x52;
138         TWI_RESET;
139
140         printf("\r\nStart\r\n");
141
142         set_sleep_mode(SLEEP_MODE_IDLE);
143         sei();
144         while(1) {
145                 switch(ireg) {
146                         case 1: // ir left
147                                 dist_left = get_distance(0);
148                                 break;
149                         case 3: // ir right
150                                 dist_right = get_distance(1);
151                                 break;
152                         case 0xff: // Magic reg that starts the bootloader
153                                 if (bootloader == 0xa5) {
154                                         cli();
155                                         {
156                                                 void (*start)(void) = (void*)0x1800;
157                                                 start();
158                                         }
159                                 }
160                                 break;
161                 }
162
163                 sleep_mode();
164         }
165
166         return 0;
167 }