Your Arduino Balancing Robot (YABR)

Your Arduino Balancing Robot (YABR) is a self-balancing robot that you can build yourself as a school project or as a fun project with your kids. It might look simple but there is a lot that you can learn from building this self-balancing robot.

In contrast to most self-balancing robots, this one uses stepper motors instead of regular DC motors. The main reason is that stepper motors are precise and have no performance loss when the battery voltage drops. One pulse is always an exact amount of motion. Regular DC motors can have mechanical friction and electric resistance differences. This can cause performance differences. As a result the robot will not move in a straight line.

The total cost to build this robot is approximately $80 if you use the hardware list below. This includes a battery, Nunchuck, charger, stepper motors, etc.

The Arduino program that you can download for free is 100% self-written and not based on any other software. The code is well commented and clearly explained. This makes it possible to further develop the code for your own purpose.

 Click to see the full image

If you encounter any problems during the build or setup please check the Q&A page first. Most questions are already answered in detail.

Step 1 – Software

First download the complete YMFC-AL software package:

YABR.zip (version 1.1)

Step 2 – Hardware

To build this robot you need hardware. I made the following list for convenience purpose alone. You are free to get your own hardware from different sources. But this is the hardware that I used/ordered:

If the 35mm stepper motors are out of stock. You could also use these stepper motors. Please note that these are 42mm in stead of 35mm. So you have to modify the frame.

Most of the following parts can be found in your local electronics store. But in case you don’t have an electronics store nearby I will put links here:

And finally you need an old inner tube and two sheets of plywood. I used 2.5mm and 12.5mm sheets.

Step 3 – Tools

And of course you need some simple tools like a soldering iron, screwdrivers, a fretsaw, compact drill, etc.

Step 4 – The build

Watch the YABR hardware build video and build the robot according to the video and the schematic that is included in the complete software package.

Building the YABR balancing robot.

Detailed pictures of my own YABR balancing robot can be found in the media section of this project page.


4.1 The diode and resistors


The resistor R1 on the schematic is needed for uploading a program to the Arduino. The TXD output of the transceiver is forced high or low. As a result the FTDI programmer cannot change this output anymore and you will get an upload error. By adding this resistor the FTDI programmer can change the voltage on the RX-pin of the Arduino despite the state the transceiver output and the program is uploaded without any problems.

The other two resistors (R2 and R3) form a voltage divider. Meaning that the 12.6 volt of the battery minus the 0.6 volt voltage drop over the diode is divided by 2.5. Resulting in a 4.8 volt on the analog input when the battery fully charged. In the main program this analog input will be used to protect the battery. This is because lipo batteries can be damaged when the voltage drops below 3 volt per cell.

The diode D1 protects all the electronics against reversed polarity. So when you accidentally reverse the connections of the battery the components won’t go up in smoke.


4.2 The MPU-6050 gyro/accelerometer


The only gyro / accelerometer that is supported by the YABR software is the MPU-6050. This is because the self-level feature requires an accelerometer and a gyro as I explain in these two videos:

MPU-6050 6dof IMU tutorial for auto-leveling quadcopters – Part 1
MPU-6050 6dof IMU tutorial for auto-leveling quadcopters – Part 2

The orientation of the gyro is important. Make sure to mount the gyro in the exact same orientation as shown on this picture. Otherwise the YABR software cannot calculate the correct angle and the robot will not work.


4.3 Hardware test


Please note that you are soldering the wires on the back side of the PCB. The schematic is drawn facing the components from the front. So everything is mirrored and you need to double check all the connections before you connect any power to the PCB.

With everything in place it’s time to connect the FTDI programmer to the Arduino pro mini. Don’t connect the battery yet. If the LED’s don’t lit up there is a short circuit in the wiring and you need to disconnect the FTDI programmer as soon as possible. Normally the Arduino pro mini is already programmed with the blink sketch so the LED on the Arduino should start to flash.

To check if the gyro is connected correctly and to check the balancing point of the robot you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier.

