File content as of revision 13:64d337c5114e:

#include "Dynamics.h"
#include <math.h>
# define M_PI           3.14159265358979323846

float calcTau(volatile float z[4], float p[10], Gains *gains, Target *target, Serial *pc){
    
    float th1 = z[0];
    float th2 = z[1];
    float dth1 = z[2];
    float dth2 = z[3];
    
    float A[2][2];
    getMassMatrix(A, z, p);
    float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0];
    
    float corrCentripComp[2];
    getCoriolisCentrip(corrCentripComp, z, p);
    float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0];
    
    float gravityComp[2];
    getGravity(gravityComp, z, p);
    float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0];
    
    float K = gains->getSwingUpK();
    float D = gains->getSwingUpD();
    
    float th2Des;
    if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z);
    else th2Des = thetaDesiredForSwingUp(-1.5, 1.5, z);
    
    float ddth2 = K*(th2Des - th2) - D*dth2;

    return gains->getCurrentP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat;
}


float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){
    
    float th1 = z[0];
    float dth1 = z[2];
    
    int numTurns = fix(th1/(2*M_PI));
    float th1Rel = th1-numTurns*2*M_PI;
    
    if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1)
    return rangeMax*abs(cos(th1Rel/2.0));
}

int fix(float val){//round toward zero
    return val > 0 ? floor(val) : ceil(val);
}