/* File: Land_Rover_Defender_Wiichuck_3
 * Arduino motor shield with ultrasonic ranging sensor and Wii nunchuck
 *
 * Change Log:
 * v1 - Initial version
 * v2 - Changed robot_distance_sensor() to resolve periodic lack of
 *      responsiveness. The function now ignores any distance further than 25
 *      inches away by adding a 3.6 ms timeout to the pulseIn() call.
 * v3 - Added buzzer on pin 6. Control with Wii nunchuck Z button.
 *
 * Hardware:
 * - Arduino Uno Rev 3 - http://arduino.cc/en/Main/ArduinoBoardUno
 * - Arduino Motor Shield R3 - http://arduino.cc/en/Main/ArduinoMotorShieldR3
 * - 6V DC motor is driving the rear axle
 * - Micro servo (5V) is steering the car
 * - HC-SR04 ultrasonic ranging sensor
 * - Wii nunchuck provides control
 * - Buzzer (3V, two wires) provides horn.
 *
 * Configuration and Pins:
 * - The battery is connected to the Vin and Gnd on the motor shield screw
 *   terminal block with an on/off switch
 * - The 6V drive motor has two wires, one yellow and one blue. The blue wire
 *   is on (+) for channel A, and the yellow is (-).
 * - The servo steering motor has three pins, brown is ground, red is 5V and
 *   yellow is PWM on pin 5.
 * - The ranging sensor has four pins. GND and VCC. White wire connects trig to
 *   pin 4, Green wire connects echo to pin 7.
 * - The Wii nunchuck uses TWI (I2C). Use the Wire library. Use pin A4 (or SDA)
 *   and A5 (or SCL).
 *
 * Motor Shield Pins:
 *  D3 - (Ch A) PWM_A
 *  D5 - TinkerKit Analog Output
 *  D6 - TinkerKit Analog Output
 *  D8 - (Ch B) BRK_B, set HIGH to brake, LOW to spin down
 *  D9 - (Ch A) BRK_A, set HIGH to brake, LOW to spin down
 *  D11 - (Ch B) PWM_B 
 *  D12 - (Ch A) DIR_A, Direction, set HIGH or LOW
 *  D13 - (Ch B) DIR_B, Direction, set HIGH or LOW
 *  A0 - (Ch A) SNS_A, Current sensing
 *  A1 - (Ch B) SNS_B, Current sensing
 *  A2 - TinkerKit Analog Input
 *  A3 - TinkerKit Analog Input
 *  *** Note for PWM_A/PWM_B:
 *  Use analogWrite(pin,value) to generate a PWM wave. After a call to
 *  analogWrite() the pin will generate a steady square wave of the specified
 *  duty cycle until the next call to analogWrite() or a call to digitalRead()
 *  or digitalWrite() on the same pin. The frequency of the PWM signal is
 *  approximately 490 Hz. Value is 0 (always off) to 255 (always on).
 *  *** Note for SNS_A/SNS_B:
 *  Measure the current going through the DC motor by reading the SNS_A and
 *  SNS_B pins. On each channel will be a voltage proportional to the measured
 *  current, which can be read as a normal analog input, through the function
 *  analogRead() on the analog input A0 and A1. It is calibrated to be 3.3V
 *  when the channel is delivering its maximum possible current (2A).
 *
 * Project Setup
 * Top Pins, Right to Left:
 *  D0  - Not connected, TTL serial RX
 *  D1  - Not connected, TTL serial TX
 *  D2  - Not Used
 *  D3  - PWM_A
 *  D4  - Ultrasonic ranging sensor trigger (white)
 *  D5  - TinkerKit Analog Output, PWM_S, 180 degree servo for steering
 *  D6  - TinkerKit Analog Output, red wire from horn.
 *  D7  - Ultrasonic ranging sensor echo (green)
 *  D8  - Not Used, BRK_B
 *  D9  - BRK_A, set LOW to free spin
 *  D10 - Red LED in illuminated pushbutton switch
 *  D11 - Not Used, PWM_B
 *  D12 - DIR_A, set HIGH for foward, LOW for reverse
 *  D13 - Not Used, DIR_B
 *  GND - Ground for red LED in illuminated pushbutton switch
 *  AREF - Not Used
 *  SDA - Wii nunchuck SDA or use pin A4
 *  SCL - Wii nunchuck SCL or use pin A5
 *
 * Bottom Pins, Right to Left:
 *  A5 - Wii nunchuck SCL
 *  A4 - Wii nunchuck SDA
 *  A3 - Not Used, TinkerKit Analog Input
 *  A2 - Not Used, TinkerKit Analog Input
 *  A1 - Not Used, SNS_B
 *  A0 - SNS_A, used to read current on drive motor, Ch A.
 *  Vin - Not Used -- use power block on motor shield
 *  Gnd - Wii nunchuck
 *  Gnd - Ultrasonic ranging sensor ground (black)
 *  5V - Ultrasonic ranging sensor VCC (red)
 *  3.3V - Wii nunchuck
 *  RESET - Not Used
 *  IOREF - Not Used
 */