Test the balancing angle of the robot and fix it in that position on a stand as I showed in the video. Upload the “hardware-check” program, open the serial monitor and set the baud rate to 9600.

After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a MPU-6050. If everything is working as expected the program will output several raw gyro values on the screen. These are just examples and your values may be different. Note down the balance value that you see in the serial output. You will need it later in the main program.


4.4 Limit the motor current


Next thing on the to do list is to set the stepper controllers to the correct drive current. If the motor current is set to high the stepper controllers will heat up and they might get damaged.

First set the potentiometer at the same position as shown in the picture below. Now always be careful when connecting the lipo battery for the first time. A short circuit can cause high currents, heat, sparks, and burns.

A good alternative is to use a small DC fuse like the one below. This fuse will blow if the wiring on the back side of the pcb holds a short circuit.

The easiest and safest way to set the correct current and to check if the wiring is correctly connected is by measuring the current in the power supply wires with a bench power supply. The power supply is limited to 500milli amps. This is an extra safety feature. If there is a short circuit the power supply will limit the current to 500 milli amps and the wiring will be fine.

First connect one motor. With the potentiometer it’s possible to set the current between 100 and 150mA. This is more than enough to get good performance. Do the same with the other stepper controller.

If you don’t have the possibility to measure the current just set the potentiometer in this position and feel if the stepper driver doesn’t get to hot. But it’s always best to measure the current.


3.5 The remote control


If you open the Nunchuck you can note the wire colors that are connected to the various pins as you can see on the schematic. The Nunchuck works on 3.3V and you can use the 3.3V output of the Arduino Uno to power the Nunchuck.

The 5V output can be used for the transceiver. Again, connect the wires as shown on the schematic to get it to work.

To check if the Nunchuck is connected correctly you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier. After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a Nunchuck. If everything is working as expected the program will output several raw joystick values on the screen.

Step 5 – Upload the software

And finally you can upload the YABR-remote program to the Arduino Uno and the YABR-robot program to the Arduino pro mini.

The YABR-remote program does not need any modification. In the robot program you need to change the accelerometer calibration value to the value that was shown by the hardware test program when the robot was in it’s balancing position.

To start the robot you need to power up the remote. Lay the robot on it’s back and switch on the power. The LED will blink indicating that the gyro is calibrating. When the blinking stops you can slowly lift the robot and it will automatically start to balance itself.

And basically that’s it. You can now control the robot with the Nunchuck.

Wii 리모컨(Wiimote)으로 RC Car 조종하기 – 2WS (Tamiya M-03)

이젠 벽돌이 되어버린 니텐도 Wii를 되살리기 차원에서 Wii 리모컨을 사용하여 RC Car를 조종하는 프로젝트를 시작해봅니다.

Source를 수정하여 Tamiya M-03 샤시에 적용해 보기로 했습니다.

Circuit@Home에서 소개 받았습니다!

RC car controlled by Wii Remote on Arduino http://www.circuitsathome.com/mcu/rc-car-controlled-by-wii-remote-on-arduino

USB Host를 적용하여 Bluetooth로 직접 연결하는 방식이기 때문에 구성은 매우 간단합니다.

Arduino는 Nano를 사용하였고 USB Host Shield와 연결하기 위해 Mother Board를 하나 꾸며보았습니다.

기능도 조금 추가했습니다.

바나나를 던지거나 하지는 못하지만 “A” 버튼을 누르면 레이저가 발사되듯이 Fire LED가 켜지도록 하였고 “+” 버튼은 Head Light, “-” 버튼은 Back Light로 사용할 수 있도록 하였습니다.

그리고 좌우방향 버튼으로도 Steering이 가능하도록 하였습니다.

 Arrow Up  Left steering
 Arrow Down  Right steering
 Arrow Left  Gear Down
 Arrow Right  Gear Up
 A  Fire & Shutting
 B  –
 +  Head Light
 –  Back Light
 1  Backward
 2  Forward
 Home  Throttle mode change

