Diff: main.cpp

Revision:

4:934b39bd5c2c

Parent:

2:5871a792937b

Child:

5:e85af85e4985

--- a/main.cpp	Sun Jul 21 07:01:28 2013 +0000
+++ b/main.cpp	Fri Aug 02 10:51:03 2013 +0000
@@ -54,8 +54,10 @@
         nikuromu3.period(N_PERIOD);
         NIKUROMU3_OFF
   
+     while(1){}
+  
     //Sequence
-        servo.TORQUE_ON(2);
+        servo.TORQUE_ON(2); //for rotation
         servo.Rotate_Servo_Float(2,-150.0);
         
         while(flightpin ==1){}
@@ -64,7 +66,7 @@
         servo.TORQUE_ON(1);
         servo.TORQUE_ON(2);
         servo.TORQUE_ON(3);
-        wait(0.3);
+        wait(0.3);//wait until mu2 kidou
     
         MyMu2.send("housyutukennti\r\n");
     
@@ -79,46 +81,34 @@
     
         MyMu2.send("tumehiraita\r\n");
     
-        wait(30);
     
-        float theta;
-        theta=-150.0;
+        wait(20.0);//changed 20 to 5
+    
         servo.TORQUE_ON(2);
-        servo.Rotate_Servo_Float(2,theta);
-        while(1) {
-                if(can.get_a_z()!=1){
-                    servo.TORQUE_ON(2);
-                    theta += 0.1;
-                    servo.Rotate_Servo_Float(2,theta);
-                }
-                else{
-                    servo.TORQUE_ON(2);
-                    break;
-                }   
-                wait(0.006);
-                led2 = !led2;
-        }
+        servo.Rotate_Servo_Float(2,-150.0);
+        servo.Rotate_Servo_Float(2,-150.0 + 360.0 - can.get_roll() );
         
-        wait(20);
         
         MyMu2.send("uemuitayo\r\n");
     
         wait(5.0);
+        
         NIKUROMU2_ON
         wait(6.0);
         NIKUROMU2_OFF
     
         MyMu2.send("tobirahiraita\r\n");
     
-        wait(10);
+        wait(5.0);
     
-        wait(5.0);
         NIKUROMU3_ON
         wait(6.0);
         NIKUROMU3_OFF
     
         MyMu2.send("anntenatennkai\r\n");
     
+        wait(5.0);
+    
         MyMu2.send("sukyannhazime\r\n");
     
         RHI_Scan();  
@@ -129,7 +119,7 @@
     servo.TORQUE_ON(3);
     
     while(1){
-        for(int j=0;j<30;j++){
+        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);
@@ -146,7 +136,7 @@
 }
 
 void data_send(void){
-    pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_a_z());
+    pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_roll());
     
     char mu2data[100];
     mu2data[0] = *(can.get_time()+0);