bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Target.h
- Committer:
- amandaghassaei
- Date:
- 2015-12-09
- Revision:
- 12:49813131dd15
- Parent:
- 9:1d9b24d7ac77
- Child:
- 15:d88f10b3b5f8
File content as of revision 12:49813131dd15:
#ifndef Target_h #define Target_h #define M_PI 3.14159265358979323846 //#include <math.h> #include "Kinematics.h" class Target{ // 1 2 3 // 4 5 6 // 7 8 9 public: Target(){ _latticePitch = 0.350; } void setPC(Serial *pc){ _pc = pc; } void setPosition(int position, float p[10]){ _position = position; _th1Final = finalAngleTh1(position, p[0]);//p[0] = linkLength _th2Final = finalAngleTh2(position, p[0]); float z[4] = {_th1Final, _th2Final, 0, 0}; _energy = getEnergy(z, p); } int getPosition(){ return _position; } float getTargetEnergy(){ return _energy; } float getFinalTh1(volatile float z[4]){ return _th1Final*finalAngleSign(z, _position); } float getFinalTh2(volatile float z[4]){ return _th2Final*finalAngleSign(z, _position); } float getTheta2ForTarget(volatile float z[4]){ // float th1 = z[0]; // float th2 = z[1]; // float dth1 = z[2]; // float dth2 = z[3]; // float approachDirection = (dth1 > 0 ? 1 : -1); // float th1Final = getFinalTh1(z); float th2Final = getFinalTh2(z); return th2Final; } private: Serial *_pc; int _position; float _energy; float _th1Final; float _th2Final; float _latticePitch; float finalAngleTh1(float targetPosition, float armLength){ float th1; if (targetIsCardinal(targetPosition)) th1 = asin(1/2.0*_latticePitch/armLength); else th1 = asin(1/2.0*_latticePitch*sqrt(double(2))/armLength); th1 += finalAngleTh1Rotation(targetPosition); return th1; } float finalAngleTh1Rotation(float targetPosition){ if (targetPosition == 2) return M_PI/2.0; if (targetPosition == 8) return -M_PI/2.0; if (targetPosition == 1 || targetPosition == 3) return M_PI/4.0; if (targetPosition == 7 || targetPosition == 9) return -M_PI/4.0; return 0; } float finalAngleTh2(float targetPosition, float armLength){ if (targetIsCardinal(targetPosition)) return M_PI-2.0*asin(1/2.0*_latticePitch/armLength); return M_PI-2.0*asin(1/2.0*_latticePitch*sqrt(double(2.0))/armLength); } bool targetIsCardinal(float targetPosition){ if (fmod(targetPosition,2) == 0) return true; return false; } float finalAngleSign(volatile float z[4], float targetPosition){ float approachDirection = targetDirection(targetPosition); if (approachDirection<0 || (approachDirection==0 && z[3]<0)) return -1; return 1; } float targetDirection(float targetPosition){ if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -1; if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1; return 0; } }; #endif