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