// If defined then output activity to serial port
//#define SERIAL_MONITOR

#include <Wire.h>
#include <Servo.h>
Servo strservo; // create servo object for steering

/* Define all constants specific to this application, such as pin numbers.
 * The convention is to use all capital letters for these names.
 */
// Pin to read current on ch A motor (3.3V = max)
#define SNS_A 0
// Pin to drive ch A motor speed, (0=stop, 255=max)
#define PWM_A 3
// Pin to drive ch A motor direction (H = forward)
#define DIR_A 12
// Pin to set ch A motor brake mode (L = free spin)
#define BRK_A 9
// Pin for steering servo (90 = straight)
#define PWM_S 5
// Pin for front ranging sensor trigger
#define FRONT_T 4
// Pin for front ranging sensor echo
#define FRONT_E 7
// Pin for red LED in pushbutton switch
#define LEDPIN 10
// Rapid blink rate to warn something is close to the distance sensor
#define BLINK_DANGER 100
// Medium blink rate to warn something is near the distance sensor
#define BLINK_WARN 500
// Slow blink rate to warn something is within range of the distance sensor
#define BLINK_DETECT 1000
// Slow blink to show we're still working. Nothing in range of distance sensor
#define BLINK_CLEAR 2000
// Pin for horn. This is a buzzer. We'll use digitalWrite() to control it.
#define HORNPIN 6

/* Initialize all global variables
 * These variables are used across multiple functions to control the state of
 * the robot. The number of variables should be minimized. If this code is
 * ported to a multi-core system, such as a Parallax Propeller board, or to a
 * multi-tasking environment, use of global variables should be eliminated.
 */
int str_pos = 90; // variable to store the steering servo position
int drv_spd = 0;  // variable to store the speed (0 to 255)
int drv_dir = LOW; // variable to store direction (H = forward)
int front_distance = 0; // Distance sensed by front ranging sensor
static uint8_t nunchuck_buf[6]; // status from the Wii nunchuck
int led_state = LOW; // If the indicator LED is on or off.
unsigned long led_interval = BLINK_CLEAR; // Blink rate for indicator LED in ms

/* Function: setup()
 * The setup function is called only once and should perform all
 * initialization.  This isn't typically so long but we're using this for some
 * power on testing.
 */
