File content as of revision 0:911517b34248:

#include "mbed.h"
#include "filter.h"
#include "HipControl.h"

#define TO_RAD(x) (x * 0.01745329252)  // *pi/180

HipControl::HipControl(PinName pwm, PinName dirpin): _pwm(pwm), _dir(dirpin), Kp(0.02), Kd(0.002), sat(0.4), Kp0(0.001), u_prev(0), T(0.001), sign(1)
{
    _pwm.period(.00005);
    _pwm=0;
    _dir=0;
}
void HipControl::clear(){
    error[1]=error[0]=0;
    }
void HipControl::pwmPeriod(float a)
{
    _pwm.period(a);
}
void HipControl::sampleTime(float time)
{
    T=time;
}
void HipControl::setGains(float P, float D)
{
    Kp=P;
    Kd=D;
}

void HipControl::setSat(float limit)
{
    sat=limit;
}


float HipControl::read()
{
    return u_prev;
}

void HipControl::openLoop(float input)
{
    u=sign*input;
    if (u > 0) {
        _dir = 1;
    } else {
        _dir = 0;
    }

    if (u > sat) {
        u = sat;
    } else if (u < -sat) {
        u = -sat;
    }
    u_prev=u;
    if (u < 0) {
        u = -u;
    }
    _pwm = u;
}

void HipControl::FL(float ref, float pos)
{
    error[0]=error[1];
    error[1] = ref - pos;

    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T+.173*sin(TO_RAD(pos)));

    if (u > 0) {
        _dir = 1;
    } else {
        _dir = 0;
    }

    if (u > sat) {
        u = sat;
    } else if (u < -sat) {
        u = -sat;
    }
    u_prev=u;
    if (u < 0) {
        u = -u;
    }
    _pwm = u;
}

void HipControl::FL_new(float ref, float pos)
{
    error[0]=error[1];
    error[1] = ref - pos;
    float blah=Kd/3*2/T+1;
    float alpha=(Kp+Kp*Kd/3*2/T+Kd*2/T)/blah;
    float beta=(Kp-Kp*Kd/3*2/T-Kd*2/T)/blah;
    float gamma=(Kd/3*2/T+1)/blah;

    u = sign*controlFilter.Butterworth_1K(alpha*error[1]+beta*error[0]-gamma*u_prev+.173*sin(TO_RAD(pos)));

    if (u > 0) {
        _dir = 1;
    } else {
        _dir = 0;
    }

    if (u > sat) {
        u = sat;
    } else if (u < -sat) {
        u = -sat;
    }
    u_prev=u;

    if (u < 0) {
        u = -u;
    }
    _pwm = u;
}

void HipControl::PD(float ref, float pos)
{
    error[0]=error[1];
    error[1] = ref - pos;

    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T);

    if (u > 0) {
        _dir = 1;
    } else {
        _dir = 0;
    }

    if (u > sat) {
        u = sat;
    } else if (u < -sat) {
        u = -sat;
    }
    u_prev=u;
    if (u < 0) {
        u = -u;
    }
    _pwm = u;
}

void HipControl::P(float ref, float pos)
{
    error[0]=error[1];
    error[1] = ref - pos;

    u = sign*Kp*error[1];
    float temp=controlFilter.Butterworth_1K(u);

    if (u > 0) {
        _dir = 1;
    } else {
        _dir = 0;
    }

    if (u > sat) {
        u = sat;
    } else if (u < -sat) {
        u = -sat;
    }
    u_prev=u;
    if (u < 0) {
        u = -u;
    }
    _pwm = u;
}
void HipControl::off()
{
    _pwm=0;
}

void HipControl::flip()
{
    sign=-sign;
}