하드웨어 (Hardware)

송신기 (Transmitter)

  • Wii 리모컨 (Wiimote)

수신기 (Reciver)

  • Arduino Nano

  • USB Host Shield
  • Bluetooth USB 어댑터 (Dongle)

  • DIY Mother Board
 
 

통신 거리는 Bluetooth 특성상 대략 10m 정도 밖에 안될 거라고 예상합니다만 그 이상도 충분합니다.

 

RC Car

  • Tamiya M-03

소프트웨어 (Software)

Arduino 용 라이브러리와 스케치를 포함한 파일은 github 에 있습니다.
이 라이브러리는 Richard Ibbotson이 만든 코드 wiiblue.pde 을 기반으로하고 있습니다.

샘플 스케치 (Sketch)

TwoWheelSterringWii.ino

#include “WiiRemote.h”

#include <MemoryFree.h>

#include <Servo.h>

#define PIN_STEERING_SIGNAL        2

#define PIN_ESC_SIGNAL             4

#define PIN_HEAD_LIGHT_SIGNAL     14

#define PIN_BACK_LIGHT_SIGNAL     15

#define PIN_FIRE_SIGNAL           17

#define PIN_STEERING_SELECT       16

#define SERIAL_DEBUG               0    // 0: active mode, 1: serial debug mode

enum eAngle

{

STEERING_ANGLE_MAX           = 165,   // to right

STEERING_ANGLE_CENTER        = 90,

STEERING_ANGLE_MIN           = 15,    // to left

STEERING_ANGLE_STEP          = 5,

STEERING_ANGLE_MAX_INVERT    = 165,   // to right

STEERING_ANGLE_CENTER_INVERT = 90,

STEERING_ANGLE_MIN_INVERT    = 15,    // to left

THROTTLE_ANGLE_MAX           = 160,   // 80,

THROTTLE_ANGLE_CENTER        = 90,

THROTTLE_ANGLE_MIN           = 10,

};

enum eServoPulse

{

SERVO_PULSE_MAX              = 2400,  // to left

SERVO_PULSE_NEUTRAL          = 1550,  // 1500 Futaba compatible, 1.55msec

SERVO_PULSE_MIN              = 600,   // to right

SERVO_PULSE_MAX_INVERT       = 600,   // to right

SERVO_PULSE_NEUTRAL_INVERT   = 1450,  // 1460 Futaba compatible, 1.55msec

SERVO_PULSE_MIN_INVERT       = 2400,  // to left

};

enum eESCPulse

{

/*

*  Futaba timing

*

*  0us     1072us         1522us          1922us

*   +———*————+-*-+————-*

*   |   n/a   |   forwad   |d|d|   Reverse   |

*   +———*————+-*-+————-*

*          Max Forwad     Neutral         Max Reverse

*

*   d: dead zone, +10us and -10us

*/

ESC_PULSE_NEUTRAL     = 1522,

ESC_PULSE_BRAKE       = 1600,

ESC_PULSE_FWD_MAX     = 800,   //1200, // 1072

ESC_PULSE_FWD_MIN     = 1510,

ESC_PULSE_FWD_3RD     = (ESC_PULSE_FWD_MIN – 240),

ESC_PULSE_FWD_2ND     = (ESC_PULSE_FWD_MIN – 160),

ESC_PULSE_FWD_1ST     = (ESC_PULSE_FWD_MIN – 80),

ESC_PULSE_REV_MAX     = 1700,  // 1922

ESC_PULSE_REV_FIX     = 1650,

ESC_PULSE_REV_MIN     = 1600,

};

enum eGear

{

GEAR_1ST = 1,

GEAR_2ND = 2,

GEAR_3RD = 3,

};

WiiRemote wiiremote;

Servo SteeringServo;

Servo ESC;

void setup()

