bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Target.h
- Revision:
- 16:5b19be27f08a
- Parent:
- 15:d88f10b3b5f8
- Child:
- 17:8a0e647cf551
diff -r d88f10b3b5f8 -r 5b19be27f08a Controls/Target.h --- 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; + } };