void setup()
{
  /* The serial output code will only be included if SERIAL_MONITOR has been
   * defined. Leaving out all the serial output code in this file saves over
   * 1k bytes of code space and frees up processing for other tasks.On the Uno
   * we're limited to about 31k.
   */
  #ifdef SERIAL_MONITOR
  Serial.begin(19200);
  Serial.println("Starting setup");
  #endif

  // Set pin modes
  pinMode(BRK_A, OUTPUT);
  pinMode(DIR_A, OUTPUT);
  pinMode(PWM_A, OUTPUT);
  pinMode(LEDPIN, OUTPUT);
  pinMode(FRONT_T, OUTPUT);
  pinMode(FRONT_E, INPUT);
  pinMode(HORNPIN, OUTPUT);

  // One time initializations
  led_on(); // turn on the red push button LED, our indicator
  strservo.attach(PWM_S); // attach steering servo
  digitalWrite(BRK_A, LOW); // set brake to free spin
  analogWrite(PWM_A, drv_spd); // set speed
  digitalWrite(DIR_A, drv_dir); // set direction

  /* To kickoff (and perform a limited power-on self-test) we will rotate the
   * steering left and right, then drive backward and forward for a few
   * centimeters.
   */

  // First we try out the steering servo
  for (str_pos = 90; str_pos < 180; str_pos += 1)
  { // go from 90 to 180 degrees in steps of 1 degree
    strservo.write(str_pos);
    delay(10); // wait 10 ms for the servo to reach position
  }

  led_toggle();
  delay(1000); // wait for a second

  for (str_pos = 180; str_pos > 0; str_pos -= 1)
  { // go from 180 to 0 degrees in steps of 1 degree
    strservo.write(str_pos);
    delay(10); // wait 10 ms for the servo to reach position
  }

  led_toggle();
  delay(1000); // wait for a second

  // Return to pointing the wheels straight ahead
  for (str_pos = 0; str_pos < 90; str_pos += 1)
  { // go from 0 to 90 degrees in steps of 1 degree
    strservo.write(str_pos);
    delay(10); // wait 10 ms for the servo to reach position
  }

  #ifdef SERIAL_MONITOR
  Serial.println("Starting Wii nunchuck initialization");
  #endif
  nunchuck_init(); // send the initialization handshake
  led_toggle();
  delay(1000); // wait for a second
  nunchuck_get_data();
  #ifdef SERIAL_MONITOR
  nunchuck_print_data();
  #endif

  // Now we try out the drive motor
  drv_spd = 64; // 256/4 = 64 or one-quarter speed
  analogWrite(PWM_A, drv_spd);
  led_toggle();
  delay(1000);

  drv_spd = 0; // Full stop
  analogWrite(PWM_A, drv_spd);
  led_toggle();
  delay(1000);

  drv_dir = HIGH; // go forward
  digitalWrite(DIR_A, drv_dir);
  drv_spd = 64; // all ahead one quarter speed
  analogWrite(PWM_A, drv_spd);
  led_toggle();
  delay(1000);

  drv_spd = 0; // Full stop
  analogWrite(PWM_A, drv_spd);
  led_toggle();
  delay(1000); // wait one second

  #ifdef SERIAL_MONITOR
  Serial.println("Setup complete");
  #endif
}

/* Function: loop()
 * The loop function is the main loop, called repeatedly until the Arduino is
 * powered off.
 */
void loop()
{
  int front_range = 0;

  delay(100);

  led_blink(); // See if it's time to turn the LED on or off

  // Read the nunchuck data
  nunchuck_get_data();
  #ifdef SERIAL_MONITOR
  nunchuck_print_data();
  #endif
  int joy_x_axis = nunchuck_buf[0]; // go left or right
  int joy_y_axis = nunchuck_buf[1]; // go forward or backward
  // Check if Z button is being pressed
  if ((nunchuck_buf[5] >> 0) & 1)
    horn_off(); // Button = 1 if not pressed
  else
    horn_on(); // Button = 0 if pressed

  // Handle steering left or right
  robot_joystick_steering(joy_x_axis);
  // Handle going forward or backward
  robot_joystick_speed(joy_y_axis);

  // Check the front distance sensor for obstacles
  front_range = robot_distance_sensor(FRONT_T, FRONT_E);
  if (  6 > front_range )
    led_blink_rate(BLINK_DANGER); // something is within 6 inches
  else if ( 12 > front_range )
    led_blink_rate(BLINK_WARN); // something is within 12 inches
  else if ( 24 > front_range )
    led_blink_rate(BLINK_DETECT); // something is within 2 feet
  else
    led_blink_rate(BLINK_CLEAR); // nothing to worry about yet
}

