File content as of revision 2:18e264a5b71d:

#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), _sample_period(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)
{
    _sample_period=time;
}
void HipControl::setGains(float P, float D)
{
    Kp=P;
    Kd=D;
}

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


float HipControl::readPWM()
{
    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])/_sample_period+.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])/_sample_period);

    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;
}