From c91d5884056b4a13f7f990aebf62f6c698856fea Mon Sep 17 00:00:00 2001 From: Erik Gilling Date: Fri, 6 May 2011 08:52:22 -0700 Subject: firmware: style cleanup Change-Id: Ib8e686356c4e73932c1ef77f4fbea3358e22582b Signed-off-by: Erik Gilling --- firmware/demokit/demokit.pde | 280 ++++++++++++++++++++----------------------- 1 file changed, 130 insertions(+), 150 deletions(-) (limited to 'firmware/demokit/demokit.pde') diff --git a/firmware/demokit/demokit.pde b/firmware/demokit/demokit.pde index 888aabc..feb112f 100644 --- a/firmware/demokit/demokit.pde +++ b/firmware/demokit/demokit.pde @@ -48,73 +48,76 @@ AndroidAccessory acc("Google, Inc.", "0000000012345678"); Servo servos[3]; -CapSense touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV); // 10M ohm resistor on demo shield +// 10M ohm resistor on demo shield +CapSense touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV); void setup(); void loop(); void init_buttons() { - pinMode( BUTTON1, INPUT ); - pinMode( BUTTON2, INPUT ); - pinMode( BUTTON3, INPUT ); - pinMode( JOY_SWITCH, INPUT ); - - digitalWrite( BUTTON1, HIGH ); // enable the internal pullups - digitalWrite( BUTTON2, HIGH ); - digitalWrite( BUTTON3, HIGH ); - digitalWrite( JOY_SWITCH, HIGH ); + pinMode(BUTTON1, INPUT); + pinMode(BUTTON2, INPUT); + pinMode(BUTTON3, INPUT); + pinMode(JOY_SWITCH, INPUT); + + // enable the internal pullups + digitalWrite(BUTTON1, HIGH); + digitalWrite(BUTTON2, HIGH); + digitalWrite(BUTTON3, HIGH); + digitalWrite(JOY_SWITCH, HIGH); } void init_relays() { - pinMode( RELAY1, OUTPUT ); - pinMode( RELAY2, OUTPUT ); + pinMode(RELAY1, OUTPUT); + pinMode(RELAY2, OUTPUT); } void init_leds() { - digitalWrite( LED1_RED, 1 ); - digitalWrite( LED1_GREEN, 1 ); - digitalWrite( LED1_BLUE, 1 ); + digitalWrite(LED1_RED, 1); + digitalWrite(LED1_GREEN, 1); + digitalWrite(LED1_BLUE, 1); - pinMode( LED1_RED, OUTPUT ); - pinMode( LED1_GREEN, OUTPUT ); - pinMode( LED1_BLUE, OUTPUT ); + pinMode(LED1_RED, OUTPUT); + pinMode(LED1_GREEN, OUTPUT); + pinMode(LED1_BLUE, OUTPUT); - digitalWrite( LED2_RED, 1 ); - digitalWrite( LED2_GREEN, 1 ); - digitalWrite( LED2_BLUE, 1 ); + digitalWrite(LED2_RED, 1); + digitalWrite(LED2_GREEN, 1); + digitalWrite(LED2_BLUE, 1); - pinMode( LED2_RED, OUTPUT ); - pinMode( LED2_GREEN, OUTPUT ); - pinMode( LED2_BLUE, OUTPUT ); + pinMode(LED2_RED, OUTPUT); + pinMode(LED2_GREEN, OUTPUT); + pinMode(LED2_BLUE, OUTPUT); - digitalWrite( LED3_RED, 1 ); - digitalWrite( LED3_GREEN, 1 ); - digitalWrite( LED3_BLUE, 1 ); + digitalWrite(LED3_RED, 1); + digitalWrite(LED3_GREEN, 1); + digitalWrite(LED3_BLUE, 1); - pinMode( LED3_RED, OUTPUT ); - pinMode( LED3_GREEN, OUTPUT ); - pinMode( LED3_BLUE, OUTPUT ); + pinMode(LED3_RED, OUTPUT); + pinMode(LED3_GREEN, OUTPUT); + pinMode(LED3_BLUE, OUTPUT); } -void init_joystick( int threshold ); +void init_joystick(int threshold); byte b1, b2, b3, b4, c; void setup() { - Serial.begin( 115200 ); + Serial.begin(115200); Serial.print("\r\nStart"); init_leds(); init_relays(); init_buttons(); - init_joystick( 5 ); // initialize with thresholding enabled, dead zone of 5 units + init_joystick( 5 ); - touch_robot.set_CS_AutocaL_Millis(0xFFFFFFFF); // autocalibrate OFF + // autocalibrate OFF + touch_robot.set_CS_AutocaL_Millis(0xFFFFFFFF); servos[0].attach(SERVO1); servos[0].write(90); @@ -150,31 +153,26 @@ void loop() char c0; if (len > 0) { - // XXX: assumes only one command per packet - Serial.print(msg[0], HEX); - Serial.print(":"); - Serial.print(msg[1], HEX); - Serial.print(":"); - Serial.println(msg[2], HEX); + // assumes only one command per packet if (msg[0] == 0x2) { if (msg[1] == 0x0) - analogWrite( LED1_RED, 255 - msg[2]); + analogWrite(LED1_RED, 255 - msg[2]); else if (msg[1] == 0x1) - analogWrite( LED1_GREEN, 255 - msg[2]); + analogWrite(LED1_GREEN, 255 - msg[2]); else if (msg[1] == 0x2) - analogWrite( LED1_BLUE, 255 - msg[2]); + analogWrite(LED1_BLUE, 255 - msg[2]); else if (msg[1] == 0x3) - analogWrite( LED2_RED, 255 - msg[2]); + analogWrite(LED2_RED, 255 - msg[2]); else if (msg[1] == 0x4) - analogWrite( LED2_GREEN, 255 - msg[2]); + analogWrite(LED2_GREEN, 255 - msg[2]); else if (msg[1] == 0x5) - analogWrite( LED2_BLUE, 255 - msg[2]); + analogWrite(LED2_BLUE, 255 - msg[2]); else if (msg[1] == 0x6) - analogWrite( LED3_RED, 255 - msg[2]); + analogWrite(LED3_RED, 255 - msg[2]); else if (msg[1] == 0x7) - analogWrite( LED3_GREEN, 255 - msg[2]); + analogWrite(LED3_GREEN, 255 - msg[2]); else if (msg[1] == 0x8) - analogWrite( LED3_BLUE, 255 - msg[2]); + analogWrite(LED3_BLUE, 255 - msg[2]); else if (msg[1] == 0x10) servos[0].write(map(msg[2], 0, 255, 0, 180)); else if (msg[1] == 0x11) @@ -183,12 +181,10 @@ void loop() servos[2].write(map(msg[2], 0, 255, 0, 180)); } else if (msg[0] == 0x3) { if (msg[1] == 0x0) - digitalWrite( RELAY1, msg[2] ? HIGH : LOW ); + digitalWrite(RELAY1, msg[2] ? HIGH : LOW); else if (msg[1] == 0x1) - digitalWrite( RELAY2, msg[2] ? HIGH : LOW ); - + digitalWrite(RELAY2, msg[2] ? HIGH : LOW); } - } msg[0] = 0x1; @@ -226,7 +222,6 @@ void loop() } switch (count++ % 0x10) { - case 0: val = analogRead(TEMP_SENSOR); msg[0] = 0x4; @@ -251,7 +246,6 @@ void loop() acc.write(msg, 3); break; - /* captoutched needs to be asynchonous */ case 0xc: touchcount = touch_robot.capSense(5); @@ -268,6 +262,7 @@ void loop() break; } } else { + // reset outputs to default values on disconnect analogWrite(LED1_RED, 255); analogWrite(LED1_GREEN, 255); analogWrite(LED1_BLUE, 255); @@ -282,7 +277,6 @@ void loop() servos[0].write(90); digitalWrite(RELAY1, LOW); digitalWrite(RELAY2, LOW); - } delay(10); @@ -290,125 +284,111 @@ void loop() // ============================================================================== // Austria Microsystems i2c Joystick +void init_joystick(int threshold) +{ + byte status = 0; -/* - If a threshold is provided, the dead zone will be programmed such that interrupts will not - be generated unless the threshold is exceeded. + pinMode(JOY_SWITCH, INPUT); + digitalWrite(JOY_SWITCH, HIGH); - Note that if you use that mode, you will have to use passage of time with no new interrupts - to detect that the stick has been released and has returned to center. - - If you need to explicitly track return to center, pass 0 as the threshold. "Center" will - still bounce around a little -*/ + pinMode(JOY_nINT, INPUT); + digitalWrite(JOY_nINT, HIGH); + pinMode(JOY_nRESET, OUTPUT); -void init_joystick( int threshold ) -{ - byte status = 0; - - pinMode( JOY_SWITCH, INPUT ); - digitalWrite( JOY_SWITCH, HIGH ); // enable the internal pullup - - pinMode( JOY_nINT, INPUT ); - digitalWrite( JOY_nINT, HIGH ); // enable the internal pullup - - pinMode( JOY_nRESET, OUTPUT ); - - digitalWrite( JOY_nRESET, 1 ); - delay(1); - digitalWrite( JOY_nRESET, 0 ); - delay(1); - digitalWrite( JOY_nRESET, 1 ); - - Wire.begin(); - - do { - status = read_joy_reg( 0x0f ); // XXX need timeout - } while ((status & 0xf0) != 0xf0); - - write_joy_reg( 0x2e, 0x86 ); // invert magnet polarity setting, per datasheet - - calibrate_joystick( threshold ); // calibrate & set up dead zone area + digitalWrite(JOY_nRESET, 1); + delay(1); + digitalWrite(JOY_nRESET, 0); + delay(1); + digitalWrite(JOY_nRESET, 1); + + Wire.begin(); + + do { + status = read_joy_reg(0x0f); + } while ((status & 0xf0) != 0xf0); + + // invert magnet polarity setting, per datasheet + write_joy_reg(0x2e, 0x86); + + calibrate_joystick(threshold); } int offset_X, offset_Y; -void calibrate_joystick( int dz ) +void calibrate_joystick(int dz) { - char iii; - int x_cal = 0; - int y_cal = 0; - - write_joy_reg( 0x0f, 0x00 ); // Low Power Mode, 20ms auto wakeup - // INTn output enabled - // INTn active after each measurement - // Normal (non-Reset) mode - delay(1); - - read_joy_reg( 0x11 ); // dummy read of Y_reg to reset interrupt - - for( iii = 0; iii != 16; iii++ ) { // read coords 16 times & average - while( !joystick_interrupt() ) // poll for interrupt - ; - x_cal += read_joy_reg( 0x10 ); // X pos - y_cal += read_joy_reg( 0x11 ); // Y pos - } - - offset_X = -(x_cal>>4); // divide by 16 to get average - offset_Y = -(y_cal>>4); - - //sprintf(msgbuf, "offsets = %d, %d\n", offset_X, offset_Y); - //Serial.print(msgbuf); - - write_joy_reg( 0x12, dz - offset_X ); // Xp, LEFT threshold for INTn - write_joy_reg( 0x13, -dz - offset_X ); // Xn, RIGHT threshold for INTn - write_joy_reg( 0x14, dz - offset_Y ); // Yp, UP threshold for INTn - write_joy_reg( 0x15, -dz - offset_Y ); // Yn, DOWN threshold for INTn - - if ( dz ) // dead zone threshold detect requested? - write_joy_reg( 0x0f, 0x04 ); // Low Power Mode, 20ms auto wakeup - // INTn output enabled - // INTn active when movement exceeds dead zone - // Normal (non-Reset) mode + char iii; + int x_cal = 0; + int y_cal = 0; + + // Low Power Mode, 20ms auto wakeup + // INTn output enabled + // INTn active after each measurement + // Normal (non-Reset) mode + write_joy_reg(0x0f, 0x00); + delay(1); + + // dummy read of Y_reg to reset interrupt + read_joy_reg(0x11); + + for(iii = 0; iii != 16; iii++) { + while(!joystick_interrupt()) {} + + x_cal += read_joy_reg(0x10); + y_cal += read_joy_reg(0x11); + } + + // divide by 16 to get average + offset_X = -(x_cal>>4); + offset_Y = -(y_cal>>4); + + write_joy_reg(0x12, dz - offset_X); // Xp, LEFT threshold for INTn + write_joy_reg(0x13, -dz - offset_X); // Xn, RIGHT threshold for INTn + write_joy_reg(0x14, dz - offset_Y); // Yp, UP threshold for INTn + write_joy_reg(0x15, -dz - offset_Y); // Yn, DOWN threshold for INTn + + // dead zone threshold detect requested? + if (dz) + write_joy_reg(0x0f, 0x04); } -void read_joystick( int *x, int *y ) +void read_joystick(int *x, int *y) { - *x = read_joy_reg( 0x10 ) + offset_X; - *y = read_joy_reg( 0x11 ) + offset_Y; // reading Y clears the interrupt + *x = read_joy_reg(0x10) + offset_X; + *y = read_joy_reg(0x11) + offset_Y; // reading Y clears the interrupt } char joystick_interrupt() { - return ( digitalRead( JOY_nINT ) == 0 ); + return digitalRead(JOY_nINT) == 0; } #define JOY_I2C_ADDR 0x40 -char read_joy_reg( char reg_addr ) +char read_joy_reg(char reg_addr) { - char c; - - Wire.beginTransmission( JOY_I2C_ADDR ); - Wire.send( reg_addr ); - Wire.endTransmission(); - - Wire.requestFrom( JOY_I2C_ADDR, 1 ); - - while(Wire.available()) - c = Wire.receive(); - - return c; + char c; + + Wire.beginTransmission(JOY_I2C_ADDR); + Wire.send(reg_addr); + Wire.endTransmission(); + + Wire.requestFrom(JOY_I2C_ADDR, 1); + + while(Wire.available()) + c = Wire.receive(); + + return c; } -void write_joy_reg( char reg_addr, char val ) +void write_joy_reg(char reg_addr, char val) { - Wire.beginTransmission( JOY_I2C_ADDR ); - Wire.send( reg_addr ); - Wire.send( val ); - Wire.endTransmission(); + Wire.beginTransmission(JOY_I2C_ADDR); + Wire.send(reg_addr); + Wire.send(val); + Wire.endTransmission(); } -- cgit v1.2.3