avr/nano: added srf sensors
[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 forward MSB
18  * 0x06 Distance forward LSB
19  * 0x07 Distance backward MSB
20  * 0x08 Distance backward LSB
21  *
22  * 0xff Bootloader
23  */
24
25
26 #define TWI_ACK         TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
27 #define TWI_RESET       TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
28 #define TWI_NAK         TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
29
30 static volatile uint8_t ireg=0;
31 static volatile uint8_t bootloader=0;
32 static volatile uint16_t dist_left=0;
33 static volatile uint16_t dist_right=0;
34 static volatile uint16_t dist_forward=0;
35 static volatile uint16_t dist_backward=0;
36 static volatile uint8_t start_dist_fwd=0;
37 static volatile uint8_t start_dist_bwd=0;
38
39 ISR(TWI_vect)
40 {
41         static int16_t tmp16=0;
42
43         switch (TWSR & 0xF8)
44         {  
45                 case 0x60: // start write
46                         TWI_ACK;
47                         ireg = 0;
48                         break;
49                 case 0x80: // write
50                         switch(ireg) {
51                                 case 0x00: // register select
52                                         ireg = TWDR;
53
54                                         if (ireg == 0x05) start_dist_fwd=1;
55                                         if (ireg == 0x07) start_dist_bwd=1;
56
57                                         ireg--; // because we do ireg++ below
58                                         TWI_ACK;
59                                         break;
60                                 case 0xff: // bootloader
61                                         bootloader = TWDR;
62                                 default:
63                                         TWI_NAK;
64                         }
65                         ireg++;
66                         break;
67                 case 0xA8: // start read
68                 case 0xB8: // read
69                         switch(ireg) {
70                                 case 0x01: // Distance left MSB
71                                         tmp16 = dist_left;
72                                         TWDR = tmp16>>8;
73                                         TWI_ACK;
74                                         break;
75                                 case 0x02: // Distance right LSB
76                                         TWDR = tmp16;
77                                         TWI_ACK;
78                                         break;
79                                 case 0x03: // Distance right MSB
80                                         tmp16 = dist_right;
81                                         TWDR = tmp16>>8;
82                                         TWI_ACK;
83                                         break;
84                                 case 0x04: // Distance right LSB
85                                         TWDR = tmp16;
86                                         TWI_ACK;
87                                         break;
88                                 case 0x05: // Distance forward MSB
89                                         tmp16 = dist_forward;
90                                         TWDR = tmp16>>8;
91                                         TWI_ACK;
92                                         break;
93                                 case 0x06: // Distance forward LSB
94                                         TWDR = tmp16;
95                                         TWI_ACK;
96                                         break;
97                                 case 0x07: // Distance backward MSB
98                                         tmp16 = dist_backward;
99                                         TWDR = tmp16>>8;
100                                         TWI_ACK;
101                                         break;
102                                 case 0x08: // Distance backward LSB
103                                         TWDR = tmp16;
104                                         TWI_ACK;
105                                         break;
106                                 default:
107                                         TWDR = 0;
108                                         TWI_NAK;
109                         }
110                         ireg++;
111                         break;
112                 default:
113                         TWI_RESET;
114         }
115 }
116
117
118 uint16_t ReadChannel(uint8_t mux) {
119         uint8_t i;
120         uint16_t result;
121
122         ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler 
123         // setzen auf 8 (1) und ADC aktivieren (1)
124
125         ADMUX = mux;                      // Kanal waehlen
126         ADMUX |= (1<<REFS1) | (1<<REFS0); // interne Referenzspannung nutzen 
127
128         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
129            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
130         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
131         while ( ADCSRA & (1<<ADSC)  ) {
132                      // auf Abschluss der Konvertierung warten 
133         }
134         result = ADCW;  // ADCW muss einmal gelesen werden,
135         // sonst wird Ergebnis der nächsten Wandlung
136         // nicht übernommen.
137
138         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
139         result = 0;
140         for( i=0; i<5; i++ )
141         {
142                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
143                 while ( ADCSRA & (1<<ADSC) ) {
144                            // auf Abschluss der Konvertierung warten
145                 }
146                 result += ADCW;             // Wandlungsergebnisse aufaddieren
147         }
148
149         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
150
151         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
152         
153         return result;
154 }
155
156
157 static unsigned short get_distance(uint8_t i) {
158         return ReadChannel(i);
159 }
160
161
162 ISR(INT0_vect) {
163         static uint16_t t_start=0;
164         uint16_t t_now = TCNT1;
165         uint16_t t_diff;
166
167         if (bit_is_set(PIND, 2)) { // high level
168                 // start timer
169                 t_start = t_now;
170         } else {
171                 t_diff = t_now - t_start;
172                 dist_forward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
173                 // disable this interrupt
174                 EIMSK |= (1 << INT0);
175         }
176 }
177
178
179 ISR(INT1_vect) {
180         static uint16_t t_start=0;
181         uint16_t t_now = TCNT1;
182         uint16_t t_diff;
183
184         if (bit_is_set(PIND, 3)) { // high level
185                 // start timer
186                 t_start = t_now;
187         } else {
188                 t_diff = t_now - t_start;
189                 dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
190                 // disable this interrupt
191                 EIMSK |= (1 << INT1);
192         }
193 }
194
195
196 int main(void) {
197         bootloader = 0x00;
198         setup_uart(9600);
199         uart_setup_stdout();
200
201         // I2C
202         TWAR = 0x52;
203         TWI_RESET;
204
205         // Timer 1: Normal mode, Top: 0xffff, Prescaler: F_CPU/256=62500Hz
206         TCCR1A = 0x0;
207         TCCR1B = (1 << CS12);
208
209         // External Interrupts
210         EICRA = (1 << ISC10) | (1 << ISC00);
211
212         printf("\r\nStart\r\n");
213
214         set_sleep_mode(SLEEP_MODE_IDLE);
215         sei();
216         while(1) {
217                 switch(ireg) {
218                         case 0x01: // ir left
219                                 dist_left = get_distance(0);
220                                 break;
221                         case 0x03: // ir right
222                                 dist_right = get_distance(1);
223                                 break;
224                         case 0xff: // Magic reg that starts the bootloader
225                                 if (bootloader == 0xa5) {
226                                         cli();
227                                         {
228                                                 void (*start)(void) = (void*)0x1800;
229                                                 start();
230                                         }
231                                 }
232                                 break;
233                 }
234
235                 if (start_dist_fwd) {
236                         start_dist_fwd = 0;
237                         dist_forward = 0;
238
239                         DDRD |= (1 << 2);
240                         PORTD |= (1 << 2);
241                         _delay_us(10);
242                         PORTD &= ~(1 << 2);
243                         DDRD &= ~(1 << 2);
244                         // wait for interrupt
245                         EIMSK |= (1 << INT0);
246                 }
247                 if (start_dist_bwd) {
248                         start_dist_bwd = 0;
249                         dist_backward = 0;
250
251                         DDRD |= (1 << 3);
252                         PORTD |= (1 << 3);
253                         _delay_us(10);
254                         PORTD &= ~(1 << 3);
255                         DDRD &= ~(1 << 3);
256                         // wait for interrupt
257                         EIMSK |= (1 << INT1);
258                 }
259
260                 sleep_mode();
261         }
262
263         return 0;
264 }