bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Controls.h
- Revision:
- 11:711d3c207e8c
- Parent:
- 10:769cc457c3a4
- Child:
- 12:49813131dd15
diff -r 769cc457c3a4 -r 711d3c207e8c Controls/Controls.h --- a/Controls/Controls.h Sat Dec 05 09:04:23 2015 +0000 +++ b/Controls/Controls.h Tue Dec 08 22:52:46 2015 +0000 @@ -20,14 +20,14 @@ void setup(){ setInverted(false); - float m1 = 0.55; - float m2 = m1; - float l1 = 0.30;//length of links + float m1 = 0.93159230; + float m2 = 0.45433433; + float l1 = 0.275;//length of links float l2 = l1; - float I1 = 1/3.0*m1*l1*l1;//model as rod rotating around one end - float I2 = 1/3.0*m2*l2*l2; - float c1 = 0.5*l1;//location of center of mass along link - float c2 = 0.5*l2; + float I1 = 0.03736067; + float I2 = 0.01778165; + float c1 = l1-0.08567346;//location of center of mass along link + float c2 = 0.17594269; float g = 9.81; float lattice_pitch = 0.35; _parameters[0] = l1; @@ -42,7 +42,7 @@ _parameters[9] = lattice_pitch; _lastTorque = motor.getTorque(); - _manualTau = 0; + _manualTheta = 0; } void setInverted(bool inverted){ @@ -100,11 +100,11 @@ }; Motor motor; - void setTorque(float torque){ - _manualTau = torque; + void setTheta(float theta){ + _manualTheta = theta; }; - float _manualTau; + float _manualTheta; //imu MyMPU6050 myMPU6050_1; @@ -112,13 +112,14 @@ void updateIMUS(){ getActiveIMU()->loop(); + updateThetas(); } void loop(){ // getActiveIMU().disableInterrupt(); - - updateThetas(); + // float z[4] = {1,2,3,4}; +// _pc->printf("%f\n", calcTau(z, _parameters, &gains, _pc)); float tau = calcTau(_z, _parameters, &gains, _pc); // float tau = getTheta1();