/* Function: robot_joystick_steering(int joy_x_axis)
 *
 * Act on steering commands from nunchuck joystick sensor. The value of the x
 * axis ranges from left = 24, center = 120, right = 221.
 * The front wheel servo uses left = 0, center = 90, wheels right = 180.
 * We must map joy_x_axis to a new value for str_pos.
 *
 * A simple approach is to right shift by 5 bits, then set str_pos to one of
 * seven values based on the result. This gives us:
 *    X (binary)   : >>5 : str_pos
 *  24 : 0001 1000 : 000 :   0
 *  31 : 0001 1111 : 000 :   0
 *  32 : 0010 0000 : 001 :  30
 *  63 : 0011 1111 : 001 :  30
 *  64 : 0100 0000 : 010 :  60
 *  95 : 0101 1111 : 010 :  60
 *  96 : 0110 0000 : 011 :  90
 * 127 : 0111 1111 : 011 :  90
 * 128 : 1000 0000 : 100 : 120
 * 159 : 1001 1111 : 100 : 120
 * 160 : 1010 0000 : 101 : 150
 * 191 : 1011 1111 : 101 : 150
 * 192 : 1100 0000 : 110 : 180
 * 221 : 1101 1101 : 110 : 180
 * This is almost perfect but 120 is the value of the joystick when centered.
 * The center of our 96-127 range is 112. To adjust for this we must subtract 8
 * before we right shift. Otherwise our car will tend to steer right.
 */
void robot_joystick_steering(int joy_x_axis)
{
  /* Switch statements are easy to understand. A good compiler can optimize
   * this into small, fast code. I'm not sure how good the Arduino and AVR
   * compiler are at this yet.
   *
   * The switch statement determines what state we should be in by calculating
   * the new state x where x = (joy_x_axis - 8) >> 5. This takes the current
   * sensor reading of the x axis, adjusts for our different center points,
   * then drops the least significant five bits to arrive at one of 7 states.
   *
   * Other approaches would be to multiply x by 30. Or if the processor did not
   * have a multiply operation we could execute a loop to add 30 and decrement
   * x until x reaches 0. Since x only ranges from 0 to 6 this will be faster
   * than doing a multiply even on some processors that support integer
   * multiplication.
   *
   * We don't check to see if the new value for str_pos is the same as the old
   * value. We just update and write to the servo. Recall that the servo is
   * controlled by a PWM signal. Writing a value to the servo only changes an
   * internal register used by the PWM hardware in the processor chip to
   * control the duty cycle of the square wave. The PWM signal is constantly
   * being sent anyway so changing the register doesn't require additional
   * power. Effectively, a check for any change to str_pos is being handled for
   * us in hardware.
   */
  switch ( (joy_x_axis - 8) >> 5 ) {
    case 0:
      str_pos = 0;
      break;
    case 1:
      str_pos = 30;
      break;
    case 2:
      str_pos = 60;
      break;
    case 3:
      str_pos = 90;
      break;
    case 4:
      str_pos = 120;
      break;
    case 5:
      str_pos = 150;
      break;
    default:
      str_pos = 180;
  }
  strservo.write(str_pos);
}

