firmware: style cleanup
Change-Id: Ib8e686356c4e73932c1ef77f4fbea3358e22582b
Signed-off-by: Erik Gilling <konkers@android.com>
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 @@
"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 );
+ 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 );
+ // 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 @@
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 @@
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 @@
}
switch (count++ % 0x10) {
-
case 0:
val = analogRead(TEMP_SENSOR);
msg[0] = 0x4;
@@ -251,7 +246,6 @@
acc.write(msg, 3);
break;
- /* captoutched needs to be asynchonous */
case 0xc:
touchcount = touch_robot.capSense(5);
@@ -268,6 +262,7 @@
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 @@
servos[0].write(90);
digitalWrite(RELAY1, LOW);
digitalWrite(RELAY2, LOW);
-
}
delay(10);
@@ -290,125 +284,111 @@
// ==============================================================================
// Austria Microsystems i2c Joystick
-
-/*
- If a threshold is provided, the dead zone will be programmed such that interrupts will not
- be generated unless the threshold is exceeded.
-
- 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
-*/
-
-
-void init_joystick( int threshold )
+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
+ byte status = 0;
- pinMode( JOY_nRESET, OUTPUT );
+ pinMode(JOY_SWITCH, INPUT);
+ digitalWrite(JOY_SWITCH, HIGH);
- digitalWrite( JOY_nRESET, 1 );
- delay(1);
- digitalWrite( JOY_nRESET, 0 );
- delay(1);
- digitalWrite( JOY_nRESET, 1 );
+ pinMode(JOY_nINT, INPUT);
+ digitalWrite(JOY_nINT, HIGH);
- 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
+ pinMode(JOY_nRESET, OUTPUT);
- 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;
+ 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);
+ // 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);
- read_joy_reg( 0x11 ); // dummy read of Y_reg to reset interrupt
+ // dummy read of Y_reg to reset interrupt
+ read_joy_reg(0x11);
- 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
+ for(iii = 0; iii != 16; iii++) {
+ while(!joystick_interrupt()) {}
- 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
+ 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();
}
