Diff: Controls/Target.h

Revision:

12:49813131dd15

Parent:

9:1d9b24d7ac77

Child:

15:d88f10b3b5f8

--- a/Controls/Target.h	Tue Dec 08 22:52:46 2015 +0000
+++ b/Controls/Target.h	Wed Dec 09 03:08:30 2015 +0000
@@ -1,33 +1,113 @@
 #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(){
-            setPosition(6);
+            _latticePitch = 0.350;
         }
         
         void setPC(Serial *pc){
             _pc = pc;
         }
     
-        void setPosition(int position){
+        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;
+        }
     
 };