File content as of revision 0:2d7d98ccddfa:

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

RawSerial pc(USBTX, USBRX);

#define SERVO1_PIN p22

#define MAX_PULSE 0.00245//[s]
#define MIN_PULSE 0.00065//[s]
Servo servo1(SERVO1_PIN, MAX_PULSE, MIN_PULSE);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

DigitalOut nicrom(p21);

void angleChange(){
    const int BUFFER_SIZE = 16;
    static float angle = 0;
    static char buff[BUFFER_SIZE] = {'\0'};
    static int count = 0;
    static bool startFlag = false;
    
    char temp = pc.getc();   
    if(temp == 'u') nicrom = 1;
    else if(temp == 'd')    nicrom = 0;
    pc.printf("%c", temp);
    if(temp == '@'){
        startFlag = true;
        memset(buff, '\0', BUFFER_SIZE);
        
        return;
    }
    if(startFlag){
        
        if(temp == '\r'){
            buff[count] = '\0';
            startFlag = false;
            angle = (float)atof(buff);
            servo1 = angle;
            pc.printf("\r\nangle : %3.2f [degree]\r\n", servo1());
            pc.printf("Please send like @***.*<CR><LF>\r\n\r\n");
            pc.printf("---------------------------------------\r\n");
            /*
            if(angle < -90.0)   angle = -90.0f;
            else if(angle > 90.0)  angle = 90.0f;
            servo1.position(angle);
            pc.printf("angle : %3.0f [degree]\r\n", angle);*/
               
            memset(buff, '\0', BUFFER_SIZE);
            count = 0;
            return;
        }   
        buff[count] = temp;
        count++;
        if(count > BUFFER_SIZE - 1){//null文字分空けておく
            startFlag = false;
            memset(buff, '\0', BUFFER_SIZE);
            count = 0;
            pc.printf("\r");
            pc.printf("\033[K");
            return; 
        }
        
    }
}

void ledDriver(unsigned char bin){
    unsigned char cmd = bin & 0b00000001;
    if(cmd) led1 = 1;
    else    led1 = 0;
    cmd = bin & 0b00000010;
    if(cmd) led2 = 1;
    else    led2 = 0;            
    cmd = bin & 0b00000100;
    if(cmd) led3 = 1;
    else    led3 = 0;
    cmd = bin & 0b00001000;
    if(cmd) led4 = 1;
    else    led4 = 0;
}

int main() {
    
    wait(3.0);
    
    pc.baud(115200);
    pc.attach(&angleChange, Serial::RxIrq);
    //char count = 0b00000001;
    pc.printf("\r\nStart boot phase:\r\n");
    for(int i = 0; i < 100; i++){
        pc.printf("\033[K");
        pc.printf("Loading... : %3d [%%]\r", i);
        wait(0.01);   
    }
    pc.printf("Loading... : %3d [%%]\r\n", 100);
    pc.printf("  -> OK!\r\n");
    wait(1.0);
    pc.printf("Initialize...\r\n");
    for(int i = 0;i < 20; i++){
        ledDriver(0b00001111);
        wait(0.05);
        ledDriver(0b00000000);
        wait(0.05);   
    }
    servo1 = 0.0f;
    pc.printf("angle : %3.2f [degree]\r\n", servo1());
    pc.printf("  -> OK!\r\n");
    wait(0.5);
    
    pc.printf("End boot phase\r\n");
    wait(1.0);
    pc.printf("Start _Servo Driving System ver 1.5.1_\r\n");
    pc.printf("\r\nPlease send like @***.*<CR><LF>, ***.* = angle[degree]\r\n");
   
    while(1) {    
        ledDriver(0b00000001);
        wait(0.2);
        ledDriver(0b00000010);
        wait(0.2);
        ledDriver(0b00000100);
        wait(0.2);
        ledDriver(0b00001000);
        wait(0.2);
    }
}