File content as of revision 0:b90181e726c7:

#include "mbed.h"
#include "QEI.h"
#include "RS405cb.h"

#define PI 3.141592653589793

Serial pc(USBTX, USBRX);
DigitalOut debug1(LED1);
DigitalOut debug2(LED2);
DigitalOut debug3(LED3);
DigitalOut debug4(LED4);
RS405cb servo(p13,p14,p15);

double Theta1 = 128, Theta2 = 128, Theta = 128;

void setup(){
    servo.TORQUE_ON(1);
    servo.TORQUE_ON(2);
    servo.TORQUE_ON(3);
    servo.Rotate_Servo_Float(1,0.0);
    servo.Rotate_Servo_Float(2,0.0);
    servo.Rotate_Servo_Float(3,0.0);
    pc.format(8,Serial::None,1);
}
void izumi(){
    //0~255 to -180~180
    Theta1 = Theta1 * 360.0 / 255.0 - 180.0;
    Theta2 = Theta2 * 360.0 / 255.0 - 180.0;
    Theta = (Theta * 360.0 / 255.0 - 180.0) / 2;    //gear 1:2
}    
void robot_arm(int theta_1, int theta_2,int theta,int theta0_1,int theta0_2 , int theta0) {
    //theta_1 : 1 ban ue no arm
    //theta_2 : mannnaka no arm
    //theta   : 1 ban sita no arm
    for(int i=0; i<=120; i++) {
        double m1,m2,m3;
        m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
        m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
        m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
        servo.Rotate_Servo_Float(1,m1);
        servo.Rotate_Servo_Float(2,m2);             
        servo.Rotate_Servo_Float(3,m3);       
        wait(0.01);
    }
}

int main() {
    setup();
    
    pc.putc(0);         //Image analysis order
    //wait(7.0);          //Waiting analysis. 
    int Wait = pc.getc();
    debug1 = 1;
    pc.putc(1);         //Theta get order
    Theta1 = pc.getc();   //Get Theta1
    debug2 = 1;
    
    pc.putc(2);         //Theta2 get order
    Theta2 = pc.getc();   //Get Theta2
    debug3 = 1;
    
    pc.putc(3);         //Theta1 get order
    Theta = pc.getc();   //Get Theta
    debug4 = 1;

    izumi();
    while(1){
        robot_arm(Theta1, Theta2, Theta,0,0,0);
        wait(2.0);
        robot_arm(0,0,0,Theta1,Theta2,Theta);
        wait(2.0);
    }
}