Francisco Guerra / Mbed 2 deprecated SymbitronValves

Amplitude Modulation Code

Dependencies:   HIDScope mbed

Fork of SymbitronValves by First Last

main.cpp@4:e69e2983bb6e, 2015-06-30 (annotated)

Committer:
vsluiter
Date:
Tue Jun 30 08:23:32 2015 +0000
Revision:
4:e69e2983bb6e
Parent:
3:28efa5d4ebe2
Child:
5:4772ef79675f
Reasonable working setup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:80993ec26dfd 1 #include "mbed.h"
vsluiter 0:80993ec26dfd 2 #include "FastPWM.h"
vsluiter 1:926025d703f4 3 #include "HIDScope.h"
vsluiter 1:926025d703f4 4 #include "tsi_sensor.h"
vsluiter 0:80993ec26dfd 5
vsluiter 1:926025d703f4 6 #define V1_CLOSE D2
vsluiter 1:926025d703f4 7 #define V1_OPEN D3
vsluiter 1:926025d703f4 8 #define V2_CLOSE D4
vsluiter 1:926025d703f4 9 #define V2_OPEN D5
vsluiter 1:926025d703f4 10 #define V3_CLOSE A2
vsluiter 1:926025d703f4 11 #define V3_OPEN A3
vsluiter 1:926025d703f4 12 #define V4_CLOSE D8
vsluiter 1:926025d703f4 13 #define V4_OPEN D9
vsluiter 1:926025d703f4 14
vsluiter 4:e69e2983bb6e 15 #define TSI_A PTB16
vsluiter 1:926025d703f4 16 #define TSI_B PTB17
vsluiter 0:80993ec26dfd 17
vsluiter 0:80993ec26dfd 18 #define PRESSURE A0
vsluiter 0:80993ec26dfd 19
vsluiter 4:e69e2983bb6e 20 DigitalOut v1open(V1_OPEN);
vsluiter 4:e69e2983bb6e 21 DigitalOut v1close(V1_CLOSE);
vsluiter 1:926025d703f4 22 HIDScope scope(3);
vsluiter 1:926025d703f4 23 AnalogIn pressure(PRESSURE);
vsluiter 1:926025d703f4 24 TSIAnalogSlider slider(TSI_A, TSI_B, 100);
vsluiter 4:e69e2983bb6e 25 Ticker valvetick;
vsluiter 4:e69e2983bb6e 26
vsluiter 4:e69e2983bb6e 27 float setpoint1 = 0;
vsluiter 4:e69e2983bb6e 28
vsluiter 0:80993ec26dfd 29 class Valve
vsluiter 0:80993ec26dfd 30 {
vsluiter 4:e69e2983bb6e 31 public:
vsluiter 0:80993ec26dfd 32 Valve(PinName open, PinName close);
vsluiter 1:926025d703f4 33 void write(float pwm, bool direct_mode=true);
vsluiter 4:e69e2983bb6e 34 void SetDeadtime(float deadtime) {
vsluiter 4:e69e2983bb6e 35 if(_period < deadtime) _period = deadtime;
vsluiter 4:e69e2983bb6e 36 _deadtime = deadtime;
vsluiter 4:e69e2983bb6e 37 InitializeValve(_period,_deadtime);
vsluiter 4:e69e2983bb6e 38 };
vsluiter 4:e69e2983bb6e 39 private:
vsluiter 0:80993ec26dfd 40 float _pwm;
vsluiter 0:80993ec26dfd 41 float _period;
vsluiter 1:926025d703f4 42 float _deadtime;
vsluiter 1:926025d703f4 43 void InitializeValve(float period, float deadtime);
vsluiter 0:80993ec26dfd 44 FastPWM _open;
vsluiter 0:80993ec26dfd 45 FastPWM _close;
vsluiter 0:80993ec26dfd 46 };
vsluiter 0:80993ec26dfd 47
vsluiter 0:80993ec26dfd 48 Valve::Valve(PinName open, PinName close): _open(open), _close(close)
vsluiter 0:80993ec26dfd 49 {
vsluiter 2:e7421003d1c9 50 InitializeValve(0.05,0.006);
vsluiter 1:926025d703f4 51 }
vsluiter 1:926025d703f4 52
vsluiter 1:926025d703f4 53 void Valve::InitializeValve(float period, float deadtime)
vsluiter 1:926025d703f4 54 {
vsluiter 1:926025d703f4 55 _period = period;
vsluiter 1:926025d703f4 56 _deadtime = deadtime;
vsluiter 0:80993ec26dfd 57 _open.period(_period);
vsluiter 0:80993ec26dfd 58 _close.period(_period);
vsluiter 0:80993ec26dfd 59 _open.pulsewidth(0);
vsluiter 4:e69e2983bb6e 60 _close.pulsewidth(0);
vsluiter 0:80993ec26dfd 61 }
vsluiter 0:80993ec26dfd 62
vsluiter 1:926025d703f4 63
vsluiter 1:926025d703f4 64 void Valve::write(float pwm, bool direct_mode)
vsluiter 0:80993ec26dfd 65 {
vsluiter 1:926025d703f4 66 //input conditioning
vsluiter 0:80993ec26dfd 67 if(pwm > 1)
vsluiter 0:80993ec26dfd 68 pwm = 1;
vsluiter 0:80993ec26dfd 69 if(pwm < -1)
vsluiter 0:80993ec26dfd 70 pwm = -1;
vsluiter 4:e69e2983bb6e 71
vsluiter 4:e69e2983bb6e 72 if(direct_mode) {
vsluiter 1:926025d703f4 73 if(pwm > 1)
vsluiter 1:926025d703f4 74 pwm = 1;
vsluiter 1:926025d703f4 75 if(pwm < -1)
vsluiter 1:926025d703f4 76 pwm = -1;
vsluiter 4:e69e2983bb6e 77 if(pwm > 0) {
vsluiter 1:926025d703f4 78 _close.pulsewidth(0);
vsluiter 1:926025d703f4 79 _open.pulsewidth(_period*pwm);
vsluiter 4:e69e2983bb6e 80 } else {
vsluiter 1:926025d703f4 81 _open.pulsewidth(0);
vsluiter 1:926025d703f4 82 _close.pulsewidth(_period*pwm*-1);
vsluiter 1:926025d703f4 83 }
vsluiter 4:e69e2983bb6e 84 } else {
vsluiter 4:e69e2983bb6e 85 if(pwm > 0.001) {
vsluiter 1:926025d703f4 86 _close.pulsewidth(0);
vsluiter 1:926025d703f4 87 _open.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm) );
vsluiter 4:e69e2983bb6e 88 } else if(pwm < 0.001) {
vsluiter 1:926025d703f4 89 _open.pulsewidth(0);
vsluiter 1:926025d703f4 90 _close.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm*-1));
vsluiter 4:e69e2983bb6e 91 } else {
vsluiter 1:926025d703f4 92 _open.pulsewidth(0);
vsluiter 1:926025d703f4 93 _close.pulsewidth(0);
vsluiter 1:926025d703f4 94 }
vsluiter 0:80993ec26dfd 95 }
vsluiter 0:80993ec26dfd 96 }
vsluiter 0:80993ec26dfd 97
vsluiter 1:926025d703f4 98 class PIDcontroller
vsluiter 1:926025d703f4 99 {
vsluiter 4:e69e2983bb6e 100 public:
vsluiter 4:e69e2983bb6e 101 PIDcontroller(float p, float i, float d, float dt);
vsluiter 1:926025d703f4 102 float control(float setpoint, float measurement);
vsluiter 4:e69e2983bb6e 103 private:
vsluiter 1:926025d703f4 104 float _p;
vsluiter 1:926025d703f4 105 float _i;
vsluiter 1:926025d703f4 106 float _d;
vsluiter 1:926025d703f4 107 float _dt;
vsluiter 1:926025d703f4 108 float _integrator;
vsluiter 1:926025d703f4 109 float _previous_error;
vsluiter 1:926025d703f4 110 };
vsluiter 1:926025d703f4 111
vsluiter 1:926025d703f4 112 PIDcontroller::PIDcontroller(float p, float i, float d, float dt)
vsluiter 1:926025d703f4 113 {
vsluiter 1:926025d703f4 114 _p = p;
vsluiter 1:926025d703f4 115 _i = i;
vsluiter 1:926025d703f4 116 _d = d;
vsluiter 1:926025d703f4 117 _dt = dt;
vsluiter 1:926025d703f4 118 _previous_error = 0;
vsluiter 1:926025d703f4 119 _integrator = 0;
vsluiter 1:926025d703f4 120 }
vsluiter 1:926025d703f4 121
vsluiter 1:926025d703f4 122 float PIDcontroller::control(float setpoint, float measurement)
vsluiter 1:926025d703f4 123 {
vsluiter 1:926025d703f4 124 float output = 0;
vsluiter 1:926025d703f4 125 float error = setpoint - measurement;
vsluiter 1:926025d703f4 126 _integrator += error*_dt;
vsluiter 1:926025d703f4 127 output = error * _p;
vsluiter 1:926025d703f4 128 output += _integrator * _i;
vsluiter 2:e7421003d1c9 129 output += ((_previous_error-error)/_dt)*_d;
vsluiter 1:926025d703f4 130 _previous_error = error;
vsluiter 1:926025d703f4 131 return output;
vsluiter 1:926025d703f4 132 }
vsluiter 1:926025d703f4 133
vsluiter 1:926025d703f4 134 float getPressure_Pa(void)
vsluiter 1:926025d703f4 135 {
vsluiter 1:926025d703f4 136 float voltage = pressure.read()*3.3;
vsluiter 1:926025d703f4 137 float sensor_output = (voltage*2)-0.2;//offset compensation
vsluiter 1:926025d703f4 138 float pressure = sensor_output*10000;
vsluiter 1:926025d703f4 139 return pressure;
vsluiter 1:926025d703f4 140 }
vsluiter 1:926025d703f4 141
vsluiter 4:e69e2983bb6e 142 void valvecontrol(void)
vsluiter 4:e69e2983bb6e 143 {
vsluiter 4:e69e2983bb6e 144 static uint16_t usbcounter = 0;
vsluiter 4:e69e2983bb6e 145 float meas = getPressure_Pa();
vsluiter 4:e69e2983bb6e 146 if(setpoint1 - meas >2000) {
vsluiter 4:e69e2983bb6e 147 v1close = 0;
vsluiter 4:e69e2983bb6e 148 v1open = 1;
vsluiter 4:e69e2983bb6e 149 } else if (meas - setpoint1 > 1000) {
vsluiter 4:e69e2983bb6e 150 v1close = 1;
vsluiter 4:e69e2983bb6e 151 v1open = 0;
vsluiter 4:e69e2983bb6e 152 }
vsluiter 4:e69e2983bb6e 153 else
vsluiter 4:e69e2983bb6e 154 v1open = v1close = 0;
vsluiter 4:e69e2983bb6e 155 usbcounter++;
vsluiter 4:e69e2983bb6e 156 if(usbcounter == 200) {
vsluiter 4:e69e2983bb6e 157 scope.set(0,setpoint1);
vsluiter 4:e69e2983bb6e 158 scope.set(1,meas);
vsluiter 4:e69e2983bb6e 159 scope.set(2,meas);
vsluiter 4:e69e2983bb6e 160 scope.send();
vsluiter 4:e69e2983bb6e 161 usbcounter = 0;
vsluiter 4:e69e2983bb6e 162 }
vsluiter 4:e69e2983bb6e 163 }
vsluiter 4:e69e2983bb6e 164
vsluiter 4:e69e2983bb6e 165 int main()
vsluiter 4:e69e2983bb6e 166 {
vsluiter 4:e69e2983bb6e 167 valvetick.attach(valvecontrol,0.0001);
vsluiter 4:e69e2983bb6e 168 while(1) {
vsluiter 4:e69e2983bb6e 169 setpoint1 = slider.readPercentage()*30000;
vsluiter 4:e69e2983bb6e 170 wait(0.1);
vsluiter 4:e69e2983bb6e 171 }
vsluiter 4:e69e2983bb6e 172 /*
vsluiter 2:e7421003d1c9 173 PIDcontroller pid(0.001,0,0.000,0.2);
vsluiter 2:e7421003d1c9 174 //Valve v1(V1_OPEN, V1_CLOSE);
vsluiter 0:80993ec26dfd 175 Valve v2(V2_OPEN, V2_CLOSE);
vsluiter 0:80993ec26dfd 176 Valve v3(V3_OPEN, V3_CLOSE);
vsluiter 0:80993ec26dfd 177 Valve v4(V4_OPEN, V4_CLOSE);
vsluiter 4:e69e2983bb6e 178
vsluiter 0:80993ec26dfd 179 while(1) {
vsluiter 1:926025d703f4 180 float pid_out;
vsluiter 2:e7421003d1c9 181 float setpoint;
vsluiter 3:28efa5d4ebe2 182 float meas;
vsluiter 0:80993ec26dfd 183 static float time = 0;
vsluiter 3:28efa5d4ebe2 184 wait(0.01);
vsluiter 1:926025d703f4 185 time +=0.2;
vsluiter 3:28efa5d4ebe2 186 setpoint = slider.readPercentage()*20000;
vsluiter 2:e7421003d1c9 187 pid_out = pid.control(setpoint, getPressure_Pa());
vsluiter 2:e7421003d1c9 188 //if(pid_out > 0.1)
vsluiter 2:e7421003d1c9 189 // v1.write(0.1);
vsluiter 2:e7421003d1c9 190 //if(pid_out < -0.1)
vsluiter 2:e7421003d1c9 191 // v1.write(-0.1);
vsluiter 3:28efa5d4ebe2 192 meas = getPressure_Pa();
vsluiter 3:28efa5d4ebe2 193 if(abs(setpoint - meas)>4000)
vsluiter 4:e69e2983bb6e 194 {
vsluiter 3:28efa5d4ebe2 195 int32_t count = 0;
vsluiter 3:28efa5d4ebe2 196 if(meas < setpoint )
vsluiter 2:e7421003d1c9 197 {
vsluiter 3:28efa5d4ebe2 198 while( (getPressure_Pa() < setpoint) && count < 100)
vsluiter 3:28efa5d4ebe2 199 {
vsluiter 3:28efa5d4ebe2 200 wait(0.001);
vsluiter 2:e7421003d1c9 201 v1open = 1;
vsluiter 3:28efa5d4ebe2 202 count++;
vsluiter 3:28efa5d4ebe2 203 }
vsluiter 2:e7421003d1c9 204 }
vsluiter 2:e7421003d1c9 205 else
vsluiter 2:e7421003d1c9 206 {
vsluiter 3:28efa5d4ebe2 207 while( (getPressure_Pa() > setpoint) && count < 100)
vsluiter 3:28efa5d4ebe2 208 {
vsluiter 3:28efa5d4ebe2 209 v1close = 1;
vsluiter 3:28efa5d4ebe2 210 wait(0.001);
vsluiter 3:28efa5d4ebe2 211 count++;
vsluiter 3:28efa5d4ebe2 212 }
vsluiter 2:e7421003d1c9 213 }
vsluiter 2:e7421003d1c9 214 v1open = v1close = 0;
vsluiter 4:e69e2983bb6e 215
vsluiter 2:e7421003d1c9 216 }
vsluiter 2:e7421003d1c9 217 //v1.write(pid_out,false);
vsluiter 1:926025d703f4 218 v2.write(0);//sin(time));
vsluiter 1:926025d703f4 219 v3.write(0);
vsluiter 1:926025d703f4 220 v4.write(0);//sin(time),false);
vsluiter 1:926025d703f4 221 scope.set(0,getPressure_Pa());
vsluiter 1:926025d703f4 222 scope.set(1,slider.readPercentage());
vsluiter 1:926025d703f4 223 scope.set(2,pid_out);
vsluiter 1:926025d703f4 224 scope.send();
vsluiter 4:e69e2983bb6e 225 }*/
vsluiter 0:80993ec26dfd 226 }