File content as of revision 15:d88f10b3b5f8:

#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]){
            if (position != 4 && position != 6) position = 6;
            _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