{

#if SERIAL_DEBUG

Serial.begin(115200);

Serial.print(“\r\nfreeMemory() reports: “);

Serial.print(freeMemory(), DEC);

Serial.println(“Serial connect…”);

#endif

SteeringServo.attach(PIN_STEERING_SIGNAL);

SteeringServo.writeMicroseconds(SERVO_PULSE_NEUTRAL);

ESC.attach(PIN_ESC_SIGNAL);

ESC.writeMicroseconds(ESC_PULSE_NEUTRAL);

pinMode(PIN_HEAD_LIGHT_SIGNAL, OUTPUT);

pinMode(PIN_BACK_LIGHT_SIGNAL, OUTPUT);

digitalWrite(PIN_HEAD_LIGHT_SIGNAL, LOW);

digitalWrite(PIN_BACK_LIGHT_SIGNAL, LOW);

pinMode(PIN_FIRE_SIGNAL, OUTPUT);

digitalWrite(PIN_FIRE_SIGNAL, LOW);

pinMode(PIN_STEERING_SELECT, INPUT);

digitalWrite(PIN_STEERING_SELECT, LOW);

wiiremote.init();

/*

unsigned char wiiremote_bdaddr[6] = {0x00, 0x1e, 0x35, 0xda, 0x48, 0xbc};

wiiremote.setBDAddress(wiiremote_bdaddr, 6);

wiiremote.setBDAddressMode(BD_ADDR_FIXED);

*/

#if SERIAL_DEBUG

Serial.println(“Wiimote connecting…”);

Serial.println(“Please press 1 button and 2 button simultaneously”);

#endif

}

void loop()

{

wiiremote.task(&myapp);

}

int steering_angle          = STEERING_ANGLE_CENTER;

int steering_angle_invert   = STEERING_ANGLE_CENTER_INVERT;

int old_steering_angle      = STEERING_ANGLE_CENTER;

bool analog_throttle        = false;  // false = use “One” button as throttle

int throttle_angle          = THROTTLE_ANGLE_CENTER;

int gear                    = GEAR_1ST;

int pulse_steering;

int pulse_esc;

bool fire                   = false;                  // fire

bool head_light             = false;                  // head light

bool back_light             = false;                  // back light

void myapp(void)

