File content as of revision 67:6e183e1e9bcd:

#include "mbed.h"
#include "WiiNunchuck.h"
#include "Servo.h"

#define LED1 PC_10
#define LED2 PC_12
#define LED3 PC_11

#define SERVO1 PA_6
#define SERVO2 PA_7
#define SERVO3 PB_11
#define SERVO4 PB_1
#define SERVO5 PB_10
#define SERVO6 PB_3

//#define PWM_PERIOD 20 // ms
//#define PULSE_MIN 500.0 // us
//#define PULSE_MAX 2500.0 // us


#define SWITCH1 PC_8
#define SWITCH2 PC_6

#define SDA PC_1
#define SCL PC_0

#define POW_EN PA_4

WiiNunchuck nchk(SDA, SCL);
Serial pc(USBTX, USBRX);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalInOut pow_en(POW_EN);

InterruptIn switch1(SWITCH1);
InterruptIn switch2(SWITCH2);

Servo servo_vert(SERVO1);
Servo servo_horz(SERVO2);
Servo servo_rot(SERVO4);
Servo servo_grab(SERVO5);

int accx, accy, accz;
unsigned char joyx, joyy;
bool buttonx, buttonz;

void init();
void switch1_irq();
void switch2_irq();
void enable_servos(bool on);
void servo_angle(PwmOut *servo, float angle);

bool servos_enabled = false;


// main() runs in its own thread in the OS
int main()
{
    init();
    
    while(1)
    {
        char key = pc.getc();
        if(key==0x1B)
        {                               //If the keypress was an arrow key
            key = pc.getc();                        //Check again!
            if (key == 0x5B)
            {
                key = pc.getc();
                switch(key) {                         //Check to see which arrow key...
                    case 0x41:                        //It was the UP arrow key...
                        servo_vert = servo_vert + 0.01;
                        pc.printf("vert %f", servo_vert.read());
                        break;
                    case 0x42:                        //It was the DOWN arrow key...
                        servo_vert = servo_vert - 0.01;
                        pc.printf("vert %f", servo_vert.read());
                        break;
                    case 0x43:                        //It was the RIGHT arrow key...
                        servo_rot = servo_rot + 0.01;
                        pc.printf("\n\r RIGHT!");
                        break;
                    case 0x44:                        //It was the LEFT arrow key...
                        servo_rot = servo_rot - 0.01;
                        pc.printf("\n\r LEFT!");
                        break;
                }
            }
        }
        
        wait(0.01);  
    }
}

void init()
{
    pow_en.output();
    pow_en.mode(OpenDrain);
    enable_servos(false);
    
    servo_vert.calibrate(0.0007, 90)

    servo_vert = 0.5;
    servo_horz = 0.5;
    servo_rot = 0.5;
    servo_grab = 0.5;
    /*servo_horz.position(90);
    servo_rot.position(90);
    servo_grab.position(90);*/
    
    switch1.mode(PullUp);
    switch1.fall(&switch1_irq);
    switch2.mode(PullUp);
    switch2.fall(&switch2_irq);
    
    //led1 = 1;
}

void switch1_irq()
{
    if(servos_enabled)
        enable_servos(false);
    else
        enable_servos(true);
}

void switch2_irq()
{
}

void enable_servos(bool on)
{
    if(on)
    {
        pow_en.output();
        pow_en = 0;
        servos_enabled = true;
        led2 = 1; 
    }
    else
    {
        pow_en = 1;
        //pow_en.mode(OpenDrain);
        pow_en.input();
        servos_enabled = false;
        led2 = 0; 
    }
}