File content as of revision 2:531e6b0346e6:

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

/*---------------------------------------------------
Created by ohtake_i
This program is for ToyBox.
Nomal routine waits for commands from PC.
----------------------------------------------------*/

//for serials---------------------------------------------------
    Serial pc(USBTX,USBRX); //for debug
    Serial MyXbee(p9,p10); //for communication with pc and machine
//-----------------------------------------------------------

//for LEDs-----------------------------------------
    DigitalOut debug1(LED1); //for debug
    DigitalOut debug2(LED2); //for debug
    DigitalOut debug3(LED3); //for debug
    DigitalOut debug4(LED4); //for debug
//---------------------------------------------

//for servo-----------------------------------------
    RS405cb servo(p13,p14,p15);    //TX,RX,PERMIT  (PERMIT means RE/DE Pin of ltc485)
                               //can control two servo motors.
//---------------------------------------------

//for Electronical Valve-----------------------------------------
    PwmOut valve(p25);
    #define VALVE_OPEN valve=1.0;
    #define VALVE_CLOSE valve=0.5;
//---------------------------------------------

//for EMG30-----------------------------------------
    //robot_arm
    QEI arm_enc(p29, p30, NC, 624);
    PwmOut arm(p22);
    //jump_mechanism
    //QEI jump_enc(p27, p28, NC, 624);
    PwmOut jump(p23);
//---------------------------------------------

//for control_angle
    void Control_Motor();
    void Sample_Move();
    //void Control_DCMotor(double desired_value, double offset, double speed_control);
    void Control_DCMotor(int desired_value, int offset, double speed_control);

int main() {
    while(1) {
        printf("Start\r\n");
        Control_DCMotor(180.0,5.0,0.05);
        //Sample_Move();
        //VALVE_OPEN;
        //VALVE_CLOSE;
        //servo.TORQUE_ON(1);
        //servo.Rotate_Servo_Float(1,0.0);   
    }
}

//Servo performance test
void Sample_Move(void) {
    servo.TORQUE_ON(1);
    servo.TORQUE_ON(2);
    while(1) {
        servo.Rotate_Servo_Float(1,0.0);
        servo.Rotate_Servo_Float(2,0.0);
        wait(5);
        for(int j=0; j<60; j++) {
            for(int i=0; i<120; i++) {
                servo.Rotate_Servo_Float(1,j*1.0);
                servo.Rotate_Servo_Float(2,60.0-i*1.0);
                wait(0.05);
            }
            for(int i=0; i<120; i++) {
                servo.Rotate_Servo_Float(1,j*1.0);
                servo.Rotate_Servo_Float(2,-60.0+i*1.0);
                wait(0.05);
            }
        }
    }
}

//Control z-axis angle of rotation
//void Control_DCMotor(double desired_value, double offset, double speed_control) {
void Control_DCMotor(int desired_value, int offset, double speed_control) {
    //double tmp = abs(arm_enc.getPulses());  //get rev value
    printf("Control_DCMotor_Start\r\n");
    while(1) {
        //if(desired_value - offset <= tmp && tmp <= desired_value + offset) {
        if(desired_value - offset <= abs(arm_enc.getPulses()) && abs(arm_enc.getPulses()) <= desired_value + offset) {
            arm = 0.5;
            break;
            printf("1 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
        } 
        //else if(tmp < desired_value - offset) {
        else if(abs(arm_enc.getPulses())
         < desired_value - offset) {
            arm = 0.5 + speed_control;
            printf("2 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
        } else {
            arm = 0.5 - speed_control;
            printf("3 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
        }
    }
} 
 
//Control Command Servo & DCMotor for robot_arm
void robot_arm(double theta_1, double theta_2, double theta_3) {
    //theta_1 : z-axis angle of rotation, for EMG30
    //theta_2 : x-axis angle of rotation, for RS401CR
    //theta_3 : x-axis angle of rotation, for RS301CR
    double x, y, c[2];
    while(1) {
        printf("Please Enter the X-coordinate \r\n");
        scanf("%lf", &c[0]);
        printf("Please Enter the Y-coordinate \r\n");
        scanf("%lf", &c[1]);
        break;
    }
    x = c[0];
    y = c[1];
    pc.printf("x = %lf  y = %lf\r\n", x, y);    //for debug
}