{

#if SERIAL_DEBUG

Serial.print(“\r\n”);

#endif

/* Steering */

steering_angle = getSteeringAngle();

steering_angle_invert = getSteeringAngleInvert();

if (digitalRead(PIN_STEERING_SELECT) == HIGH) {

pulse_steering = map(steering_angle,

STEERING_ANGLE_MIN, STEERING_ANGLE_MAX,

SERVO_PULSE_MAX, SERVO_PULSE_MIN);

SteeringServo.writeMicroseconds(pulse_steering);

} else {

pulse_steering = map(steering_angle_invert,

STEERING_ANGLE_MIN_INVERT, STEERING_ANGLE_MAX_INVERT,

SERVO_PULSE_MAX_INVERT, SERVO_PULSE_MIN_INVERT);

SteeringServo.writeMicroseconds(pulse_steering);

}

if (wiiremote.buttonPressed(WIIREMOTE_UP)) {

steering_angle = STEERING_ANGLE_MIN;

steering_angle_invert = STEERING_ANGLE_MIN_INVERT;

if (digitalRead(PIN_STEERING_SELECT) == HIGH) {

pulse_steering = map(steering_angle, STEERING_ANGLE_MIN, STEERING_ANGLE_MAX, SERVO_PULSE_MAX, SERVO_PULSE_MIN);

SteeringServo.writeMicroseconds(pulse_steering);

} else {

pulse_steering = map(steering_angle_invert, STEERING_ANGLE_MIN_INVERT, STEERING_ANGLE_MAX_INVERT, SERVO_PULSE_MAX_INVERT, SERVO_PULSE_MIN_INVERT);

SteeringServo.writeMicroseconds(pulse_steering);

}

} else if (wiiremote.buttonPressed(WIIREMOTE_DOWN)) {

steering_angle = STEERING_ANGLE_MAX;

steering_angle_invert = STEERING_ANGLE_MAX_INVERT;

if (digitalRead(PIN_STEERING_SELECT) == HIGH) {

pulse_steering = map(steering_angle, STEERING_ANGLE_MIN, STEERING_ANGLE_MAX, SERVO_PULSE_MAX, SERVO_PULSE_MIN);

SteeringServo.writeMicroseconds(pulse_steering);

} else {

pulse_steering = map(steering_angle_invert, STEERING_ANGLE_MIN_INVERT, STEERING_ANGLE_MAX_INVERT, SERVO_PULSE_MAX_INVERT, SERVO_PULSE_MIN_INVERT);

SteeringServo.writeMicroseconds(pulse_steering);

}

}

#if SERIAL_DEBUG

Serial.print(“\tServo=”);

Serial.print(pulse_steering);

#endif

/* Brake and Throttle */

if (wiiremote.buttonPressed(WIIREMOTE_ONE)) {

if (pulse_esc < ESC_PULSE_NEUTRAL) {

// moving forward before press “One”

brake();

pulse_esc = ESC_PULSE_NEUTRAL;

} else {

// while stopping or moving backward, keep moving backward

pulse_esc = ESC_PULSE_REV_FIX;

}

} else {

if (analog_throttle) {

throttle_angle = getThrottleAngle();

pulse_esc = map(throttle_angle,

THROTTLE_ANGLE_MIN, THROTTLE_ANGLE_MAX,

ESC_PULSE_FWD_MIN, ESC_PULSE_FWD_MAX);

} else if (wiiremote.buttonPressed(WIIREMOTE_TWO)) {

switch (gear) {

case GEAR_1ST:

pulse_esc = ESC_PULSE_FWD_1ST;

break;

case GEAR_2ND:

pulse_esc = ESC_PULSE_FWD_2ND;

break;

case GEAR_3RD:

pulse_esc = ESC_PULSE_FWD_3RD;

break;

default:

pulse_esc = ESC_PULSE_NEUTRAL;

break;

}

} else {

pulse_esc = ESC_PULSE_NEUTRAL;

}

}

ESC.writeMicroseconds(pulse_esc);

//delay(15);

#if SERIAL_DEBUG

Serial.print(“\tESC=”);

Serial.print(pulse_esc);

#endif

/* Throttle mode */

if (wiiremote.buttonClicked(WIIREMOTE_HOME)) {

analog_throttle = !analog_throttle;

if (analog_throttle) {

wiiremote.setLED(WIIREMOTE_LED4); // analog mode

} else {

wiiremote.setLED(WIIREMOTE_LED1); // fixed mode, 1st gear

gear = GEAR_1ST;

}

}

/* Shift up or down */

if (!analog_throttle) {

if (wiiremote.buttonClicked(WIIREMOTE_RIGHT)) {

shiftUp();

} else if (wiiremote.buttonClicked(WIIREMOTE_LEFT)) {

shiftDown();

}

}

/* Fire */

if (wiiremote.buttonPressed(WIIREMOTE_A)) {

digitalWrite(PIN_FIRE_SIGNAL, HIGH);

} else {

digitalWrite(PIN_FIRE_SIGNAL, LOW);

}

/* Head light LED */

if (wiiremote.buttonClicked(WIIREMOTE_PLUS)) {

head_light = !head_light;

if (head_light) {

digitalWrite(PIN_HEAD_LIGHT_SIGNAL, HIGH);

} else {

digitalWrite(PIN_HEAD_LIGHT_SIGNAL, LOW);

}

}

/* Back light LED */

if (wiiremote.buttonClicked(WIIREMOTE_MINUS)) {

back_light = !back_light;

if (back_light) {

digitalWrite(PIN_BACK_LIGHT_SIGNAL, HIGH);

} else {

digitalWrite(PIN_BACK_LIGHT_SIGNAL, LOW);

}

}

} // myapp

int getSteeringAngle(void)

