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/Target.h@20:f13b949b623b, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
17:8a0e647cf551 
publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 2:17b18ea93551 1 #ifndef Target_h
amandaghassaei 2:17b18ea93551 2 #define Target_h
amandaghassaei 2:17b18ea93551 3
amandaghassaei 12:49813131dd15 4 #define M_PI 3.14159265358979323846
amandaghassaei 12:49813131dd15 5 //#include <math.h>
amandaghassaei 12:49813131dd15 6 #include "Kinematics.h"
amandaghassaei 12:49813131dd15 7
amandaghassaei 2:17b18ea93551 8
amandaghassaei 9:1d9b24d7ac77 9 class Target{
amandaghassaei 12:49813131dd15 10 // 1 2 3
amandaghassaei 12:49813131dd15 11 // 4 5 6
amandaghassaei 12:49813131dd15 12 // 7 8 9
amandaghassaei 2:17b18ea93551 13
amandaghassaei 2:17b18ea93551 14 public:
amandaghassaei 2:17b18ea93551 15
amandaghassaei 2:17b18ea93551 16 Target(){
amandaghassaei 12:49813131dd15 17 _latticePitch = 0.350;
amandaghassaei 16:5b19be27f08a 18 setTargetingStarted(false);
amandaghassaei 16:5b19be27f08a 19 setFinalDTh1(0);
amandaghassaei 2:17b18ea93551 20 }
amandaghassaei 9:1d9b24d7ac77 21
amandaghassaei 9:1d9b24d7ac77 22 void setPC(Serial *pc){
amandaghassaei 9:1d9b24d7ac77 23 _pc = pc;
amandaghassaei 9:1d9b24d7ac77 24 }
amandaghassaei 2:17b18ea93551 25
amandaghassaei 12:49813131dd15 26 void setPosition(int position, float p[10]){
amandaghassaei 15:d88f10b3b5f8 27 if (position != 4 && position != 6) position = 6;
amandaghassaei 2:17b18ea93551 28 _position = position;
amandaghassaei 12:49813131dd15 29 _th1Final = finalAngleTh1(position, p[0]);//p[0] = linkLength
amandaghassaei 12:49813131dd15 30 _th2Final = finalAngleTh2(position, p[0]);
amandaghassaei 16:5b19be27f08a 31 float z[4] = {_th1Final, _th2Final, _dth1Final, 0};
amandaghassaei 12:49813131dd15 32 _energy = getEnergy(z, p);
amandaghassaei 2:17b18ea93551 33 }
amandaghassaei 2:17b18ea93551 34
amandaghassaei 2:17b18ea93551 35 int getPosition(){
amandaghassaei 2:17b18ea93551 36 return _position;
amandaghassaei 2:17b18ea93551 37 }
amandaghassaei 12:49813131dd15 38
amandaghassaei 12:49813131dd15 39 float getTargetEnergy(){
amandaghassaei 12:49813131dd15 40 return _energy;
amandaghassaei 12:49813131dd15 41 }
amandaghassaei 12:49813131dd15 42
amandaghassaei 12:49813131dd15 43 float getFinalTh1(volatile float z[4]){
amandaghassaei 12:49813131dd15 44 return _th1Final*finalAngleSign(z, _position);
amandaghassaei 12:49813131dd15 45 }
amandaghassaei 12:49813131dd15 46
amandaghassaei 12:49813131dd15 47 float getFinalTh2(volatile float z[4]){
amandaghassaei 12:49813131dd15 48 return _th2Final*finalAngleSign(z, _position);
amandaghassaei 12:49813131dd15 49 }
amandaghassaei 12:49813131dd15 50
amandaghassaei 16:5b19be27f08a 51 void setFinalDTh1(float dth1Final){
amandaghassaei 16:5b19be27f08a 52 _dth1Final = dth1Final;
amandaghassaei 16:5b19be27f08a 53 }
amandaghassaei 16:5b19be27f08a 54
amandaghassaei 16:5b19be27f08a 55 float getFinalDTh1(){
amandaghassaei 16:5b19be27f08a 56 return _dth1Final;
amandaghassaei 16:5b19be27f08a 57 }
amandaghassaei 16:5b19be27f08a 58
amandaghassaei 16:5b19be27f08a 59 bool getTargetingStarted(){
amandaghassaei 16:5b19be27f08a 60 return _isTargeting;
amandaghassaei 16:5b19be27f08a 61 }
amandaghassaei 16:5b19be27f08a 62
amandaghassaei 16:5b19be27f08a 63 void setTargetingStarted(bool state){
amandaghassaei 16:5b19be27f08a 64 _isTargeting = state;
amandaghassaei 16:5b19be27f08a 65 }
amandaghassaei 16:5b19be27f08a 66
amandaghassaei 16:5b19be27f08a 67 bool shouldSwitchToTargetingController(volatile float z[4], float p[10]){
amandaghassaei 16:5b19be27f08a 68
amandaghassaei 16:5b19be27f08a 69 float th1 = z[0];
amandaghassaei 12:49813131dd15 70 // float th2 = z[1];
amandaghassaei 16:5b19be27f08a 71 float dth1 = z[2];
amandaghassaei 12:49813131dd15 72 // float dth2 = z[3];
amandaghassaei 16:5b19be27f08a 73
amandaghassaei 16:5b19be27f08a 74 float currentEnergy = getEnergy(z, p);
amandaghassaei 16:5b19be27f08a 75 float th1Final = getFinalTh1(z);
amandaghassaei 16:5b19be27f08a 76 float th2Final = getFinalTh2(z);
amandaghassaei 16:5b19be27f08a 77
amandaghassaei 16:5b19be27f08a 78 float targetEnergy = getTargetEnergy();
amandaghassaei 16:5b19be27f08a 79
amandaghassaei 16:5b19be27f08a 80 if (currentEnergy < targetEnergy) return false;
amandaghassaei 16:5b19be27f08a 81 // if (abs(dth1) < getFinalDTh1()/2.0) return false;
amandaghassaei 16:5b19be27f08a 82
amandaghassaei 16:5b19be27f08a 83 float targetApproachDir = 1;
amandaghassaei 16:5b19be27f08a 84 if (dth1<0) targetApproachDir = -1;
amandaghassaei 16:5b19be27f08a 85 float desiredTargetApproachDirection = targetDirection(_position);
amandaghassaei 16:5b19be27f08a 86 if (desiredTargetApproachDirection != 0 && targetApproachDir != desiredTargetApproachDirection) return false;
amandaghassaei 16:5b19be27f08a 87
amandaghassaei 16:5b19be27f08a 88 float th1Rel = boundTheta(th1);
amandaghassaei 16:5b19be27f08a 89
amandaghassaei 16:5b19be27f08a 90 if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/4) return false;
amandaghassaei 16:5b19be27f08a 91 if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/3) return true;
amandaghassaei 16:5b19be27f08a 92 return false;
amandaghassaei 16:5b19be27f08a 93 }
amandaghassaei 16:5b19be27f08a 94
amandaghassaei 16:5b19be27f08a 95 float calcTargetingForce(volatile float z[4], float p[10], float K, float D){
amandaghassaei 17:8a0e647cf551 96
amandaghassaei 16:5b19be27f08a 97 float th1 = z[0];
amandaghassaei 16:5b19be27f08a 98 float th2 = z[1];
amandaghassaei 16:5b19be27f08a 99 float dth1 = z[2];
amandaghassaei 16:5b19be27f08a 100 float dth2 = z[3];
amandaghassaei 16:5b19be27f08a 101
amandaghassaei 16:5b19be27f08a 102 float th1Final = getFinalTh1(z);
amandaghassaei 16:5b19be27f08a 103 float th2Final = getFinalTh2(z);
amandaghassaei 16:5b19be27f08a 104
amandaghassaei 16:5b19be27f08a 105 float targetPosition[2];
amandaghassaei 16:5b19be27f08a 106 targetPosition[0] = targetXPosition(_position);
amandaghassaei 16:5b19be27f08a 107 targetPosition[1] = targetYPosition(_position);
amandaghassaei 16:5b19be27f08a 108
amandaghassaei 16:5b19be27f08a 109 float mAngle = th1Final;
amandaghassaei 16:5b19be27f08a 110 if (mAngle > M_PI/2.0) mAngle -= M_PI/2.0;
amandaghassaei 16:5b19be27f08a 111 else if (mAngle < -M_PI/2.0) mAngle += M_PI/2.0;
amandaghassaei 16:5b19be27f08a 112 float m = 1.0/tan(mAngle);
amandaghassaei 16:5b19be27f08a 113 float a = -m;
amandaghassaei 16:5b19be27f08a 114 float b = 1;
amandaghassaei 16:5b19be27f08a 115 float c = m*targetPosition[0]-targetPosition[1];
amandaghassaei 16:5b19be27f08a 116
amandaghassaei 16:5b19be27f08a 117 float gripperPosition[2];
amandaghassaei 16:5b19be27f08a 118 getGripperPosition(gripperPosition, z, p);
amandaghassaei 16:5b19be27f08a 119 float gripperVelocity[2];
amandaghassaei 16:5b19be27f08a 120 getGripperVelocity(gripperVelocity, z, p);
amandaghassaei 16:5b19be27f08a 121
amandaghassaei 16:5b19be27f08a 122 float x = (b*(b*gripperPosition[0] - a*gripperPosition[1])-a*c)/(a*a + b*b)-gripperPosition[0];
amandaghassaei 16:5b19be27f08a 123 float y = (a*(-b*gripperPosition[0] + a*gripperPosition[1])-b*c)/(a*a + b*b)-gripperPosition[1];
amandaghassaei 16:5b19be27f08a 124 float norm = sqrt(x*x + y*y);
amandaghassaei 16:5b19be27f08a 125 float xUnit = x/norm;
amandaghassaei 16:5b19be27f08a 126 float yUnit = y/norm;
amandaghassaei 16:5b19be27f08a 127 float dNorm = gripperVelocity[0]*xUnit + gripperVelocity[1]*yUnit;
amandaghassaei 16:5b19be27f08a 128
amandaghassaei 16:5b19be27f08a 129 // float K = gains->getSwingUpK();
amandaghassaei 16:5b19be27f08a 130 // float D = gains->getSwingUpD();
amandaghassaei 16:5b19be27f08a 131
amandaghassaei 16:5b19be27f08a 132 float gain = (K*norm - D*dNorm);
amandaghassaei 16:5b19be27f08a 133 float forceTaskSp[2] = {gain*xUnit, gain*yUnit};
amandaghassaei 16:5b19be27f08a 134 float Jtrans[2][2];
amandaghassaei 17:8a0e647cf551 135
amandaghassaei 17:8a0e647cf551 136 getGripperJacobianTranspose(Jtrans, z, p);
amandaghassaei 16:5b19be27f08a 137
amandaghassaei 16:5b19be27f08a 138 return Jtrans[0][1]*forceTaskSp[0]+Jtrans[1][1]*forceTaskSp[1];
amandaghassaei 12:49813131dd15 139 }
amandaghassaei 2:17b18ea93551 140
amandaghassaei 2:17b18ea93551 141
amandaghassaei 2:17b18ea93551 142 private:
amandaghassaei 2:17b18ea93551 143
amandaghassaei 9:1d9b24d7ac77 144 Serial *_pc;
amandaghassaei 12:49813131dd15 145
amandaghassaei 16:5b19be27f08a 146 bool _isTargeting;
amandaghassaei 16:5b19be27f08a 147
amandaghassaei 9:1d9b24d7ac77 148
amandaghassaei 2:17b18ea93551 149 int _position;
amandaghassaei 12:49813131dd15 150 float _energy;
amandaghassaei 12:49813131dd15 151 float _th1Final;
amandaghassaei 12:49813131dd15 152 float _th2Final;
amandaghassaei 16:5b19be27f08a 153 float _dth1Final;
amandaghassaei 12:49813131dd15 154 float _latticePitch;
amandaghassaei 12:49813131dd15 155
amandaghassaei 12:49813131dd15 156 float finalAngleTh1(float targetPosition, float armLength){
amandaghassaei 12:49813131dd15 157 float th1;
amandaghassaei 12:49813131dd15 158 if (targetIsCardinal(targetPosition)) th1 = asin(1/2.0*_latticePitch/armLength);
amandaghassaei 12:49813131dd15 159 else th1 = asin(1/2.0*_latticePitch*sqrt(double(2))/armLength);
amandaghassaei 12:49813131dd15 160 th1 += finalAngleTh1Rotation(targetPosition);
amandaghassaei 12:49813131dd15 161 return th1;
amandaghassaei 12:49813131dd15 162 }
amandaghassaei 12:49813131dd15 163
amandaghassaei 12:49813131dd15 164 float finalAngleTh1Rotation(float targetPosition){
amandaghassaei 12:49813131dd15 165 if (targetPosition == 2) return M_PI/2.0;
amandaghassaei 12:49813131dd15 166 if (targetPosition == 8) return -M_PI/2.0;
amandaghassaei 12:49813131dd15 167 if (targetPosition == 1 || targetPosition == 3) return M_PI/4.0;
amandaghassaei 12:49813131dd15 168 if (targetPosition == 7 || targetPosition == 9) return -M_PI/4.0;
amandaghassaei 12:49813131dd15 169 return 0;
amandaghassaei 12:49813131dd15 170 }
amandaghassaei 12:49813131dd15 171
amandaghassaei 12:49813131dd15 172 float finalAngleTh2(float targetPosition, float armLength){
amandaghassaei 12:49813131dd15 173 if (targetIsCardinal(targetPosition)) return M_PI-2.0*asin(1/2.0*_latticePitch/armLength);
amandaghassaei 12:49813131dd15 174 return M_PI-2.0*asin(1/2.0*_latticePitch*sqrt(double(2.0))/armLength);
amandaghassaei 12:49813131dd15 175 }
amandaghassaei 12:49813131dd15 176
amandaghassaei 12:49813131dd15 177 bool targetIsCardinal(float targetPosition){
amandaghassaei 12:49813131dd15 178 if (fmod(targetPosition,2) == 0) return true;
amandaghassaei 12:49813131dd15 179 return false;
amandaghassaei 12:49813131dd15 180 }
amandaghassaei 12:49813131dd15 181
amandaghassaei 12:49813131dd15 182 float finalAngleSign(volatile float z[4], float targetPosition){
amandaghassaei 12:49813131dd15 183 float approachDirection = targetDirection(targetPosition);
amandaghassaei 12:49813131dd15 184 if (approachDirection<0 || (approachDirection==0 && z[3]<0)) return -1;
amandaghassaei 12:49813131dd15 185 return 1;
amandaghassaei 12:49813131dd15 186 }
amandaghassaei 12:49813131dd15 187
amandaghassaei 12:49813131dd15 188 float targetDirection(float targetPosition){
amandaghassaei 12:49813131dd15 189 if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -1;
amandaghassaei 12:49813131dd15 190 if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1;
amandaghassaei 12:49813131dd15 191 return 0;
amandaghassaei 12:49813131dd15 192 }
amandaghassaei 16:5b19be27f08a 193
amandaghassaei 16:5b19be27f08a 194 float targetXPosition(float targetPosition){
amandaghassaei 16:5b19be27f08a 195 if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -_latticePitch;
amandaghassaei 16:5b19be27f08a 196 if (targetPosition == 2 || targetPosition == 5 || targetPosition == 8) return 0;
amandaghassaei 16:5b19be27f08a 197 if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return _latticePitch;
amandaghassaei 17:8a0e647cf551 198 _pc->printf("invalid target position %f\n", targetPosition);
amandaghassaei 16:5b19be27f08a 199 return _latticePitch;
amandaghassaei 16:5b19be27f08a 200 }
amandaghassaei 16:5b19be27f08a 201
amandaghassaei 16:5b19be27f08a 202 float targetYPosition(float targetPosition){
amandaghassaei 16:5b19be27f08a 203 if (targetPosition < 4) return _latticePitch;
amandaghassaei 16:5b19be27f08a 204 if (targetPosition < 7) return 0;
amandaghassaei 16:5b19be27f08a 205 if (targetPosition < 10) return -_latticePitch;
amandaghassaei 17:8a0e647cf551 206 _pc->printf("invalid target position %f\n", targetPosition);
amandaghassaei 16:5b19be27f08a 207 return 0;
amandaghassaei 16:5b19be27f08a 208 }
amandaghassaei 2:17b18ea93551 209
amandaghassaei 2:17b18ea93551 210 };
amandaghassaei 2:17b18ea93551 211
amandaghassaei 2:17b18ea93551 212 #endif