Target.h

00001 #ifndef Target_h
00002 #define Target_h
00003 
00004 #define M_PI 3.14159265358979323846
00005 //#include <math.h>
00006 #include "Kinematics.h"
00007 
00008 
00009 class Target{
00010 //            1   2   3
00011 //            4   5   6
00012 //            7   8   9
00013 
00014     public:
00015     
00016         Target(){
00017             _latticePitch = 0.350;
00018             setTargetingStarted(false);
00019             setFinalDTh1(0);
00020         }
00021         
00022         void setPC(Serial *pc){
00023             _pc = pc;
00024         }
00025     
00026         void setPosition(int position, float p[10]){
00027             if (position != 4 && position != 6) position = 6;
00028             _position = position;
00029             _th1Final = finalAngleTh1(position, p[0]);//p[0] = linkLength
00030             _th2Final = finalAngleTh2(position, p[0]);
00031             float z[4] = {_th1Final, _th2Final, _dth1Final, 0};
00032             _energy = getEnergy(z, p);
00033         }
00034     
00035         int getPosition(){
00036             return _position;
00037         }
00038         
00039         float getTargetEnergy(){
00040             return _energy;
00041         }
00042         
00043         float getFinalTh1(volatile float z[4]){
00044             return _th1Final*finalAngleSign(z, _position);
00045         }
00046         
00047         float getFinalTh2(volatile float z[4]){
00048             return _th2Final*finalAngleSign(z, _position);
00049         }
00050         
00051         void setFinalDTh1(float dth1Final){
00052             _dth1Final = dth1Final;
00053         }
00054         
00055         float getFinalDTh1(){
00056             return _dth1Final;
00057         }
00058         
00059         bool getTargetingStarted(){
00060             return _isTargeting;
00061         }
00062         
00063         void setTargetingStarted(bool state){
00064             _isTargeting = state;
00065         }
00066         
00067         bool shouldSwitchToTargetingController(volatile float z[4], float p[10]){
00068 
00069             float th1 = z[0];
00070 //            float th2 = z[1];
00071             float dth1 = z[2];
00072 //            float dth2 = z[3];
00073             
00074             float currentEnergy = getEnergy(z, p);
00075             float th1Final = getFinalTh1(z);
00076             float th2Final = getFinalTh2(z);
00077             
00078             float targetEnergy = getTargetEnergy();
00079             
00080             if (currentEnergy < targetEnergy) return false;
00081 //            if (abs(dth1) < getFinalDTh1()/2.0) return false;
00082                 
00083             float targetApproachDir = 1;
00084             if (dth1<0) targetApproachDir = -1;
00085             float desiredTargetApproachDirection = targetDirection(_position);
00086             if (desiredTargetApproachDirection != 0 && targetApproachDir != desiredTargetApproachDirection) return false;            
00087             
00088             float th1Rel = boundTheta(th1);
00089             
00090             if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/4) return false;        
00091             if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/3) return true;
00092             return false;
00093         }
00094         
00095         float calcTargetingForce(volatile float z[4], float p[10], float K, float D){
00096                         
00097             float th1 = z[0];
00098             float th2 = z[1];
00099             float dth1 = z[2];
00100             float dth2 = z[3];
00101                
00102             float th1Final = getFinalTh1(z);
00103             float th2Final = getFinalTh2(z);
00104                 
00105             float targetPosition[2];
00106             targetPosition[0] = targetXPosition(_position);
00107             targetPosition[1] = targetYPosition(_position);
00108             
00109             float mAngle = th1Final;
00110             if (mAngle > M_PI/2.0) mAngle -= M_PI/2.0;
00111             else if (mAngle < -M_PI/2.0) mAngle += M_PI/2.0;
00112             float m = 1.0/tan(mAngle);
00113             float a = -m;
00114             float b = 1;
00115             float c = m*targetPosition[0]-targetPosition[1];
00116             
00117             float gripperPosition[2];
00118             getGripperPosition(gripperPosition, z, p);
00119             float gripperVelocity[2];
00120             getGripperVelocity(gripperVelocity, z, p);
00121             
00122             float x = (b*(b*gripperPosition[0] - a*gripperPosition[1])-a*c)/(a*a + b*b)-gripperPosition[0];
00123             float y = (a*(-b*gripperPosition[0] + a*gripperPosition[1])-b*c)/(a*a + b*b)-gripperPosition[1];
00124             float norm = sqrt(x*x + y*y);
00125             float xUnit = x/norm;
00126             float yUnit = y/norm;
00127             float dNorm = gripperVelocity[0]*xUnit + gripperVelocity[1]*yUnit;
00128             
00129 //            float K = gains->getSwingUpK();
00130 //            float D = gains->getSwingUpD();
00131             
00132             float gain = (K*norm - D*dNorm);
00133             float forceTaskSp[2] = {gain*xUnit, gain*yUnit};
00134             float Jtrans[2][2];
00135             
00136             getGripperJacobianTranspose(Jtrans, z, p);            
00137         
00138             return Jtrans[0][1]*forceTaskSp[0]+Jtrans[1][1]*forceTaskSp[1];
00139         }
00140     
00141     
00142     private:
00143     
00144         Serial *_pc;
00145         
00146         bool _isTargeting;
00147         
00148     
00149         int _position;
00150         float _energy;
00151         float _th1Final;
00152         float _th2Final;
00153         float _dth1Final;
00154         float _latticePitch;
00155         
00156         float finalAngleTh1(float targetPosition, float armLength){
00157             float th1;
00158             if (targetIsCardinal(targetPosition)) th1 = asin(1/2.0*_latticePitch/armLength);
00159             else th1 = asin(1/2.0*_latticePitch*sqrt(double(2))/armLength);
00160             th1 += finalAngleTh1Rotation(targetPosition);
00161             return th1;
00162         }
00163         
00164         float finalAngleTh1Rotation(float targetPosition){
00165             if (targetPosition == 2) return M_PI/2.0;
00166             if (targetPosition == 8) return -M_PI/2.0;
00167             if (targetPosition == 1 || targetPosition == 3) return M_PI/4.0;
00168             if (targetPosition == 7 || targetPosition == 9) return -M_PI/4.0;
00169             return 0;
00170         }
00171         
00172         float finalAngleTh2(float targetPosition, float armLength){
00173             if (targetIsCardinal(targetPosition)) return M_PI-2.0*asin(1/2.0*_latticePitch/armLength);
00174             return M_PI-2.0*asin(1/2.0*_latticePitch*sqrt(double(2.0))/armLength);
00175         }
00176         
00177         bool targetIsCardinal(float targetPosition){
00178             if (fmod(targetPosition,2) == 0) return true;
00179             return false;
00180         }
00181         
00182         float finalAngleSign(volatile float z[4], float targetPosition){
00183             float approachDirection = targetDirection(targetPosition);
00184             if (approachDirection<0 || (approachDirection==0 && z[3]<0)) return -1;
00185             return 1;
00186         }
00187         
00188         float targetDirection(float targetPosition){
00189             if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -1;
00190             if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1;
00191             return 0;
00192         }
00193         
00194         float targetXPosition(float targetPosition){
00195             if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -_latticePitch;
00196             if (targetPosition == 2 || targetPosition == 5 || targetPosition == 8) return 0;
00197             if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return _latticePitch;
00198             _pc->printf("invalid target position %f\n", targetPosition);
00199             return _latticePitch;
00200         }
00201         
00202         float targetYPosition(float targetPosition){
00203             if (targetPosition < 4) return _latticePitch;
00204             if (targetPosition < 7) return 0;
00205             if (targetPosition < 10) return -_latticePitch;
00206             _pc->printf("invalid target position %f\n", targetPosition);
00207             return 0;
00208         }
00209     
00210 };
00211 
00212 #endif