bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Target.h@12:49813131dd15, 2015-12-09 (annotated)
- Committer:
- amandaghassaei
- Date:
- Wed Dec 09 03:08:30 2015 +0000
- Revision:
- 12:49813131dd15
- Parent:
- 9:1d9b24d7ac77
- Child:
- 15:d88f10b3b5f8
simple targeting;
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 | 2:17b18ea93551 | 18 | } |
amandaghassaei | 9:1d9b24d7ac77 | 19 | |
amandaghassaei | 9:1d9b24d7ac77 | 20 | void setPC(Serial *pc){ |
amandaghassaei | 9:1d9b24d7ac77 | 21 | _pc = pc; |
amandaghassaei | 9:1d9b24d7ac77 | 22 | } |
amandaghassaei | 2:17b18ea93551 | 23 | |
amandaghassaei | 12:49813131dd15 | 24 | void setPosition(int position, float p[10]){ |
amandaghassaei | 2:17b18ea93551 | 25 | _position = position; |
amandaghassaei | 12:49813131dd15 | 26 | _th1Final = finalAngleTh1(position, p[0]);//p[0] = linkLength |
amandaghassaei | 12:49813131dd15 | 27 | _th2Final = finalAngleTh2(position, p[0]); |
amandaghassaei | 12:49813131dd15 | 28 | float z[4] = {_th1Final, _th2Final, 0, 0}; |
amandaghassaei | 12:49813131dd15 | 29 | _energy = getEnergy(z, p); |
amandaghassaei | 2:17b18ea93551 | 30 | } |
amandaghassaei | 2:17b18ea93551 | 31 | |
amandaghassaei | 2:17b18ea93551 | 32 | int getPosition(){ |
amandaghassaei | 2:17b18ea93551 | 33 | return _position; |
amandaghassaei | 2:17b18ea93551 | 34 | } |
amandaghassaei | 12:49813131dd15 | 35 | |
amandaghassaei | 12:49813131dd15 | 36 | float getTargetEnergy(){ |
amandaghassaei | 12:49813131dd15 | 37 | return _energy; |
amandaghassaei | 12:49813131dd15 | 38 | } |
amandaghassaei | 12:49813131dd15 | 39 | |
amandaghassaei | 12:49813131dd15 | 40 | float getFinalTh1(volatile float z[4]){ |
amandaghassaei | 12:49813131dd15 | 41 | return _th1Final*finalAngleSign(z, _position); |
amandaghassaei | 12:49813131dd15 | 42 | } |
amandaghassaei | 12:49813131dd15 | 43 | |
amandaghassaei | 12:49813131dd15 | 44 | float getFinalTh2(volatile float z[4]){ |
amandaghassaei | 12:49813131dd15 | 45 | return _th2Final*finalAngleSign(z, _position); |
amandaghassaei | 12:49813131dd15 | 46 | } |
amandaghassaei | 12:49813131dd15 | 47 | |
amandaghassaei | 12:49813131dd15 | 48 | float getTheta2ForTarget(volatile float z[4]){ |
amandaghassaei | 12:49813131dd15 | 49 | |
amandaghassaei | 12:49813131dd15 | 50 | // float th1 = z[0]; |
amandaghassaei | 12:49813131dd15 | 51 | // float th2 = z[1]; |
amandaghassaei | 12:49813131dd15 | 52 | // float dth1 = z[2]; |
amandaghassaei | 12:49813131dd15 | 53 | // float dth2 = z[3]; |
amandaghassaei | 12:49813131dd15 | 54 | |
amandaghassaei | 12:49813131dd15 | 55 | // float approachDirection = (dth1 > 0 ? 1 : -1); |
amandaghassaei | 12:49813131dd15 | 56 | // float th1Final = getFinalTh1(z); |
amandaghassaei | 12:49813131dd15 | 57 | float th2Final = getFinalTh2(z); |
amandaghassaei | 12:49813131dd15 | 58 | |
amandaghassaei | 12:49813131dd15 | 59 | return th2Final; |
amandaghassaei | 12:49813131dd15 | 60 | } |
amandaghassaei | 2:17b18ea93551 | 61 | |
amandaghassaei | 2:17b18ea93551 | 62 | |
amandaghassaei | 2:17b18ea93551 | 63 | private: |
amandaghassaei | 2:17b18ea93551 | 64 | |
amandaghassaei | 9:1d9b24d7ac77 | 65 | Serial *_pc; |
amandaghassaei | 12:49813131dd15 | 66 | |
amandaghassaei | 9:1d9b24d7ac77 | 67 | |
amandaghassaei | 2:17b18ea93551 | 68 | int _position; |
amandaghassaei | 12:49813131dd15 | 69 | float _energy; |
amandaghassaei | 12:49813131dd15 | 70 | float _th1Final; |
amandaghassaei | 12:49813131dd15 | 71 | float _th2Final; |
amandaghassaei | 12:49813131dd15 | 72 | float _latticePitch; |
amandaghassaei | 12:49813131dd15 | 73 | |
amandaghassaei | 12:49813131dd15 | 74 | float finalAngleTh1(float targetPosition, float armLength){ |
amandaghassaei | 12:49813131dd15 | 75 | float th1; |
amandaghassaei | 12:49813131dd15 | 76 | if (targetIsCardinal(targetPosition)) th1 = asin(1/2.0*_latticePitch/armLength); |
amandaghassaei | 12:49813131dd15 | 77 | else th1 = asin(1/2.0*_latticePitch*sqrt(double(2))/armLength); |
amandaghassaei | 12:49813131dd15 | 78 | th1 += finalAngleTh1Rotation(targetPosition); |
amandaghassaei | 12:49813131dd15 | 79 | return th1; |
amandaghassaei | 12:49813131dd15 | 80 | } |
amandaghassaei | 12:49813131dd15 | 81 | |
amandaghassaei | 12:49813131dd15 | 82 | float finalAngleTh1Rotation(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 83 | if (targetPosition == 2) return M_PI/2.0; |
amandaghassaei | 12:49813131dd15 | 84 | if (targetPosition == 8) return -M_PI/2.0; |
amandaghassaei | 12:49813131dd15 | 85 | if (targetPosition == 1 || targetPosition == 3) return M_PI/4.0; |
amandaghassaei | 12:49813131dd15 | 86 | if (targetPosition == 7 || targetPosition == 9) return -M_PI/4.0; |
amandaghassaei | 12:49813131dd15 | 87 | return 0; |
amandaghassaei | 12:49813131dd15 | 88 | } |
amandaghassaei | 12:49813131dd15 | 89 | |
amandaghassaei | 12:49813131dd15 | 90 | float finalAngleTh2(float targetPosition, float armLength){ |
amandaghassaei | 12:49813131dd15 | 91 | if (targetIsCardinal(targetPosition)) return M_PI-2.0*asin(1/2.0*_latticePitch/armLength); |
amandaghassaei | 12:49813131dd15 | 92 | return M_PI-2.0*asin(1/2.0*_latticePitch*sqrt(double(2.0))/armLength); |
amandaghassaei | 12:49813131dd15 | 93 | } |
amandaghassaei | 12:49813131dd15 | 94 | |
amandaghassaei | 12:49813131dd15 | 95 | bool targetIsCardinal(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 96 | if (fmod(targetPosition,2) == 0) return true; |
amandaghassaei | 12:49813131dd15 | 97 | return false; |
amandaghassaei | 12:49813131dd15 | 98 | } |
amandaghassaei | 12:49813131dd15 | 99 | |
amandaghassaei | 12:49813131dd15 | 100 | float finalAngleSign(volatile float z[4], float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 101 | float approachDirection = targetDirection(targetPosition); |
amandaghassaei | 12:49813131dd15 | 102 | if (approachDirection<0 || (approachDirection==0 && z[3]<0)) return -1; |
amandaghassaei | 12:49813131dd15 | 103 | return 1; |
amandaghassaei | 12:49813131dd15 | 104 | } |
amandaghassaei | 12:49813131dd15 | 105 | |
amandaghassaei | 12:49813131dd15 | 106 | float targetDirection(float targetPosition){ |
amandaghassaei | 12:49813131dd15 | 107 | if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -1; |
amandaghassaei | 12:49813131dd15 | 108 | if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1; |
amandaghassaei | 12:49813131dd15 | 109 | return 0; |
amandaghassaei | 12:49813131dd15 | 110 | } |
amandaghassaei | 2:17b18ea93551 | 111 | |
amandaghassaei | 2:17b18ea93551 | 112 | }; |
amandaghassaei | 2:17b18ea93551 | 113 | |
amandaghassaei | 2:17b18ea93551 | 114 | #endif |