bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Target.h@16:5b19be27f08a, 2015-12-10 (annotated)
- Committer:
- amandaghassaei
- Date:
- Thu Dec 10 10:55:35 2015 +0000
- Revision:
- 16:5b19be27f08a
- Parent:
- 15:d88f10b3b5f8
- Child:
- 17:8a0e647cf551
targeting code added;
Who changed what in which revision?
User | Revision | Line number | New 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 | 16:5b19be27f08a | 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 | 16:5b19be27f08a | 135 | getGripperJacobianTranspose(Jtrans, z, p); |
amandaghassaei | 16:5b19be27f08a | 136 | |
amandaghassaei | 16:5b19be27f08a | 137 | return Jtrans[0][1]*forceTaskSp[0]+Jtrans[1][1]*forceTaskSp[1]; |
amandaghassaei | 12:49813131dd15 | 138 | } |
amandaghassaei | 2:17b18ea93551 | 139 | |
amandaghassaei | 2:17b18ea93551 | 140 | |
amandaghassaei | 2:17b18ea93551 | 141 | private: |
amandaghassaei | 2:17b18ea93551 | 142 | |
amandaghassaei | 9:1d9b24d7ac77 | 143 | Serial *_pc; |
amandaghassaei | 12:49813131dd15 | 144 | |
amandaghassaei | 16:5b19be27f08a | 145 | bool _isTargeting; |
amandaghassaei | 16:5b19be27f08a | 146 | |
amandaghassaei | 9:1d9b24d7ac77 | 147 | |
amandaghassaei | 2:17b18ea93551 | 148 | int _position; |
amandaghassaei | 12:49813131dd15 | 149 | float _energy; |
amandaghassaei | 12:49813131dd15 | 150 | float _th1Final; |
amandaghassaei | 12:49813131dd15 | 151 | float _th2Final; |
amandaghassaei | 16:5b19be27f08a | 152 | float _dth1Final; |
amandaghassaei | 12:49813131dd15 | 153 | float _latticePitch; |
amandaghassaei | 12:49813131dd15 | 154 | |
amandaghassaei | 12:49813131dd15 | 155 | float finalAngleTh1(float targetPosition, float armLength){ |
amandaghassaei | 12:49813131dd15 | 156 | float th1; |
amandaghassaei | 12:49813131dd15 | 157 | if (targetIsCardinal(targetPosition)) th1 = asin(1/2.0*_latticePitch/armLength); |
amandaghassaei | 12:49813131dd15 | 158 | else th1 = asin(1/2.0*_latticePitch*sqrt(double(2))/armLength); |
amandaghassaei | 12:49813131dd15 | 159 | th1 += finalAngleTh1Rotation(targetPosition); |
amandaghassaei | 12:49813131dd15 | 160 | return th1; |
amandaghassaei | 12:49813131dd15 | 161 | } |
amandaghassaei | 12:49813131dd15 | 162 | |
amandaghassaei | 12:49813131dd15 | 163 | float finalAngleTh1Rotation(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 164 | if (targetPosition == 2) return M_PI/2.0; |
amandaghassaei | 12:49813131dd15 | 165 | if (targetPosition == 8) return -M_PI/2.0; |
amandaghassaei | 12:49813131dd15 | 166 | if (targetPosition == 1 || targetPosition == 3) return M_PI/4.0; |
amandaghassaei | 12:49813131dd15 | 167 | if (targetPosition == 7 || targetPosition == 9) return -M_PI/4.0; |
amandaghassaei | 12:49813131dd15 | 168 | return 0; |
amandaghassaei | 12:49813131dd15 | 169 | } |
amandaghassaei | 12:49813131dd15 | 170 | |
amandaghassaei | 12:49813131dd15 | 171 | float finalAngleTh2(float targetPosition, float armLength){ |
amandaghassaei | 12:49813131dd15 | 172 | if (targetIsCardinal(targetPosition)) return M_PI-2.0*asin(1/2.0*_latticePitch/armLength); |
amandaghassaei | 12:49813131dd15 | 173 | return M_PI-2.0*asin(1/2.0*_latticePitch*sqrt(double(2.0))/armLength); |
amandaghassaei | 12:49813131dd15 | 174 | } |
amandaghassaei | 12:49813131dd15 | 175 | |
amandaghassaei | 12:49813131dd15 | 176 | bool targetIsCardinal(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 177 | if (fmod(targetPosition,2) == 0) return true; |
amandaghassaei | 12:49813131dd15 | 178 | return false; |
amandaghassaei | 12:49813131dd15 | 179 | } |
amandaghassaei | 12:49813131dd15 | 180 | |
amandaghassaei | 12:49813131dd15 | 181 | float finalAngleSign(volatile float z[4], float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 182 | float approachDirection = targetDirection(targetPosition); |
amandaghassaei | 12:49813131dd15 | 183 | if (approachDirection<0 || (approachDirection==0 && z[3]<0)) return -1; |
amandaghassaei | 12:49813131dd15 | 184 | return 1; |
amandaghassaei | 12:49813131dd15 | 185 | } |
amandaghassaei | 12:49813131dd15 | 186 | |
amandaghassaei | 12:49813131dd15 | 187 | float targetDirection(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 188 | if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -1; |
amandaghassaei | 12:49813131dd15 | 189 | if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1; |
amandaghassaei | 12:49813131dd15 | 190 | return 0; |
amandaghassaei | 12:49813131dd15 | 191 | } |
amandaghassaei | 16:5b19be27f08a | 192 | |
amandaghassaei | 16:5b19be27f08a | 193 | float targetXPosition(float targetPosition){ |
amandaghassaei | 16:5b19be27f08a | 194 | if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -_latticePitch; |
amandaghassaei | 16:5b19be27f08a | 195 | if (targetPosition == 2 || targetPosition == 5 || targetPosition == 8) return 0; |
amandaghassaei | 16:5b19be27f08a | 196 | if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return _latticePitch; |
amandaghassaei | 16:5b19be27f08a | 197 | _pc->printf("invalid target position %f", targetPosition); |
amandaghassaei | 16:5b19be27f08a | 198 | return _latticePitch; |
amandaghassaei | 16:5b19be27f08a | 199 | } |
amandaghassaei | 16:5b19be27f08a | 200 | |
amandaghassaei | 16:5b19be27f08a | 201 | float targetYPosition(float targetPosition){ |
amandaghassaei | 16:5b19be27f08a | 202 | if (targetPosition < 4) return _latticePitch; |
amandaghassaei | 16:5b19be27f08a | 203 | if (targetPosition < 7) return 0; |
amandaghassaei | 16:5b19be27f08a | 204 | if (targetPosition < 10) return -_latticePitch; |
amandaghassaei | 16:5b19be27f08a | 205 | _pc->printf("invalid target position %f", targetPosition); |
amandaghassaei | 16:5b19be27f08a | 206 | return 0; |
amandaghassaei | 16:5b19be27f08a | 207 | } |
amandaghassaei | 2:17b18ea93551 | 208 | |
amandaghassaei | 2:17b18ea93551 | 209 | }; |
amandaghassaei | 2:17b18ea93551 | 210 | |
amandaghassaei | 2:17b18ea93551 | 211 | #endif |