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@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?

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 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