Diff: main.cpp

Revision:

7:6aaf33eac704

Parent:

6:c206e90971e1

--- a/main.cpp	Mon Aug 05 08:58:22 2013 +0000
+++ b/main.cpp	Fri Aug 16 08:25:12 2013 +0000
@@ -24,6 +24,8 @@
     Timeout landing_timeout;
 //for mission
     DigitalOut led1(LED1);
+    DigitalOut led3(LED3);
+    DigitalOut led4(LED4);
 
 //for pc
     #define PC_BAUD 9600
@@ -67,8 +69,11 @@
         NIKUROMU3_OFF
   
     //servo init
-        servo.TORQUE_ON(2); //for rotation
-        servo.Rotate_Servo_Float(2,-150.0);
+        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);
@@ -78,6 +83,8 @@
     
     //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");
@@ -92,12 +99,21 @@
         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.5);
+            wait(0.1);
+            led2 = !led2;
+            wait(0.1);
+            led3 = !led3;
+            wait(0.1);
+            led4 = !led4;
+            wait(0.1);
         }       
         //after emission    
             MU2_permit = 0;//on
@@ -107,16 +123,24 @@
             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;
@@ -138,11 +162,16 @@
                 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);
@@ -153,8 +182,10 @@
         
         wait(5);//rest
     
-    //5.open door   
+   //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);
@@ -165,6 +196,8 @@
     
     //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);
@@ -175,11 +208,15 @@
     
     //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");
 }