{

double rad;

int deg;

rad = acos((double) wiiremote.Report.Accel.Y);

deg = (int) (rad * 180.0 / PI);

/* clipping */

if (deg > STEERING_ANGLE_MAX) { deg = STEERING_ANGLE_MAX; }

if (deg < STEERING_ANGLE_MIN) { deg = STEERING_ANGLE_MIN; }

return deg;

}

int getSteeringAngleInvert(void)

{

double rad;

int deg;

rad = acos((double) wiiremote.Report.Accel.Y);

deg = (int) (rad * 180.0 / PI);

/* clipping */

if (deg > STEERING_ANGLE_MAX_INVERT) { deg = STEERING_ANGLE_MAX_INVERT; }

if (deg < STEERING_ANGLE_MIN_INVERT) { deg = STEERING_ANGLE_MIN_INVERT; }

return deg;

}

int getThrottleAngle(void)

{

double rad;

double compensate_z;

int deg;

rad = asin((double) wiiremote.Report.Accel.Y);

compensate_z = (double) wiiremote.Report.Accel.Z / cos(rad);

rad = asin(compensate_z);

deg = (int) (rad * 180.0 / PI);

/* clipping */

if (deg > THROTTLE_ANGLE_MAX) { deg = THROTTLE_ANGLE_MAX; }

if (deg < THROTTLE_ANGLE_MIN) { deg = THROTTLE_ANGLE_MIN; }

return deg;

}

inline void brake(void)

{

ESC.writeMicroseconds(ESC_PULSE_BRAKE);

delay(15);

ESC.writeMicroseconds(ESC_PULSE_NEUTRAL);

delay(15);

}

inline void shiftUp(void)

{

if (gear < GEAR_3RD) {

gear++;

wiiremote.setLED( (WIIREMOTE_LED1 << (gear – GEAR_1ST)) );

}

}

inline void shiftDown(void)

{

if (gear > GEAR_1ST) {

gear–;

wiiremote.setLED( (WIIREMOTE_LED1 << (gear – GEAR_1ST)) );

}

}

Wii 리모컨(Wiimote)으로 RC Car 조종하기

이젠 벽돌이 되어버린 니텐도 Wii를 바라보다 갑자기 아깝다는 생각이 들어 폭풍 검색을 하다가 Wii 리모컨을 활용할 수 있는 괜찮은 소스를 찾은 것 같아 아래와 같이 소개 합니다.

잘~ 응용하면 재밋는 장난감이 되겠네요.

Circuit@Home에서 소개 받았습니다!

RC car controlled by Wii Remote on Arduino http://www.circuitsathome.com/mcu/rc-car-controlled-by-wii-remote-on-arduino

곳곳에서 Wii 리모컨을 사용한 재미있는 프로젝트를 볼 수 있습니다. PC에서 Bluetooth Stack를 이용한 프로젝트는 많은 있지만, PC를 사용하지 않고 마이컴 (AVR, PIC)에서 Bluetooth Stack를 이용한 프로젝트는 적은 것 같습니다. 그래서 (새삼 느낌입니다 만…) 마리오 카트를 조종하는 느낌으로 무선조종을 만들어 보았습니다. #바나나를 던지거나 하지는 않습니다.

하드웨어 (Hardware)

Arduino에서 Bluetooth Stack을 실현하여 무선조종서보와 ESC를 제어합니다.

송신기 (Transmitter)

  • Wii 리모컨 (Wiimote)

수신기 (Reciver)

  • Arduino
  • USB Host Shield
  • Bluetooth USB 어댑터 (Dongle)

통신 거리는 Bluetooth 특성상 대략 10m 정도 밖에 안될 거라고 예상합니다. 통신 거리를 늘리려면 XBee 등을 사용해야 할 것 입니다.

소프트웨어 (Software)

Arduino 용 라이브러리는 github 에 있습니다.
이 라이브러리는 Richard Ibbotson이 만든 코드 wiiblue.pde 을 기반으로하고 있습니다.
또한 이 라이브러리를 사용하기 위해서는, Oleg Mazurov가 개발 한 라이브러리 가 필요합니다.

