File content as of revision 0:7857b4c95c75:

#include "mbed.h"
#include "RS405cb.h" 
#include "MU2Class.h"

//for debug
    Serial pc(USBTX,USBRX);
    DigitalOut led2(LED2);
//for communication
    CAN can(p30, p29);
    MU2Class MyMu2(p9,p10);
    DigitalOut MU2_permit(p11);
//for drive actuator
    //PwmOut motor1(p21);
    RS405cb servo(p13,p14,p18);//TX,RX,PERMIT
//for fusing PWM
    PwmOut nikuromu1(p22);
    PwmOut nikuromu2(p23);
    PwmOut nikuromu3(p24);
//for Flight confirmation
    DigitalIn flightpin(p26);
 
//for CAN
    #define CAN_BAUD 4000000
    int  id;
    char smsg[8];
 
//for PWM
    #define M_PERIOD 0.01 
    #define MOTOR_OFF motor1.pulsewidth(0.0);
    #define N_PERIOD 0.01
    #define NIKUROMU1_OFF nikuromu1.pulsewidth(0.0);
    #define NIKUROMU2_OFF nikuromu2.pulsewidth(0.0);
    #define NIKUROMU3_OFF nikuromu3.pulsewidth(0.0);
    #define NIKUROMU1_ON nikuromu1.pulsewidth(0.003);
    #define NIKUROMU2_ON nikuromu2.pulsewidth(0.003);
    #define NIKUROMU3_ON nikuromu3.pulsewidth(0.003);

//for servo 
    void RHI_Scan(void);
    
int main() {
    //correspondence off
        MU2_permit = 1;//off
        
    //PWM init
        //motor1.period(M_PERIOD);
        //MOTOR_OFF
        nikuromu1.period(N_PERIOD);
        NIKUROMU1_OFF
        nikuromu2.period(N_PERIOD);
        NIKUROMU2_OFF
        nikuromu3.period(N_PERIOD);
        NIKUROMU3_OFF
        
    //for CAN init 
        CANMessage msg;
        can.frequency(CAN_BAUD);
        
        
    //for servo test
        //led2 =1;
        //RHI_Scan();
        //servo.Rotate_Servo_Float_Test(3);
        //servo.ID_CHANGE(1,3);
        /*
        float theta;
        theta=-150.0;
        servo.TORQUE_ON(3);
        servo.Rotate_Servo_Float(3,theta);
        while(1) {
            if(can.read(msg)) {
                pc.printf("Message received: %d,%d\n\r", msg.data[0],msg.data[2]);
                if(msg.data[2]!=1){
                    servo.TORQUE_ON(3);
                    theta += 1.0;
                    servo.Rotate_Servo_Float(3,theta);
                    //wait(0.02);
                }
                else{servo.TORQUE_ON(3);}
                led2 = !led2;
            } 
        }
        */
        
    //Sequence
    
    while(flightpin ==1){}
    
    MU2_permit = 0;//on
    
    wait(5);
    
    
    id=1;
    smsg[0]=1;
    can.write(CANMessage(id,smsg)); 
    
    NIKUROMU1_ON
    wait(6.0);
    NIKUROMU1_OFF
    
    wait(10);
    
    float theta;
    theta=-150.0;
    servo.TORQUE_ON(3);
    servo.Rotate_Servo_Float(3,theta);
    while(1) {
        if(can.read(msg)) {
            pc.printf("Message received:%d,%d,%d\n\r",msg.id, msg.data[0],msg.data[2]);
            if(msg.data[2]!=1){
                servo.TORQUE_ON(3);
                theta += 1.0;
                servo.Rotate_Servo_Float(3,theta);
            }
            else{
                servo.TORQUE_ON(3);
                break;
            }
            led2 = !led2;
         } 
    }
    
    
    wait(5.0);
    NIKUROMU2_ON
    wait(6.0);
    NIKUROMU2_OFF
    
    wait(5.0);
    NIKUROMU3_ON
    wait(6.0);
    NIKUROMU3_OFF
    
    RHI_Scan();   
    
    //MyMu2.send("Hello.This is mbed. ");   
}

void RHI_Scan(void){
    servo.TORQUE_ON(1);
    servo.TORQUE_ON(2);
    
    while(1){
        for(int j=0;j<30;j++){
            for(int i=0;i<181;i++){
                servo.Rotate_Servo_Float(1,j*1.0);
                servo.Rotate_Servo_Float(2,90.0-i*1.0);
                wait(0.01);
            }
            for(int i=0;i<181;i++){
                servo.Rotate_Servo_Float(1,j*1.0);
                servo.Rotate_Servo_Float(2,-90.0+i*1.0);
                wait(0.01);
            }
            wait(0.05);
        }
    }
}