bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Controls.h
- Committer:
- amandaghassaei
- Date:
- 2015-12-03
- Revision:
- 8:1a3a69fecedf
- Parent:
- 7:7efcd3bf3302
- Child:
- 9:1d9b24d7ac77
File content as of revision 8:1a3a69fecedf:
#ifndef Controls_h #define Controls_h #include <math.h> #include "Gains.h" #include "Target.h" #include "MyMPU6050.h" #include "Motor.h" #include "Dynamics.h" class Controls: public CommDelegate{ public: Controls():myMPU6050_1(p9, p10, p11), myMPU6050_2(p28, p27, p18){//I2C_SDA, I2C_SCL, int_pin _inverted = false; float m1 = 0.55; float m2 = m1; float l1 = 0.30;//length of links float l2 = l1; float I1 = 1/3*m1*pow(l1,2);//model as rod rotating around one end float I2 = 1/3*m2*pow(l2,2); float c1 = 0.5*l1;//location of center of mass along link float c2 = 0.5*l2; float g = 9.81; float lattice_pitch = 0.35; _parameters[0] = l1; _parameters[1] = l2; _parameters[2] = c1; _parameters[3] = c2; _parameters[4] = m1; _parameters[5] = m2; _parameters[6] = I1; _parameters[7] = I2; _parameters[8] = g; _parameters[9] = lattice_pitch; } Gains gains; Target target; Motor motor; //imu MyMPU6050 myMPU6050_1; MyMPU6050 myMPU6050_2; void loop(){ if (_inverted) myMPU6050_2.loop(); else myMPU6050_1.loop(); updateThetas(); float tau = calcTau(_z, _parameters); } float getTheta1(){ return _z[0]; } float getDTheta1(){ return _z[2]; } float getTheta2(){ return _z[1]; } float getDTheta2(){ return _z[3]; } private: float _parameters[10]; float _z[4];//theta1, theta2, dtheta2, dtheta2 void updateThetas(){ _z[0] = _getTheta1(); _z[2] = _getDTheta1(); _z[1] = _getTheta2(); _z[3] = _getDTheta2(); } float _getTheta1(){ if (_inverted) return myMPU6050_2.getTheta(); return myMPU6050_1.getTheta(); } float _getDTheta1(){ if (_inverted) return myMPU6050_2.getDTheta(); return myMPU6050_1.getDTheta(); } float _getTheta2(){ if (_inverted) return -motor.getTheta(); return motor.getTheta(); } float _getDTheta2(){ if (_inverted) return -motor.getDTheta(); return motor.getDTheta(); } bool _inverted; }; #endif