Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Controls/Controls.h@9:1d9b24d7ac77, 2015-12-05 (annotated)

Committer:
amandaghassaei
Date:
Sat Dec 05 00:40:42 2015 +0000
Revision:
9:1d9b24d7ac77
Parent:
8:1a3a69fecedf
Child:
10:769cc457c3a4
basic motor pwm working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 7:7efcd3bf3302 1 #ifndef Controls_h
amandaghassaei 7:7efcd3bf3302 2 #define Controls_h
amandaghassaei 7:7efcd3bf3302 3
amandaghassaei 8:1a3a69fecedf 4 #include <math.h>
amandaghassaei 7:7efcd3bf3302 5 #include "Gains.h"
amandaghassaei 7:7efcd3bf3302 6 #include "Target.h"
amandaghassaei 8:1a3a69fecedf 7 #include "MyMPU6050.h"
amandaghassaei 8:1a3a69fecedf 8 #include "Motor.h"
amandaghassaei 8:1a3a69fecedf 9 #include "Dynamics.h"
amandaghassaei 7:7efcd3bf3302 10
amandaghassaei 8:1a3a69fecedf 11 class Controls: public CommDelegate{
amandaghassaei 7:7efcd3bf3302 12
amandaghassaei 7:7efcd3bf3302 13 public:
amandaghassaei 7:7efcd3bf3302 14
amandaghassaei 9:1d9b24d7ac77 15 Controls():myMPU6050_1(p28, p27, p18), myMPU6050_2(p9, p10, p11){//I2C_SDA, I2C_SCL, int_pin
amandaghassaei 8:1a3a69fecedf 16
amandaghassaei 9:1d9b24d7ac77 17 setInverted(false);
amandaghassaei 8:1a3a69fecedf 18
amandaghassaei 8:1a3a69fecedf 19 float m1 = 0.55;
amandaghassaei 8:1a3a69fecedf 20 float m2 = m1;
amandaghassaei 8:1a3a69fecedf 21 float l1 = 0.30;//length of links
amandaghassaei 8:1a3a69fecedf 22 float l2 = l1;
amandaghassaei 8:1a3a69fecedf 23 float I1 = 1/3*m1*pow(l1,2);//model as rod rotating around one end
amandaghassaei 8:1a3a69fecedf 24 float I2 = 1/3*m2*pow(l2,2);
amandaghassaei 8:1a3a69fecedf 25 float c1 = 0.5*l1;//location of center of mass along link
amandaghassaei 8:1a3a69fecedf 26 float c2 = 0.5*l2;
amandaghassaei 8:1a3a69fecedf 27 float g = 9.81;
amandaghassaei 8:1a3a69fecedf 28 float lattice_pitch = 0.35;
amandaghassaei 8:1a3a69fecedf 29 _parameters[0] = l1;
amandaghassaei 8:1a3a69fecedf 30 _parameters[1] = l2;
amandaghassaei 8:1a3a69fecedf 31 _parameters[2] = c1;
amandaghassaei 8:1a3a69fecedf 32 _parameters[3] = c2;
amandaghassaei 8:1a3a69fecedf 33 _parameters[4] = m1;
amandaghassaei 8:1a3a69fecedf 34 _parameters[5] = m2;
amandaghassaei 8:1a3a69fecedf 35 _parameters[6] = I1;
amandaghassaei 8:1a3a69fecedf 36 _parameters[7] = I2;
amandaghassaei 8:1a3a69fecedf 37 _parameters[8] = g;
amandaghassaei 8:1a3a69fecedf 38 _parameters[9] = lattice_pitch;
amandaghassaei 8:1a3a69fecedf 39
amandaghassaei 7:7efcd3bf3302 40 }
amandaghassaei 9:1d9b24d7ac77 41
amandaghassaei 9:1d9b24d7ac77 42 void setInverted(bool inverted){
amandaghassaei 9:1d9b24d7ac77 43 _inverted = inverted;
amandaghassaei 9:1d9b24d7ac77 44 if (!_inverted) {
amandaghassaei 9:1d9b24d7ac77 45 myMPU6050_2.disable();
amandaghassaei 9:1d9b24d7ac77 46 myMPU6050_1.enable();
amandaghassaei 9:1d9b24d7ac77 47 } else {
amandaghassaei 9:1d9b24d7ac77 48 myMPU6050_1.disable();
amandaghassaei 9:1d9b24d7ac77 49 myMPU6050_2.enable();
amandaghassaei 9:1d9b24d7ac77 50 }
amandaghassaei 9:1d9b24d7ac77 51 }
amandaghassaei 9:1d9b24d7ac77 52
amandaghassaei 9:1d9b24d7ac77 53 void setPC(Serial *pc){
amandaghassaei 9:1d9b24d7ac77 54 _pc = pc;
amandaghassaei 9:1d9b24d7ac77 55 motor.setPC(pc);
amandaghassaei 9:1d9b24d7ac77 56 gains.setPC(pc);
amandaghassaei 9:1d9b24d7ac77 57 target.setPC(pc);
amandaghassaei 9:1d9b24d7ac77 58 myMPU6050_1.setPC(pc);
amandaghassaei 9:1d9b24d7ac77 59 myMPU6050_2.setPC(pc);
amandaghassaei 9:1d9b24d7ac77 60 }
amandaghassaei 7:7efcd3bf3302 61
amandaghassaei 7:7efcd3bf3302 62 Gains gains;
amandaghassaei 9:1d9b24d7ac77 63 void setSwingUpK(float k){
amandaghassaei 9:1d9b24d7ac77 64 gains.setSwingUpK(k);
amandaghassaei 9:1d9b24d7ac77 65 };
amandaghassaei 9:1d9b24d7ac77 66 void setSwingUpD(float d){
amandaghassaei 9:1d9b24d7ac77 67 gains.setSwingUpD(d);
amandaghassaei 9:1d9b24d7ac77 68 };
amandaghassaei 9:1d9b24d7ac77 69 void setK2(float k2){
amandaghassaei 9:1d9b24d7ac77 70 gains.setK2(k2);
amandaghassaei 9:1d9b24d7ac77 71 };
amandaghassaei 9:1d9b24d7ac77 72 void setD2(float d2){
amandaghassaei 9:1d9b24d7ac77 73 gains.setD2(d2);
amandaghassaei 9:1d9b24d7ac77 74 };
amandaghassaei 9:1d9b24d7ac77 75 float getSwingUpK(){
amandaghassaei 9:1d9b24d7ac77 76 return gains.getSwingUpK();
amandaghassaei 9:1d9b24d7ac77 77 };
amandaghassaei 9:1d9b24d7ac77 78 float getSwingUpD(){
amandaghassaei 9:1d9b24d7ac77 79 return gains.getSwingUpD();
amandaghassaei 9:1d9b24d7ac77 80 };
amandaghassaei 9:1d9b24d7ac77 81 float getK2(){
amandaghassaei 9:1d9b24d7ac77 82 return gains.getK2();
amandaghassaei 9:1d9b24d7ac77 83 };
amandaghassaei 9:1d9b24d7ac77 84 float getD2(){
amandaghassaei 9:1d9b24d7ac77 85 return gains.getD2();
amandaghassaei 9:1d9b24d7ac77 86 };
amandaghassaei 9:1d9b24d7ac77 87
amandaghassaei 7:7efcd3bf3302 88 Target target;
amandaghassaei 9:1d9b24d7ac77 89 void setTargetPosition(int position){
amandaghassaei 9:1d9b24d7ac77 90 target.setPosition(position);
amandaghassaei 9:1d9b24d7ac77 91 };
amandaghassaei 9:1d9b24d7ac77 92 int getTargetPosition(){
amandaghassaei 9:1d9b24d7ac77 93 return target.getPosition();
amandaghassaei 9:1d9b24d7ac77 94 };
amandaghassaei 8:1a3a69fecedf 95
amandaghassaei 8:1a3a69fecedf 96 Motor motor;
amandaghassaei 9:1d9b24d7ac77 97 void setTorque(float torque){
amandaghassaei 9:1d9b24d7ac77 98 motor.setTorque(torque);
amandaghassaei 9:1d9b24d7ac77 99 };
amandaghassaei 7:7efcd3bf3302 100
amandaghassaei 8:1a3a69fecedf 101 //imu
amandaghassaei 8:1a3a69fecedf 102 MyMPU6050 myMPU6050_1;
amandaghassaei 8:1a3a69fecedf 103 MyMPU6050 myMPU6050_2;
amandaghassaei 7:7efcd3bf3302 104
amandaghassaei 7:7efcd3bf3302 105 void loop(){
amandaghassaei 8:1a3a69fecedf 106 if (_inverted) myMPU6050_2.loop();
amandaghassaei 8:1a3a69fecedf 107 else myMPU6050_1.loop();
amandaghassaei 8:1a3a69fecedf 108 updateThetas();
amandaghassaei 8:1a3a69fecedf 109
amandaghassaei 9:1d9b24d7ac77 110 // float tau = calcTau(_z, _parameters, &gains, _pc);
amandaghassaei 9:1d9b24d7ac77 111 float tau = getTheta1();
amandaghassaei 9:1d9b24d7ac77 112 motor.setTorque(tau);
amandaghassaei 8:1a3a69fecedf 113 }
amandaghassaei 8:1a3a69fecedf 114
amandaghassaei 8:1a3a69fecedf 115 float getTheta1(){
amandaghassaei 8:1a3a69fecedf 116 return _z[0];
amandaghassaei 7:7efcd3bf3302 117 }
amandaghassaei 8:1a3a69fecedf 118 float getDTheta1(){
amandaghassaei 8:1a3a69fecedf 119 return _z[2];
amandaghassaei 8:1a3a69fecedf 120 }
amandaghassaei 8:1a3a69fecedf 121 float getTheta2(){
amandaghassaei 8:1a3a69fecedf 122 return _z[1];
amandaghassaei 8:1a3a69fecedf 123 }
amandaghassaei 8:1a3a69fecedf 124 float getDTheta2(){
amandaghassaei 8:1a3a69fecedf 125 return _z[3];
amandaghassaei 8:1a3a69fecedf 126 }
amandaghassaei 8:1a3a69fecedf 127
amandaghassaei 9:1d9b24d7ac77 128 void printPWM(){
amandaghassaei 9:1d9b24d7ac77 129 motor.printPWM();
amandaghassaei 9:1d9b24d7ac77 130 }
amandaghassaei 9:1d9b24d7ac77 131
amandaghassaei 8:1a3a69fecedf 132
amandaghassaei 7:7efcd3bf3302 133
amandaghassaei 7:7efcd3bf3302 134 private:
amandaghassaei 7:7efcd3bf3302 135
amandaghassaei 9:1d9b24d7ac77 136 Serial *_pc;
amandaghassaei 9:1d9b24d7ac77 137
amandaghassaei 8:1a3a69fecedf 138 float _parameters[10];
amandaghassaei 7:7efcd3bf3302 139
amandaghassaei 8:1a3a69fecedf 140 float _z[4];//theta1, theta2, dtheta2, dtheta2
amandaghassaei 8:1a3a69fecedf 141
amandaghassaei 8:1a3a69fecedf 142 void updateThetas(){
amandaghassaei 8:1a3a69fecedf 143 _z[0] = _getTheta1();
amandaghassaei 8:1a3a69fecedf 144 _z[2] = _getDTheta1();
amandaghassaei 8:1a3a69fecedf 145 _z[1] = _getTheta2();
amandaghassaei 8:1a3a69fecedf 146 _z[3] = _getDTheta2();
amandaghassaei 8:1a3a69fecedf 147
amandaghassaei 8:1a3a69fecedf 148 }
amandaghassaei 8:1a3a69fecedf 149 float _getTheta1(){
amandaghassaei 8:1a3a69fecedf 150 if (_inverted) return myMPU6050_2.getTheta();
amandaghassaei 8:1a3a69fecedf 151 return myMPU6050_1.getTheta();
amandaghassaei 8:1a3a69fecedf 152 }
amandaghassaei 8:1a3a69fecedf 153 float _getDTheta1(){
amandaghassaei 8:1a3a69fecedf 154 if (_inverted) return myMPU6050_2.getDTheta();
amandaghassaei 8:1a3a69fecedf 155 return myMPU6050_1.getDTheta();
amandaghassaei 8:1a3a69fecedf 156 }
amandaghassaei 8:1a3a69fecedf 157 float _getTheta2(){
amandaghassaei 8:1a3a69fecedf 158 if (_inverted) return -motor.getTheta();
amandaghassaei 8:1a3a69fecedf 159 return motor.getTheta();
amandaghassaei 8:1a3a69fecedf 160 }
amandaghassaei 8:1a3a69fecedf 161 float _getDTheta2(){
amandaghassaei 8:1a3a69fecedf 162 if (_inverted) return -motor.getDTheta();
amandaghassaei 8:1a3a69fecedf 163 return motor.getDTheta();
amandaghassaei 8:1a3a69fecedf 164 }
amandaghassaei 8:1a3a69fecedf 165
amandaghassaei 8:1a3a69fecedf 166 bool _inverted;
amandaghassaei 7:7efcd3bf3302 167
amandaghassaei 7:7efcd3bf3302 168 };
amandaghassaei 7:7efcd3bf3302 169
amandaghassaei 7:7efcd3bf3302 170 #endif