Amplitude Modulation Code

Dependencies:   HIDScope mbed

Fork of SymbitronValves by First Last

main.cpp

Committer:
vsluiter
Date:
2015-06-30
Revision:
4:e69e2983bb6e
Parent:
3:28efa5d4ebe2
Child:
5:4772ef79675f

File content as of revision 4:e69e2983bb6e:

#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

DigitalOut v1open(V1_OPEN);
DigitalOut v1close(V1_CLOSE);
HIDScope scope(3);
AnalogIn pressure(PRESSURE);
TSIAnalogSlider slider(TSI_A, TSI_B, 100);
Ticker valvetick;

float setpoint1 = 0;

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

void valvecontrol(void)
{
    static uint16_t usbcounter = 0;
    float meas = getPressure_Pa();
    if(setpoint1 - meas >2000) {
        v1close = 0;
        v1open = 1;
    } else if (meas - setpoint1 > 1000) {
        v1close = 1;
        v1open = 0;
    }
    else
        v1open = v1close = 0;
    usbcounter++;
    if(usbcounter == 200) {
        scope.set(0,setpoint1);
        scope.set(1,meas);
        scope.set(2,meas);
        scope.send();
        usbcounter = 0;
    }
}

int main()
{
    valvetick.attach(valvecontrol,0.0001);
    while(1) {
        setpoint1 = slider.readPercentage()*30000;
        wait(0.1);
    }
    /*
    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);

    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();
    }*/
}