File content as of revision 7:6aaf33eac704:

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

//for debug
    Serial pc(USBTX,USBRX);
    DigitalOut led2(LED2);
//for communication
    myCAN can(p30, p29);
    MU2Class MyMu2(p9,p10);
    DigitalOut MU2_permit(p11);
    Ticker Send_Call;
//for drive actuator
    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 landing
    Timeout landing_timeout;
//for mission
    DigitalOut led1(LED1);
    DigitalOut led3(LED3);
    DigitalOut led4(LED4);

//for pc
    #define PC_BAUD 9600
    #define COMMUNICATION_RATE 0.3 //sec
    void data_send(void);
   
//for PWM
    #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);

//for landing
    #define DEADLINE 30.0 //sec
    void landing(void);
    char landing_flg=0;
    
//for mission
    int mission_status=0;
    
int main() {
//////////Init///////////////////////////////////////////////////////////////////////////////
    //for pc
        pc.baud(PC_BAUD);

    //correspondence off
        MU2_permit = 1;//off
        
    //PWM init
        nikuromu1.period(N_PERIOD);
        NIKUROMU1_OFF
        nikuromu2.period(N_PERIOD);
        NIKUROMU2_OFF
        nikuromu3.period(N_PERIOD);
        NIKUROMU3_OFF
  
    //servo init
        for(int i=0;i<10;i++){
            servo.TORQUE_ON(2); //for rotation
            servo.Rotate_Servo_Float(2,-150.0);
            wait(0.01);
        }
  
    //for debug test
        //servo.Rotate_Servo_Float_Test(1);
        //servo.ID_CHANGE(1,2);
  
///////Sequence////////////////////////////////////////////////////////////////////////
    
    //0.MU2 check
        mission_status = 0;
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MU2_permit = 0;//on
        wait(1.0);//wait until mu2 will be steady
        MyMu2.send("we will start cansat mission soon.\r\n");
        MyMu2.send("this message is MU2 confirmation.\r\n");
        MyMu2.send("we will turn off MU2 for  regulation afetr 3 sec.\r\n");
        MyMu2.send("3\r\n");
        wait(1);
        MyMu2.send("2\r\n");
        wait(1);
        MyMu2.send("1\r\n");
        wait(1);
        MyMu2.send("0 please check MU2 is off \r\n");
        MU2_permit = 1;//off
        Send_Call.attach(&data_send,COMMUNICATION_RATE);
        
    
    //1.freeze sequence while flightpin are fixed
        mission_status = 1;
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        while(flightpin == 1){
            led1 = !led1;
            wait(0.1);
            led2 = !led2;
            wait(0.1);
            led3 = !led3;
            wait(0.1);
            led4 = !led4;
            wait(0.1);
        }       
        //after emission    
            MU2_permit = 0;//on
            wait(0.5);//wait until mu2 will be steady
            servo.TORQUE_ON(1);
            servo.TORQUE_ON(2);
            servo.TORQUE_ON(3);
            MyMu2.send("emission finished!\r\n");
            
            
    //2.fix mechanism start
            mission_status = 2; 
            can.make_mission_senddata(mission_status);
            can.send(MISSION);
            MyMu2.send("start fix mechanism\r\n");
            NIKUROMU1_ON
            wait(6.0);
            NIKUROMU1_OFF
            MyMu2.send("finish fix mechanism\r\n");
            wait(2.0);
            // rotate 22deg to avoid nails return
//            servo.Rotate_Servo_Float(2,
    
    //3.check landing
        mission_status = 3; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        int roll_data[10];
        int roll_average;
        int roll_breakup;
        landing_timeout.attach(&landing, DEADLINE);
        while(landing_flg==0){
            MyMu2.send("falling...\r\n");
            roll_average=0;
            roll_breakup=0;
            for(int i=0;i<10;i++){
                roll_data[i]=can.get_roll();
                roll_average += roll_data[i];
                wait(0.5);
            }
            roll_average = roll_average/10;
            for(int i=0;i<10;i++){
                roll_breakup += (roll_data[i]-roll_average)*(roll_data[i]-roll_average);
            }
            if(roll_breakup < 10){
                landing_flg=1;
            }
        }
        
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
    
        wait(5);//rest
        
    //4.rotation mechanism start 
        mission_status = 4; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MyMu2.send("start rotation mechanism\r\n");
        servo.TORQUE_ON(2);
        servo.Rotate_Servo_Float(2,-150.0);
        float theta;
        theta = -150.0 + 360.0 - can.get_roll();
        if(theta < 150.0){servo.Rotate_Servo_Float(2,theta);}   
        MyMu2.send("finish rotation mechanism\r\n");
        
        wait(5);//rest
    
   //5.open door   
        mission_status = 5; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MyMu2.send("start opening door\r\n");   
        NIKUROMU2_ON
        wait(6.0);
        NIKUROMU2_OFF
        MyMu2.send("finish opening door\r\n");
        
        wait(5);//rest
    
    //6.develope parabolic antenna
        mission_status = 6; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MyMu2.send("start developing parabolic antenna\r\n");  
        NIKUROMU3_ON
        wait(6.0);
        NIKUROMU3_OFF
        MyMu2.send("finish developing parabolic antenna\r\n");  
        
        wait(5);//rest
    
    //7.start RHI scaning 
        mission_status = 7; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MyMu2.send("start RHI scaning\r\n");
        RHI_Scan();  
        
    //8.end of sequence
        mission_status = 8; 
        can.make_mission_senddata(mission_status);
        can.send(MISSION);
        MyMu2.send("END!\r\n");
}

