bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Controls.h
- Revision:
- 18:0cfe72d8a006
- Parent:
- 17:8a0e647cf551
- Child:
- 19:270735e44c98
diff -r 8a0e647cf551 -r 0cfe72d8a006 Controls/Controls.h --- a/Controls/Controls.h Fri Dec 11 00:44:45 2015 +0000 +++ b/Controls/Controls.h Fri Dec 11 06:16:33 2015 +0000 @@ -51,9 +51,11 @@ if (!_inverted) { myMPU6050_2.disable(); myMPU6050_1.enable(); +// motor.setGearRatio(10.164); } else { myMPU6050_1.disable(); myMPU6050_2.enable(); +// motor.setGearRatio(11.164); } } @@ -120,14 +122,23 @@ void updateIMUS(){ getActiveIMU()->loop(); - updateThetas(); } void loop(){ getActiveIMU()->disableInterrupt(); + updateThetas(); + float tau = calcTau(_z, _parameters, &gains, &target, _pc); - float tau = calcTau(_z, _parameters, &gains, &target, _pc); +// float K = gains.getSwingUpK(); +// float D = gains.getSwingUpD(); +// +// float th1 = _z[0]; +// float th2 = _z[1]; +// float dth1 = _z[2]; +// float dth2 = _z[3]; +// float tau = (K*(_manualTheta - th2) - D*dth2); + motor.setTorque(tau); getActiveIMU()->enableInterrupt();