File content as of revision 3:28efa5d4ebe2:

#include "mbed.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "tsi_sensor.h"

#define V1_CLOSE  D2
#define V1_OPEN   D3
#define V2_CLOSE  D4
#define V2_OPEN   D5
#define V3_CLOSE  A2
#define V3_OPEN   A3
#define V4_CLOSE  D8
#define V4_OPEN   D9

#define TSI_A PTB16 
#define TSI_B PTB17

#define PRESSURE A0

HIDScope scope(3);
AnalogIn pressure(PRESSURE);
TSIAnalogSlider slider(TSI_A, TSI_B, 100);
    
class Valve
{
    public:
    Valve(PinName open, PinName close);
    void write(float pwm, bool direct_mode=true);
    void SetDeadtime(float deadtime){if(_period < deadtime) _period = deadtime; _deadtime = deadtime; InitializeValve(_period,_deadtime);};
    private:
    float _pwm;
    float _period;
    float _deadtime;
    void InitializeValve(float period, float deadtime);
    FastPWM _open;
    FastPWM _close;
};

Valve::Valve(PinName open, PinName close): _open(open), _close(close)
{
    InitializeValve(0.05,0.006);
}

void Valve::InitializeValve(float period, float deadtime)
{
    _period = period;
    _deadtime = deadtime;
    _open.period(_period);
    _close.period(_period);
    _open.pulsewidth(0);
    _close.pulsewidth(0);   
}


void Valve::write(float pwm, bool direct_mode)
{
    //input conditioning
    if(pwm > 1)
        pwm = 1;
    if(pwm < -1)
        pwm = -1;
        
    if(direct_mode)
    {
        if(pwm > 1)
            pwm = 1;
        if(pwm < -1)
            pwm = -1;
        if(pwm > 0)
        {
            _close.pulsewidth(0);
            _open.pulsewidth(_period*pwm);
        }
        else
        {
            _open.pulsewidth(0);
            _close.pulsewidth(_period*pwm*-1);
        }
    }
    else
    {
        if(pwm > 0.001)
        {
            _close.pulsewidth(0);
            _open.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm) );
        }
        else if(pwm < 0.001)
        {
            _open.pulsewidth(0);
            _close.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm*-1));
        }
        else
        {
            _open.pulsewidth(0);
            _close.pulsewidth(0);
        }
    }
}

class PIDcontroller
{
    public:
    PIDcontroller(float p, float i, float d, float dt); 
    float control(float setpoint, float measurement);
    private:
    float _p;
    float _i;
    float _d;
    float _dt;
    float _integrator;
    float _previous_error;
};

PIDcontroller::PIDcontroller(float p, float i, float d, float dt)
{
    _p = p;
    _i = i;
    _d = d;
    _dt = dt;
    _previous_error = 0;
    _integrator = 0;
}

float PIDcontroller::control(float setpoint, float measurement)
{
    float output = 0;
    float error = setpoint - measurement;
    _integrator += error*_dt;
    output  = error * _p;
    output += _integrator * _i;
    output += ((_previous_error-error)/_dt)*_d;
    _previous_error = error;
    return output;
}

float getPressure_Pa(void)
{
    float voltage = pressure.read()*3.3;
    float sensor_output = (voltage*2)-0.2;//offset compensation
    float pressure = sensor_output*10000;
    return pressure;
}

int main() {
    PIDcontroller pid(0.001,0,0.000,0.2);
    //Valve v1(V1_OPEN, V1_CLOSE);
    Valve v2(V2_OPEN, V2_CLOSE);
    Valve v3(V3_OPEN, V3_CLOSE);
    Valve v4(V4_OPEN, V4_CLOSE);
    DigitalOut v1open(V1_OPEN);
    DigitalOut v1close(V1_CLOSE);
    while(1) {
        float pid_out;
        float setpoint;
        float meas;
        static float time = 0;
        wait(0.01);
        time +=0.2;
        setpoint = slider.readPercentage()*20000;
        pid_out = pid.control(setpoint, getPressure_Pa());
        //if(pid_out > 0.1)
        //    v1.write(0.1);
        //if(pid_out < -0.1)
         //   v1.write(-0.1);
        meas = getPressure_Pa();
        if(abs(setpoint - meas)>4000)
        {   
            int32_t count = 0;
            if(meas < setpoint )
            {
                while( (getPressure_Pa() < setpoint) && count < 100)
                {
                    wait(0.001);
                    v1open = 1;
                    count++;
                }
            }
            else
            {
                while( (getPressure_Pa() > setpoint) && count < 100)
                {
                        v1close = 1;
                    wait(0.001);
                    count++;
                }
            }
            v1open = v1close = 0;
                
        }
        //v1.write(pid_out,false);
        v2.write(0);//sin(time));
        v3.write(0);
        v4.write(0);//sin(time),false);
        scope.set(0,getPressure_Pa());
        scope.set(1,slider.readPercentage());
        scope.set(2,pid_out);
        scope.send();
    }
}