Diff: main.cpp

Revision:

1:f4719bf17974

Parent:

0:b90181e726c7

--- a/main.cpp	Wed Jan 29 09:46:41 2014 +0000
+++ b/main.cpp	Thu Jan 30 04:56:41 2014 +0000
@@ -32,14 +32,17 @@
     //theta_1 : 1 ban ue no arm
     //theta_2 : mannnaka no arm
     //theta   : 1 ban sita no arm
+        double m1,m2,m3;
     for(int i=0; i<=120; i++) {
-        double m1,m2,m3;
+        m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
+        servo.Rotate_Servo_Float(3,m3);       
+        wait(0.01);
+    }
+    for(int i=0; i<=120; i++) {
         m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
         m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
-        m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
         servo.Rotate_Servo_Float(1,m1);
         servo.Rotate_Servo_Float(2,m2);             
-        servo.Rotate_Servo_Float(3,m3);       
         wait(0.01);
     }
 }
@@ -47,6 +50,7 @@
 int main() {
     setup();
     
+    debug1 = 1;
     pc.putc(0);         //Image analysis order
     //wait(7.0);          //Waiting analysis. 
     int Wait = pc.getc();