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@10:769cc457c3a4, 2015-12-05 (annotated)

Committer:
amandaghassaei
Date:
Sat Dec 05 09:04:23 2015 +0000
Revision:
10:769cc457c3a4
Parent:
9:1d9b24d7ac77
Child:
11:711d3c207e8c
swinging looking good;

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