Diff: Controls/Target.h

Revision:

16:5b19be27f08a

Parent:

15:d88f10b3b5f8

Child:

17:8a0e647cf551

--- a/Controls/Target.h	Thu Dec 10 09:46:56 2015 +0000
+++ b/Controls/Target.h	Thu Dec 10 10:55:35 2015 +0000
@@ -15,6 +15,8 @@
     
         Target(){
             _latticePitch = 0.350;
+            setTargetingStarted(false);
+            setFinalDTh1(0);
         }
         
         void setPC(Serial *pc){
@@ -26,7 +28,7 @@
             _position = position;
             _th1Final = finalAngleTh1(position, p[0]);//p[0] = linkLength
             _th2Final = finalAngleTh2(position, p[0]);
-            float z[4] = {_th1Final, _th2Final, 0, 0};
+            float z[4] = {_th1Final, _th2Final, _dth1Final, 0};
             _energy = getEnergy(z, p);
         }
     
@@ -46,18 +48,93 @@
             return _th2Final*finalAngleSign(z, _position);
         }
         
-        float getTheta2ForTarget(volatile float z[4]){
-            
-//            float th1 = z[0];
+        void setFinalDTh1(float dth1Final){
+            _dth1Final = dth1Final;
+        }
+        
+        float getFinalDTh1(){
+            return _dth1Final;
+        }
+        
+        bool getTargetingStarted(){
+            return _isTargeting;
+        }
+        
+        void setTargetingStarted(bool state){
+            _isTargeting = state;
+        }
+        
+        bool shouldSwitchToTargetingController(volatile float z[4], float p[10]){
+
+            float th1 = z[0];
 //            float th2 = z[1];
-//            float dth1 = z[2];
+            float dth1 = z[2];
 //            float dth2 = z[3];
-
-//            float approachDirection = (dth1 > 0 ? 1 : -1);
-//            float th1Final = getFinalTh1(z);
-            float th2Final = getFinalTh2(z);            
-                    
-            return th2Final;
+            
+            float currentEnergy = getEnergy(z, p);
+            float th1Final = getFinalTh1(z);
+            float th2Final = getFinalTh2(z);
+            
+            float targetEnergy = getTargetEnergy();
+            
+            if (currentEnergy < targetEnergy) return false;
+//            if (abs(dth1) < getFinalDTh1()/2.0) return false;
+                
+            float targetApproachDir = 1;
+            if (dth1<0) targetApproachDir = -1;
+            float desiredTargetApproachDirection = targetDirection(_position);
+            if (desiredTargetApproachDirection != 0 && targetApproachDir != desiredTargetApproachDirection) return false;            
+            
+            float th1Rel = boundTheta(th1);
+            
+            if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/4) return false;        
+            if (targetApproachDir*th1Rel > targetApproachDir*th1Final-M_PI/3) return true;
+            return false;
+        }
+        
+        float calcTargetingForce(volatile float z[4], float p[10], float K, float D){
+            
+            float th1 = z[0];
+            float th2 = z[1];
+            float dth1 = z[2];
+            float dth2 = z[3];
+               
+            float th1Final = getFinalTh1(z);
+            float th2Final = getFinalTh2(z);
+                
+            float targetPosition[2];
+            targetPosition[0] = targetXPosition(_position);
+            targetPosition[1] = targetYPosition(_position);
+            
+            float mAngle = th1Final;
+            if (mAngle > M_PI/2.0) mAngle -= M_PI/2.0;
+            else if (mAngle < -M_PI/2.0) mAngle += M_PI/2.0;
+            float m = 1.0/tan(mAngle);
+            float a = -m;
+            float b = 1;
+            float c = m*targetPosition[0]-targetPosition[1];
+            
+            float gripperPosition[2];
+            getGripperPosition(gripperPosition, z, p);
+            float gripperVelocity[2];
+            getGripperVelocity(gripperVelocity, z, p);
+            
+            float x = (b*(b*gripperPosition[0] - a*gripperPosition[1])-a*c)/(a*a + b*b)-gripperPosition[0];
+            float y = (a*(-b*gripperPosition[0] + a*gripperPosition[1])-b*c)/(a*a + b*b)-gripperPosition[1];
+            float norm = sqrt(x*x + y*y);
+            float xUnit = x/norm;
+            float yUnit = y/norm;
+            float dNorm = gripperVelocity[0]*xUnit + gripperVelocity[1]*yUnit;
+            
+//            float K = gains->getSwingUpK();
+//            float D = gains->getSwingUpD();
+            
+            float gain = (K*norm - D*dNorm);
+            float forceTaskSp[2] = {gain*xUnit, gain*yUnit};
+            float Jtrans[2][2];
+            getGripperJacobianTranspose(Jtrans, z, p);
+        
+            return Jtrans[0][1]*forceTaskSp[0]+Jtrans[1][1]*forceTaskSp[1];
         }
     
     
@@ -65,11 +142,14 @@
     
         Serial *_pc;
         
+        bool _isTargeting;
+        
     
         int _position;
         float _energy;
         float _th1Final;
         float _th2Final;
+        float _dth1Final;
         float _latticePitch;
         
         float finalAngleTh1(float targetPosition, float armLength){
@@ -109,6 +189,22 @@
             if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return 1;
             return 0;
         }
+        
+        float targetXPosition(float targetPosition){
+            if (targetPosition == 1 || targetPosition == 4 || targetPosition == 7) return -_latticePitch;
+            if (targetPosition == 2 || targetPosition == 5 || targetPosition == 8) return 0;
+            if (targetPosition == 3 || targetPosition == 6 || targetPosition == 9) return _latticePitch;
+            _pc->printf("invalid target position %f", targetPosition);
+            return _latticePitch;
+        }
+        
+        float targetYPosition(float targetPosition){
+            if (targetPosition < 4) return _latticePitch;
+            if (targetPosition < 7) return 0;
+            if (targetPosition < 10) return -_latticePitch;
+            _pc->printf("invalid target position %f", targetPosition);
+            return 0;
+        }
     
 };