![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
Stabilizer
Dependencies: BEAR_Protocol mbed Stabilizer iSerial
Fork of MPU9250AHRS by
Diff: main.cpp
- Revision:
- 14:8101a48eb972
- Parent:
- 13:3cb75b6e2506
- Child:
- 15:10939fd0eaac
- Child:
- 16:b5b9827dd5dc
diff -r 3cb75b6e2506 -r 8101a48eb972 main.cpp --- 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");