Diff: main.cpp

Revision:

14:8101a48eb972

Parent:

13:3cb75b6e2506

Child:

15:10939fd0eaac

Child:

16:b5b9827dd5dc

--- a/main.cpp	Wed Dec 23 00:43:52 2015 +0000
+++ b/main.cpp	Wed Dec 23 11:21:43 2015 +0000
@@ -7,7 +7,7 @@
 char buffer[14];
 
 MPU9250 mpu9250;
-Stabilizer Stabilize(5,0);
+Stabilizer Stabilize(5.0f,0.0f);
 Kinematic L('Z',10,10,30,30),R('Z',10,10,30,30);
 
 Timer t;
@@ -43,7 +43,7 @@
     //Stabilize.set_Body_Lenght(5);
     Stabilize.set_current_zeta(roll);
     //Stabilize.set_zeta_set(0);
-    Stabilize.ZetaErrorCalculation();
+    //Stabilize.ZetaErrorCalculation();
     Stabilize.PID();
 
     Stabilize.set_New_Height(L.get_Position_Z());
@@ -54,10 +54,10 @@
     L.InverseKinematicCalculation();
     L.print();
 
-    R.set_Position_Y(L.get_Position_Y());
-    R.set_Position_Z(L.get_Position_Z());
-    R.set_offset_YZ(3,3);
-    R.SumPositionWithOffset();
+    R.set_Position_Y(L.get_Position_Y()+Stabilize.get_Offset_Y());
+    R.set_Position_Z(L.get_Position_Z()+Stabilize.get_Offset_Z());
+    //R.set_offset_YZ(3,3);
+    //R.SumPositionWithOffset();
     R.InverseKinematicCalculation();
     R.print();
     pc.printf("\n");