void RHI_Scan(void){
    servo.TORQUE_ON(1);
    servo.TORQUE_ON(3);
    
    while(1){
        for(int j=0;j<100;j++){
            for(int i=0;i<181;i++){
                servo.Rotate_Servo_Float(1,j*1.0);
                servo.Rotate_Servo_Float(3,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(3,-90.0+i*1.0);
                wait(0.01);
            }
            wait(0.05);
        }
    }
}

void data_send(void){
    pc.printf("%s,%s,%s,%d,%d,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_roll(),can.get_pitch(),mission_status);
    
    char mu2data[100];
    mu2data[0] = *(can.get_time()+0);
    mu2data[1] = *(can.get_time()+1);
    mu2data[2] = *(can.get_time()+2);
    mu2data[3] = *(can.get_time()+3);
    mu2data[4] = *(can.get_time()+4);
    mu2data[5] = *(can.get_time()+5);
    mu2data[6] = *(can.get_time()+6);
    mu2data[7] = *(can.get_time()+7);
        
    mu2data[8]=',';
        
    mu2data[9] = *(can.get_latitude()+0);
    mu2data[10] = *(can.get_latitude()+1);
    mu2data[11] = *(can.get_latitude()+2);
    mu2data[12] = *(can.get_latitude()+3);
    mu2data[13] = *(can.get_latitude()+4);
    mu2data[14] = *(can.get_latitude()+5);
    mu2data[15] = *(can.get_latitude()+6);
    mu2data[16] = *(can.get_latitude()+7);
    mu2data[17] = *(can.get_latitude()+8);
        
    mu2data[18]=',';
        
    mu2data[19] = *(can.get_longitude()+0);
    mu2data[20] = *(can.get_longitude()+1);
    mu2data[21] = *(can.get_longitude()+2);
    mu2data[22] = *(can.get_longitude()+3);
    mu2data[23] = *(can.get_longitude()+4);
    mu2data[24] = *(can.get_longitude()+5);
    mu2data[25] = *(can.get_longitude()+6);
    mu2data[26] = *(can.get_longitude()+7);
    mu2data[27] = *(can.get_longitude()+8);
    mu2data[28] = *(can.get_longitude()+9);
    
    mu2data[29] = ',';
    
    mu2data[30] = ((can.get_NoS())/10)+0x30;
    mu2data[31] = (can.get_NoS()%10)+0x30;
    
    mu2data[32] = ',';
    
    mu2data[33]= (can.get_roll()/100)+0x30;
    mu2data[34]=((can.get_roll()%100 - can.get_roll()%10)/10)+0x30;
    mu2data[35]=(can.get_roll()%10)+0x30;;
        
    mu2data[36]= mission_status +0x30;
    
    mu2data[37]='\r';
    mu2data[38]='\n';
    mu2data[39]='\0';
        
    MyMu2.send(mu2data);
}

void landing(void){
    landing_flg=1;
    MyMu2.send("landing_timeout\r\n");
}