ES20X mbed board I/O Tester

include the mbed library, and SERVOGEN.h with this snippet

// J. Bradshaw 20141119
// Program for testing I/O on mbed ES200 breakout board
#include "mbed.h"
#include "SERVOGEN.h"   //uses SERVOGEN.h library for generating servo pulses

DigitalOut myled(LED1);

//PC serial connection
Serial pc(USBTX, USBRX);    //tx, rx via USB connection
Serial DB9_female(p9, p10); //DB9 serial female
Serial DB9_male(p13, p14); //DB9 serial male

//Port J2
BusOut bus_J2(p5, p6, p7, p8, p11);

//header for J2
DigitalOut J2_p5(p5);
DigitalOut J2_p6(p6);
DigitalOut J2_p7(p7);
DigitalOut J2_p8(p8);
DigitalOut J2_p11(p11);

//Analog port in J3
AnalogIn    J3_P16(p16);
AnalogIn    J3_P17(p17);
AnalogIn    J3_P18(p18);
AnalogIn    J3_P19(p19);
AnalogIn    J3_P20(p20);

// servo ports p21 - p24
SERVOGEN sv1(p21);
SERVOGEN sv2(p22);
SERVOGEN sv3(p23);
SERVOGEN sv4(p24);

DigitalOut mot1_ph1(p28);
DigitalOut mot1_ph2(p27);
PwmOut mot_en1(p25);

DigitalOut mot2_ph1(p29);
DigitalOut mot2_ph2(p30);
PwmOut mot_en2(p26);
//Motor control routine for PWM on 5 pin motor driver header
// drv_num is 1 or 2 (defaults to 1, anything but 2)
// dc is signed duty cycle (+/-1.0)

void mot_control(int drv_num, float dc){        
    if(dc>1.0)
        dc=1.0;
    if(dc<-1.0)
        dc=-1.0;
    
    if(drv_num != 2){           
        if(dc > 0.0){
            mot1_ph2 = 0;
            mot1_ph1 = 1;
            mot_en1 = dc;
        }
        else if(dc < -0.0){
            mot1_ph1 = 0;
            mot1_ph2 = 1;
            mot_en1 = abs(dc);
        }
        else{
            mot1_ph1 = 0;
            mot1_ph2 = 0;
            mot_en1 = 0.0;
        }
    }                
    else{
        if(dc > 0.0){
            mot2_ph2 = 0;
            mot2_ph1 = 1;
            mot_en2 = dc;
        }
        else if(dc < -0.0){
            mot2_ph1 = 0;
            mot2_ph2 = 1;
            mot_en2 = abs(dc);
        }
        else{
            mot2_ph1 = 0;
            mot2_ph2 = 0;
            mot_en2 = 0.0;
        }
    }                   
}

//------------ Main ------------------------------
int main() {
    float analogVal[6];
    
    pc.baud(9600);
    DB9_female.baud(9600);
    DB9_male.baud(9600);
    mot_en1.period_us(20);
    mot_en2.period_us(20);
    
    while(1) {        
        bus_J2 = 0x1f;
        analogVal[0] = J3_P16;
        analogVal[1] = J3_P17;
        analogVal[2] = J3_P18;
        analogVal[3] = J3_P19;
        analogVal[4] = J3_P20;
        pc.printf("%.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);
        DB9_female.printf("Female %.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);
        DB9_male.printf("Male %.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);
        
        //toggle bus I/O for testing
        wait(.5);
        bus_J2 = 0xAA;
        wait(.1);
        bus_J2 = 0x55;
        wait(.1);
        bus_J2 = 0xAA;
        wait(.1);
        bus_J2 = 0x55;
        wait(.1);        
        bus_J2 = 0; //all off
        wait(.5);
            
        pc.printf("\r\n");
        // Run PWM and servo command signals
        for(float pos=0.0;pos<1.0;pos+=.034){
            sv1 = pos * 1000 + 1000;
            sv2 = pos * 1000 + 1000;
            sv3 = pos * 1000 + 1000;
            sv4 = pos * 1000 + 1000;

            mot_control(1, (pos*2.0) - 1.0);
            mot_control(2, (pos*2.0) - 1.0);
                        
            pc.printf("pos = %.2f  \r", pos);
            wait(.06);
        }
        for(float pos=1.0;pos>0.0;pos-=.034){
            sv1 = pos * 1000 + 1000;    
            sv2 = pos * 1000 + 1000;
            sv3 = pos * 1000 + 1000;
            sv4 = pos * 1000 + 1000;
            
            mot_control(1, (pos*2.0) - 1.0);
            mot_control(2, (pos*2.0) - 1.0);
            
            pc.printf("pos = %.2f  \r", pos);
            wait(.06);        
        }
    }
}