File content as of revision 0:371773dd3dd1:

#include "fader.h"
#define MUSTSTOP    //some motordrivers will not change direction without a stop
//#define GETSTAT

int n =0;
float sumprod = 0.0, sum = 0;;

DigitalOut track(LED2); //debug only

#ifdef USESUPPLY
AnalogIn Vmotor(p17);
float getVmotor() {
    return Vmotor*13.3;    //for 3.3V referemce and 100k,33k divider
}
#else
float getVmotor() {
    return 6.0;    //assume 6V supply
}
#endif


servo::servo(PinName p, PinName f, PinName r, PinName a): Motor(p, f, r, 0) {
    const float Ts = 0.001;//sample period in seconds, 1kHz
    deadband = 0.001; // +/- 0.5mm, noise???
    _pwm.period_us(100); //10kHz otherwise very annoying sound
    fb = new AnalogIn(a);
    flt = new medianFilter(7);
    float voltage = getVmotor(); //make the gain dependent on the supply voltage
    float Kp = 9.0 - 0.5*voltage; //gain=6 @6V
    pid = new PID(Kp , 0.05, 0.000002, Ts);//work well @6V motor supply
    pid->setInputLimits(0, 1.0);
    pid->setOutputLimits(-1.0, 1.0);
    pid->setBias(0.0);//just to set internal feedforward variable
    _setPoint = 0.0;
    pid->setSetPoint(_setPoint);
    tick.attach(this,&servo::process, Ts);
}

void servo::process() {
    update();
    float p = pos();//(filtered) value of the potentiometer as a value between 0.0 and 1.0
    pid->setProcessValue(p); //set it as the value to control (Ist-wert)
    float out = pid->compute(); //compute the motor speed
#ifdef MUSTSTOP
    if ((out > 0 && _out < 0) || (out < 0 && _out > 0))
        _out = 0.0;//stop first
    else
        _out = out;
#else
    _out = out;
#endif
    if (fabs(p - _setPoint) < deadband) {
        coast(); //near setpoint so disconnect motor to allow manual movement
        coasting = true;
    } else {
        speed(_out);
        coasting = false;
    }
}

fader::fader(PinName p, PinName f, PinName r, PinName a, PinName t): servo(p,f,r,a) {
    thres = 0.01;
    thres2 = 0.001;
    count = 0;
    command = 0;
    state = tracking;
    if (t != NC)
        touch = new AnalogIn(t);
    else
        touch = 0;
}

void fader::process() { //called by the ticker every 1 ms
    float p;
    switch (state) { //make sure that each branch calls either servo::process or servo::update for proper filtering
        case tracking:
            servo::process();
            p = pos();
            if (isCoasting()) { //servo is near it's setpoint, motor is off
                state = holding;
                lastpos = p; //save the position that was reached
                track = 1; //debug
            }
            break;
        case holding:
            update();
            p = pos();
            if (fabs(lastpos - p) > thres) { //apparently position has changed (manual move)
                state = moving;
                printf("moving from %f to %f\n", lastpos, p);
                lastpos = p;
                count = 0;
            }//if not, stay in 'holding' until next 'set' command, do not update lastpos
            break;
        case moving:
            //not tracking but coasting
            update();
            p = pos();
            if (fabs(lastpos - p) < thres2) {
                count++;
            } else {
                count = 0;
            }
            if (count > 100) { //apparently movement has stopped, movement less then thres2 for 100ms
                printf("movement stopped at %f\n", p);
                servo::set(p); //update the servo setpoint (has no effect because state is not tracking)
                if (command)
                    command(p); //invoke the OnMove handler
                state = holding; //go back to holding state to allow a new fader::set and return to tracking
            }
            lastpos = p;
            break;
        default: //cannot happen
            update();
            p = pos();
    }
    if (touch && *touch > thres) { //stub: if input fullfills some condition (touch sense on fader)
        if (command)
            command(p);
    }
#ifdef GETSTAT
    sum += p;
    sumprod += p*p;
    n++;
    if (n == 1000) {
        float mean = sum/n;
        float var = sumprod/n - mean*mean;
        printf("n=%d, E=%f, Var = %f, sDev=%f\n", n, mean, var, sqrt(var));
        n=0;
        sum=0;
        sumprod=0;
    }
#endif
}