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