File content as of revision 1:a4ab99c0f7ea:

// Tamagawaseiki IMU Demo02
// 3Servo Demonstration      
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
PwmOut servo1(p21);
PwmOut servo2(p22);
PwmOut servo3(p23);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
Serial pc(USBTX, USBRX); // tx, rx 
Serial IMU(p9,p10);// p9=tx,p10=rx

double rx,px,ax;
double rx_imu,px_imu,ax_imu; 

void IMU_get(){
      
      unsigned char c1,c2;
      int i;
      
      while(IMU.getc()!='$'){
      }
      for(i=1;i<=8;i++){
        c1=IMU.getc();
      }
      for(i=1;i<=20;i++){
        c1=IMU.getc();
      }
      c1=IMU.getc();
      c2=IMU.getc();
      rx_imu=float((c1 << 8) | c2)*180/32768;

      c1=IMU.getc();
      c2=IMU.getc();
      px_imu=float((c1 << 8) | c2)*180/32768;

      c1=IMU.getc();
      c2=IMU.getc();
      ax=float((c1 << 8) | c2)*180/32768;
    
    //for rx  
      if( rx_imu >= 0 && rx_imu <= 90){
        rx=rx_imu+90;
      }
      else if( rx > 90 && rx < 180){
        //rx=180;
      }
      else if( rx >= 180 && rx < 270){
        //rx=0;
      }
      else if(rx_imu >= 270 && rx_imu <= 360){
        rx=rx_imu-270;
      }
      
    //for px
      if( px_imu >= 0 && px_imu <= 90){
        px=px_imu+90;
      }
      else if( px > 90 && px < 180){
        //px=180;
      }
      else if(px >=180 && px < 270){
        //px=0;
      }
      else if(px_imu >= 270 && px_imu <= 360){
        px=px_imu-270;
      }
      
    // for ax
      if(ax >= 0 && ax <=90){
        ax=ax+90;
      }
      else if(ax >90 && ax <180){
        ax=180;
      }
      else if(ax >= 180 && ax <270){
        ax=0;
      }
      else if(ax >= 270 && ax <=360){
        ax=ax-270;
      }
 
}

int main() {

      int i,j,k;
      int rx_lc=9,rx_lc_old=9;
      int px_lc=9,px_lc_old=9;
      int ax_lc=9,ax_lc_old=9;
      double rx_pwidth,px_pwidth,ax_pwidth;
      
      servo1.period_ms(20);
      servo2.period_ms(20);
      servo3.period_ms(20);
      
      //IMU.baud(57600);  
      IMU.baud(119200);
      lcd.cls();
      lcd.locate(0,0);
      lcd.printf("Tamagawaseiki IMU\n");
      lcd.printf("3Servo Demo Ver.02\n");
      lcd.printf("System start ");
      
      servo1.pulsewidth(1.5/1000);
      servo2.pulsewidth(1.5/1000);
      servo3.pulsewidth(1.5/1000);

      //IMU.printf("$TSC,OFC,10*CC\r\n");                 
 
      IMU.printf("$TSC,RAW,50\r\n");// IMU data output start
      for(i=0;i<5;i++){
        lcd.printf(".");
        IMU.printf("$TSC,RAW,50\r\n");
        wait(1.0);
      } 
      
      IMU.printf("$TSC,HRST*75\r\n");// ax reset
          
      IMU_get(); 
      lcd.cls();
      wait(0.1);      
            
    while (1) {
      IMU_get();
    
    //for rx  
      if( rx >= 88 && rx <= 92){
        rx=90;
      }
      if( rx >= 170 && rx < 180){
        rx=170;
      }
      else if( rx <=10 && rx > 0){
        rx=10;
      }

   //for px
      if( px >= 88 && px <= 92){
        px=90;
      }   
      if( px >= 130 ){
        px=130;
      }
      else if(px < 50 ){
        px=50;
      }
              
      lcd.locate(0,0);
      lcd.printf("%5.1f,%5.1f,%6.2f\n",rx,px,ax);

      rx_lc=rx/10; 
      px_lc=px/10;
      ax_lc=ax/10;     
 
      ax=-ax+180;//servo pulse reverse
      
      rx_pwidth=rx/180/1000+0.001;
      px_pwidth=px/180/1000+0.001;
      ax_pwidth=ax/180/1000+0.001;      
      
      servo1.pulsewidth(rx_pwidth);
      servo2.pulsewidth(px_pwidth);
      servo3.pulsewidth(ax_pwidth);
      
      lcd.locate(0,1);
      //lcd.printf("---------+---------");
      //lcd.printf("R:%4.2fms,P:%4.2fms",rx_pwidth*1000,px_pwidth*1000);    
      lcd.locate(rx_lc_old,1);
      lcd.printf(" ");
      lcd.locate(rx_lc,1);
      lcd.printf("*");
      rx_lc_old=rx_lc;
      lcd.locate(px_lc_old,2);
      lcd.printf(" ");
      lcd.locate(px_lc,2);
      lcd.printf("*");
      px_lc_old=px_lc;
      lcd.locate(ax_lc_old,3);
      lcd.printf(" ");
      lcd.locate(ax_lc,3);
      lcd.printf("*");
      ax_lc_old=ax_lc;
              
    }//while
}//main