Francisco Guerra
/
SymbitronValves
Amplitude Modulation Code
Fork of SymbitronValves by
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(); }*/ }