/* Function: robot_joystick_speed(int joy_y_axis)
 *
 * Act on forward/backward and speed commands from the nunchuck joystick.
 * The observed y value range is full reverse = 28, center = 130, full forward
 * = 224.  Our direction is controlled by DIR_A with HIGH = forward. We keep
 * the current value in the global variable drv_dir.  Our speed is controlled
 * by PWM_A (0=stop, 255=max). We keep the current value in the global variable
 * drv_spd.  We must map joy_y_axis to new values for drv_spd and drv_dir.
 *
 * A simple approach is to right shift by 5 bits, then set drv_spd and drv_dir
 * to specific values based on the results. The problem is that if we are going
 * forward fast and try to go in reverse fast we'll draw too much current,
 * power will drop below the minimum, and the robot will reboot.
 *
 * The solution is to keep track of what state we're in and only allow a
 * transition to an adjacent state. This way we never make a big change and
 * draw too much current. As long as the main loop doesn't call this function
 * too frequently. A delay of about 100 ms in the main loop seems to avoid a
 * reboot.
 *
 *    Y (binary)   : >>5 : drv_dir : drv_spd
 *  24 : 0001 1000 : 000 :    L    :   255
 *  31 : 0001 1111 : 000 :    L    :   255
 *  32 : 0010 0000 : 001 :    L    :   170
 *  63 : 0011 1111 : 001 :    L    :   170
 *  64 : 0100 0000 : 010 :    L    :    85
 *  95 : 0101 1111 : 010 :    L    :    85
 *  96 : 0110 0000 : 011 :    L    :     0
 * 127 : 0111 1111 : 011 :    L    :     0
 * 128 : 1000 0000 : 100 :    H    :    85
 * 159 : 1001 1111 : 100 :    H    :    85
 * 160 : 1010 0000 : 101 :    H    :   170
 * 191 : 1011 1111 : 101 :    H    :   170
 * 192 : 1100 0000 : 110 :    H    :   255
 * 221 : 1101 1101 : 110 :    H    :   255
 *
 * This is almost perfect but 130 is the value of the joystick when centered.
 * The center of our 96-127 range is 112. To adjust for this we must subtract
 * 18 before we right shift.
 */
 void robot_joystick_speed(int joy_y_axis)
{
  int y, next_y;
  static int last_y=3;
  // Figure out what state the user wants us in.
  y = (joy_y_axis - 18) >> 5;
  /* Figure out what state we're able to reach. Only move one state closer to
   * what the user requested. If we're already there then take no action.
   */
  next_y = last_y;
  if (y > last_y) next_y = last_y + 1;
  if (y < last_y) next_y = last_y - 1;
  // Only do something if we're in a new state, otherwise skip to end.
  if ( next_y != last_y ) {
      // Determine new values for direction and speed
    switch (next_y) {
      case 0:
        drv_dir = LOW;
        drv_spd = 255;
        break;
      case 1:
        drv_dir = LOW;
        drv_spd = 170;
        break;
      case 2:
        drv_dir = LOW;
        drv_spd = 85;
        break;
      case 3:
        drv_dir = LOW;
        drv_spd = 0;
        break;
      case 4:
        drv_dir = HIGH;
        drv_spd = 85;
        break;
      case 5:
        drv_dir = HIGH;
        drv_spd = 170;
        break;
      default:
        drv_dir = HIGH;
        drv_spd = 255;
    }
    digitalWrite(DIR_A, drv_dir);
    analogWrite(PWM_A, drv_spd);
    last_y = next_y;
  }
}

/* Function: robot_distance_sensor(trig_pin, echo_pin)
 * Determine the distance to object in front of an ultrasonic sensor.
 *
 * The following is front the ping example included with the Arduino:
 *
 * According to Parallax's datasheet for the PING))), there are 73.746
 * microseconds per inch (i.e. sound travels at 1130 feet per second).  This
 * gives the distance travelled by the ping, outbound and return, so we divide
 * by 2 to get the distance of the obstacle.
 * See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
 *
 * We don't care about any distance less than 1 inch, and we return an integer
 * so we can't return fractional distance. To keep this simple we always return
 * a positive whole number. We should never return 0.
 *
 * We also don't care about any distance further than 24 inches. So we set the
 * pulseIn timeout to 25 * 72 * 2 microseconds, or 3.6 ms.
 */
int robot_distance_sensor(const int trig_pin, const int echo_pin)
{
  int duration;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(trig_pin, LOW);
  // Only wait long enough to detect up to 25 inches.
  duration = pulseIn(echo_pin, HIGH, (25 * 72 * 2) );
  // Always return a positive integer between 1 and 25).
  if ( (74 * 2) < duration )
    return duration / (74 * 2);
  else
    return 25;
}

/* Some LED indicator helper functions.
 * The red LED is part of the on/off switch so we know it's visible.  The
 * typical indicator LED is the yellow SMD one mounted on the Arduino board.
 * However, this is covered by the motor shield and pin 13 is used for channel
 * B motor control.
 */

// Turn the indicator LED on
void led_on()
{
  digitalWrite(LEDPIN, HIGH);
  led_state = HIGH;
}

// Turn the indicator LED off
void led_off()
{
  digitalWrite(LEDPIN, LOW);
  led_state = LOW;
}

// Toggle the indicator LED
void led_toggle()
{
  if ( led_state == HIGH )
    led_off();
  else
    led_on();
}

/* Function: led_blink()
 * See http://www.arduino.cc/en/Tutorial/BlinkWithoutDelay
 * Blink the LED at some rate set by the global variable led_interval.
 *
 * After checking if it's time to blink we set last_time = time. We could make
 * another call to millis() for more accuracy, but at 16MHz and a maximum of 4
 * cycles per instruction, 1 ms comes out to at least 4,000 instructions. This
 * function should be much less than 4k instructions, so there is no effective
 * additional accuracy.
 */
