Diff: main.cpp

Revision:

5:e85af85e4985

Parent:

4:934b39bd5c2c

Child:

6:c206e90971e1

--- a/main.cpp	Fri Aug 02 10:51:03 2013 +0000
+++ b/main.cpp	Sat Aug 03 09:13:53 2013 +0000
@@ -20,6 +20,8 @@
     PwmOut nikuromu3(p24);
 //for Flight confirmation
     DigitalIn flightpin(p26);
+//for landing
+    Timeout landing_timeout;
 
 //for pc
     #define PC_BAUD 9600
@@ -37,8 +39,18 @@
 
 //for servo 
     void RHI_Scan(void);
+
+//for landing
+    void landing(void);
+    char landing_flg=0;
+    
+void landing(void){
+    landing_flg=1;
+    MyMu2.send("landing_timeout\r\n");
+}
     
 int main() {
+//////////Init///////////////////////////////////////////////////////////////////////////////
     //for pc
         pc.baud(PC_BAUD);
         Send_Call.attach(&data_send,COMMUNICATION_RATE);
@@ -54,64 +66,108 @@
         nikuromu3.period(N_PERIOD);
         NIKUROMU3_OFF
   
-     while(1){}
-  
-    //Sequence
+    //servo init
         servo.TORQUE_ON(2); //for rotation
         servo.Rotate_Servo_Float(2,-150.0);
-        
-        while(flightpin ==1){}
+  
+    //for debug test
+        //servo.Rotate_Servo_Float_Test(1);
+        //servo.ID_CHANGE(1,2);
+  
+///////Sequence////////////////////////////////////////////////////////////////////////
     
+    //0.MU2 check
         MU2_permit = 0;//on
-        servo.TORQUE_ON(1);
-        servo.TORQUE_ON(2);
-        servo.TORQUE_ON(3);
-        wait(0.3);//wait until mu2 kidou
+        wait(0.1);//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
+    
+    //1.freeze sequence while flightpin are fixed
+        while(flightpin ==1){}       
+        //after emission    
+            MU2_permit = 0;//on
+            wait(0.1);//wait until mu2 will be steady
+            servo.TORQUE_ON(1);
+            servo.TORQUE_ON(2);
+            servo.TORQUE_ON(3);
+            MyMu2.send("emission finished!\r\n");
     
-        MyMu2.send("housyutukennti\r\n");
-    
-        wait(10);
+    //2.check landing
+        int roll_data[10];
+        int roll_average;
+        int roll_breakup;
+        landing_timeout.attach(&landing, 30.0);
+        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;
+            }
+        }
     
-        MyMu2.send("tizyounituita\r\n");
-    
-    
+        wait(5);//rest
+        
+    //3.fix mechanism start 
+        MyMu2.send("start fix mechanism\r\n");
         NIKUROMU1_ON
         wait(6.0);
         NIKUROMU1_OFF
-    
-        MyMu2.send("tumehiraita\r\n");
+        MyMu2.send("finish fix mechanism\r\n");
+        
+        wait(5);//rest
     
-    
-        wait(20.0);//changed 20 to 5
-    
+    //4.rotation mechanism start 
+        MyMu2.send("start rotation mechanism\r\n");
         servo.TORQUE_ON(2);
         servo.Rotate_Servo_Float(2,-150.0);
-        servo.Rotate_Servo_Float(2,-150.0 + 360.0 - can.get_roll() );
-        
+        servo.Rotate_Servo_Float(2,-150.0 + 360.0 - can.get_roll() );   
+        MyMu2.send("finish rotation mechanism\r\n");
         
-        MyMu2.send("uemuitayo\r\n");
+        wait(5);//rest
     
-        wait(5.0);
-        
+    //5.open door   
+        MyMu2.send("start opening door\r\n");   
         NIKUROMU2_ON
         wait(6.0);
         NIKUROMU2_OFF
-    
-        MyMu2.send("tobirahiraita\r\n");
+        MyMu2.send("finish opening door\r\n");
+        
+        wait(5);//rest
     
-        wait(5.0);
-    
+    //6.develope parabolic antenna
+        MyMu2.send("start developing parabolic antenna\r\n");  
         NIKUROMU3_ON
         wait(6.0);
         NIKUROMU3_OFF
-    
-        MyMu2.send("anntenatennkai\r\n");
+        MyMu2.send("finish developing parabolic antenna\r\n");  
+        
+        wait(5);//rest
     
-        wait(5.0);
-    
-        MyMu2.send("sukyannhazime\r\n");
-    
+    //7.start RHI scaning 
+        MyMu2.send("start RHI scaning\r\n");
         RHI_Scan();  
+        
+    //8.end of sequence
+        MyMu2.send("END!\r\n");
 }
 
 void RHI_Scan(void){
@@ -177,10 +233,16 @@
     
     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[32]='\r';
-    mu2data[33]='\n';
-    mu2data[34]='\0';
+    mu2data[36]='\r';
+    mu2data[37]='\n';
+    mu2data[38]='\0';
         
     MyMu2.send(mu2data);
 }