샘플 스케치 (Sketch)

 

SterringWii.ino

#include "WiiRemote.h"
#include <MemoryFree.h>
#include <Servo.h>
#define PIN_STEERING_SIGNAL 2
#define PIN_ESC_SIGNAL  3
#define SERIAL_DEBUG 0
enum eAngle {
  STEERING_ANGLE_MAX    = 165,  // to right
  STEERING_ANGLE_MIN    = 15,   // to left
  STEERING_ANGLE_CENTER = 90,
  STEERING_ANGLE_STEP   = 5,
  THROTTLE_ANGLE_MAX    = 80,
  THROTTLE_ANGLE_MIN    = 10,
  THROTTLE_ANGLE_CENTER = 90,
};
enum eServoPulse {
  SERVO_PULSE_NEUTRAL = 1500, // Futaba compatible, 1.55msec
  SERVO_PULSE_MAX     = 1800, // to left
  SERVO_PULSE_MIN     = 1200, // to right
};
enum eESCPulse {
/*
 *  Futaba timing
 *
 *  0us     1072us         1522us          1922us
 *   +---------*------------+-*-+-------------*
 *   |   n/a   |   forwad   |d|d|   Reverse   |
 *   +---------*------------+-*-+-------------*
 *          Max Forwad     Neutral         Max Reverse
 *
 *   d: dead zone, +10us and -10us
 */
  ESC_PULSE_NEUTRAL = 1522,
  ESC_PULSE_BRAKE   = 1600,
  ESC_PULSE_FWD_MAX = 1200, // 1072
  ESC_PULSE_FWD_MIN = 1510,
  ESC_PULSE_FWD_3RD = (ESC_PULSE_FWD_MIN - 200),
  ESC_PULSE_FWD_2ND = (ESC_PULSE_FWD_MIN - 140),
  ESC_PULSE_FWD_1ST = (ESC_PULSE_FWD_MIN - 80),
  ESC_PULSE_REV_FIX = 1650,
  ESC_PULSE_REV_MAX = 1700, // 1922
  ESC_PULSE_REV_MIN = 1600,
};
enum eGear {
  GEAR_1ST = 1,
  GEAR_2ND = 2,
  GEAR_3RD = 3,
};
WiiRemote wiiremote;
Servo SteeringServo;
Servo ESC;
void setup()
{
#if SERIAL_DEBUG
  Serial.begin(9600);
  Serial.print("\r\nfreeMemory() reports: ");
  Serial.print(freeMemory(), DEC);
#endif
  SteeringServo.attach(PIN_STEERING_SIGNAL);
  SteeringServo.writeMicroseconds(SERVO_PULSE_NEUTRAL);
  ESC.attach(PIN_ESC_SIGNAL);
  ESC.writeMicroseconds(ESC_PULSE_NEUTRAL);
  wiiremote.init();
  /*
  unsigned char wiiremote_bdaddr[6] = {0x00, 0x1e, 0x35, 0xda, 0x48, 0xbc};
  wiiremote.setBDAddress(wiiremote_bdaddr, 6);
  wiiremote.setBDAddressMode(BD_ADDR_FIXED);
  */
}
void loop()
{
  wiiremote.task(&myapp);
}
int steering_angle = STEERING_ANGLE_CENTER;
int old_steering_angle = STEERING_ANGLE_CENTER;
bool analog_throttle = false// false = use "One" button as throttle
int throttle_angle = THROTTLE_ANGLE_CENTER;
int gear = GEAR_1ST;
int pulse_steering;
int pulse_esc;
void myapp(void) {
#if SERIAL_DEBUG
  Serial.print("\r\n");
#endif
  /* Steering */
  steering_angle = getSteeringAngle();
  pulse_steering = map(steering_angle,
                       STEERING_ANGLE_MIN, STEERING_ANGLE_MAX,
                       SERVO_PULSE_MAX, SERVO_PULSE_MIN);
  SteeringServo.writeMicroseconds(pulse_steering);
  //delay(15);
  Serial.print("\tServo=");
  Serial.print(pulse_steering);
  /* Brake and Throttle */
  if (wiiremote.buttonPressed(WIIREMOTE_ONE)) {
    if (pulse_esc < ESC_PULSE_NEUTRAL) {
      // moving forward before press "One"
      brake();
      pulse_esc = ESC_PULSE_NEUTRAL;
    } else {
      // while stopping or moving backward, keep moving backward
      pulse_esc = ESC_PULSE_REV_FIX;
    }
  } else {
    if (analog_throttle) {
      throttle_angle = getThrottleAngle();
      pulse_esc = map(throttle_angle,
                      THROTTLE_ANGLE_MIN, THROTTLE_ANGLE_MAX,
                      ESC_PULSE_FWD_MIN, ESC_PULSE_FWD_MAX);
    } else if (wiiremote.buttonPressed(WIIREMOTE_TWO)) {
      switch (gear) {
       case GEAR_1ST:
        pulse_esc = ESC_PULSE_FWD_1ST;
        break;
       case GEAR_2ND:
        pulse_esc = ESC_PULSE_FWD_2ND;
        break;
       case GEAR_3RD:
        pulse_esc = ESC_PULSE_FWD_3RD;
        break;
       default:
        pulse_esc = ESC_PULSE_NEUTRAL;
        break;
      }
    } else {
      pulse_esc = ESC_PULSE_NEUTRAL;
    }
  }
  ESC.writeMicroseconds(pulse_esc);
  //delay(15);
  Serial.print("\tESC=");
  Serial.print(pulse_esc);
  /* Throttle mode */
  if (wiiremote.buttonClicked(WIIREMOTE_A)) {
    analog_throttle = !analog_throttle;
    if (analog_throttle) {
      wiiremote.setLED(WIIREMOTE_LED4); // analog mode
    } else {
      wiiremote.setLED(WIIREMOTE_LED1); // fixed mode, 1st gear
      gear = GEAR_1ST;
    }
  }
  /* Shift up or down */
  if (!analog_throttle) {
    if (wiiremote.buttonClicked(WIIREMOTE_RIGHT)) {
      shiftUp();
    } else if (wiiremote.buttonClicked(WIIREMOTE_LEFT)) {
      shiftDown();
    }
  }
} // myapp
int getSteeringAngle(void) {
  double rad;
  int deg;
  rad = acos((double) wiiremote.Report.Accel.Y);
  deg = (int) (rad * 180.0 / PI);
  /* clipping */
  if (deg > STEERING_ANGLE_MAX) { deg = STEERING_ANGLE_MAX; }
  if (deg < STEERING_ANGLE_MIN) { deg = STEERING_ANGLE_MIN; }
  return deg;
}
int getThrottleAngle(void) {
  double rad;
  double compensate_z;
  int deg;
    rad = asin((double) wiiremote.Report.Accel.Y);
    compensate_z = (double) wiiremote.Report.Accel.Z / cos(rad);
    rad = asin(compensate_z);
    deg = (int) (rad * 180.0 / PI);
  /* clipping */
  if (deg > THROTTLE_ANGLE_MAX) { deg = THROTTLE_ANGLE_MAX; }
  if (deg < THROTTLE_ANGLE_MIN) { deg = THROTTLE_ANGLE_MIN; }
  return deg;
}
inline void brake(void)
{
    ESC.writeMicroseconds(ESC_PULSE_BRAKE);
    delay(15);
    ESC.writeMicroseconds(ESC_PULSE_NEUTRAL);
    delay(15);
}
inline void shiftUp(void)
{
    if (gear < GEAR_3RD) {
        gear++;
        wiiremote.setLED( (WIIREMOTE_LED1 << (gear - GEAR_1ST)) );
    }
}
inline void shiftDown(void)
{
    if (gear > GEAR_1ST) {
        gear--;
        wiiremote.setLED( (WIIREMOTE_LED1 << (gear - GEAR_1ST)) );
    }
}