void led_blink()
{
  static unsigned long last_time = 0;
  // This returns how long we've been running as a 32-bit integer
  unsigned long time = millis();
  // Handle the overflow, which happens once every 50 days or so.
  if ( time < last_time ) last_time = 0xffffffff - last_time;
  // See if it's time to blink
  if ( time - last_time > led_interval ) {
    last_time = time;
    led_toggle();
  }
}

/* Function: led_blink_rate()
 * Set the rate at which to blink
 */
void led_blink_rate( unsigned long interval)
{
  led_interval = interval;
  led_blink();
}

/* Some horn functions.
 * The buzzer is connected to pin 6, HORNPIN.
 * Horn on sets the pin high, or 5V.
 * Horn off sets the pin low, or 0V.
 */
void horn_on()
{
  digitalWrite(HORNPIN, HIGH);
}
void horn_off()
{
  digitalWrite(HORNPIN, LOW);
}

/* Nunchuck Functions
 * From:  NunchuckPrint.ino
 * 2007 Tod E. Kurt, http://todbot.com/blog/
 *
 * CHANGES: What needed to be changed in the downloaded code
 * - In Arduino v1.0 Wire.print() became Wire.write(), and Wire.receive()
 *   became Wire.read().
 */
// initialize the I2C system, join the I2C bus, and tell the nunchuck we're
// talking to it.
void nunchuck_init()
{
  Wire.begin();                 // join i2c bus as master
  Wire.beginTransmission(0x52); // transmit to device 0x52
  Wire.write(0x40);   // sends memory address
  Wire.write(0x00);   // sends sent a zero.  
  Wire.endTransmission(); // stop transmitting
}

// Send a request for data to the nunchuck
// was "send_zero()"
void nunchuck_send_request()
{
  Wire.beginTransmission(0x52); // transmit to device 0x52
  Wire.write(0x00);   // sends one byte
  Wire.endTransmission(); // stop transmitting
}

// Receive data back from the nunchuck, 
int nunchuck_get_data()
{
    int cnt=0;
    Wire.requestFrom (0x52, 6); // request data from nunchuck
    while (Wire.available ()) {
      // receive byte as an integer
      nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read());
      cnt++;
    }
    nunchuck_send_request();  // send request for next data payload
    // If we recieved the 6 bytes, then go print them
    if (cnt >= 5) {
     return 1;   // success
    }
    return 0; //failure
}

// Print the input data we have recieved
// accel data is 10 bits long so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I multiply them by 2 * 2
void nunchuck_print_data()
{
  static int i=0;
  int joy_x_axis = nunchuck_buf[0];
  int joy_y_axis = nunchuck_buf[1];
  int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; 
  int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
  int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;

  int z_button = 0;
  int c_button = 0;

  // byte nunchuck_buf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((nunchuck_buf[5] >> 0) & 1)
    z_button = 1;
  if ((nunchuck_buf[5] >> 1) & 1)
    c_button = 1;

  if ((nunchuck_buf[5] >> 2) & 1)
    accel_x_axis += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accel_x_axis += 1;

  if ((nunchuck_buf[5] >> 4) & 1)
    accel_y_axis += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accel_y_axis += 1;

  if ((nunchuck_buf[5] >> 6) & 1)
    accel_z_axis += 2;
  if ((nunchuck_buf[5] >> 7) & 1)
    accel_z_axis += 1;

  Serial.print(i,DEC);
  Serial.print("\t");

  Serial.print("joy:");
  Serial.print(joy_x_axis,DEC);
  Serial.print(",");
  Serial.print(joy_y_axis, DEC);
  Serial.print("  \t");

  Serial.print("acc:");
  Serial.print(accel_x_axis, DEC);
  Serial.print(",");
  Serial.print(accel_y_axis, DEC);
  Serial.print(",");
  Serial.print(accel_z_axis, DEC);
  Serial.print("\t");

  Serial.print("but:");
  Serial.print(z_button, DEC);
  Serial.print(",");
  Serial.print(c_button, DEC);

  Serial.print("\r\n");  // newline
  i++;
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}

// vim: set expandtab tabstop=2 shiftwidth=2 syntax=c: