avr nano: Added Manipulator 12V enable/disable

[ros_wild_thumper.git] / avr / nano / main.c

diff --git a/avr/nano/main.c b/avr/nano/main.c
index 935ce4994d4ae418a1333a13aa44045210e1518b..07e1871420dcbc433671656420ac30f6182d8189 100644 (file)
--- a/avr/nano/main.c
+++ b/avr/nano/main.c
@@ -28,6 +28,7 @@
  * 0x0C Distance forward2 LSB
  * 0x0D Current MSB
  * 0x0E Current LSB
+ * 0x0F Manipulator 12V
  *
  * 0x15 Distance forward1 MSB (read only)
  * 0x16 Distance forward1 LSB (read only)
@@ -52,10 +53,10 @@
  * A7 ADC7
  * D0 PD0/RX
  * D1 PD1/TX
- * D2 PD2/IND0
- * D3 PD3/IND1
- * D4 PD4/T0
- * D5 PD5/T1
+ * D2 PD2/IND0 dist_fwd1
+ * D3 PD3/IND1 dist_bwd
+ * D4 PD4/T0 dist_fwd2
+ * D5 PD5/T1 Manipulator 12V
  * D6 PD6/AIN0
  * D7 PD7/AIN1
  * D8 PB0/ICP
@@ -101,12 +102,17 @@ ISR(TWI_vect)
                                        ireg = TWDR;
 
                                        if (ireg == 0x05) start_dist_fwd1=1;
-                                       if (ireg == 0x07) start_dist_bwd=1;
-                                       if (ireg == 0x0b) start_dist_fwd2=1;
+                                       else if (ireg == 0x07) start_dist_bwd=1;
+                                       else if (ireg == 0x0b) start_dist_fwd2=1;
 
                                        ireg--; // because we do ireg++ below
                                        TWI_ACK;
                                        break;
+                               case 0x0f: // Manipulator 12V
+                                       if (TWDR) PORTD |= (1 << 5);
+                                       else PORTD &= ~(1 << 5);
+                                       TWI_ACK;
+                                       break;
                                case 0xff: // bootloader
                                        bootloader = TWDR;
                                default:
@@ -346,6 +352,9 @@ int main(void) {
        setup_uart(9600);
        uart_setup_stdout();
 
+       DDRD = (1 << 5);
+       PORTD = 0x0;
+
        // I2C
        TWAR = 0x52;
        TWI_ACK;