Francisco Guerra / Mbed 2 deprecated SymbitronValves

Amplitude Modulation Code

Dependencies:   HIDScope mbed

Fork of SymbitronValves by First Last

main.cpp@1:926025d703f4, 2015-06-29 (annotated)

Committer:
vsluiter
Date:
Mon Jun 29 14:00:34 2015 +0000
Revision:
1:926025d703f4
Parent:
0:80993ec26dfd
Child:
2:e7421003d1c9
Added PID, tsi, hidscope. Not working yet

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 1:926025d703f4 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 1:926025d703f4 20 HIDScope scope(3);
vsluiter 1:926025d703f4 21 AnalogIn pressure(PRESSURE);
vsluiter 1:926025d703f4 22 TSIAnalogSlider slider(TSI_A, TSI_B, 100);
vsluiter 1:926025d703f4 23
vsluiter 0:80993ec26dfd 24 class Valve
vsluiter 0:80993ec26dfd 25 {
vsluiter 0:80993ec26dfd 26 public:
vsluiter 0:80993ec26dfd 27 Valve(PinName open, PinName close);
vsluiter 1:926025d703f4 28 void write(float pwm, bool direct_mode=true);
vsluiter 1:926025d703f4 29 void SetDeadtime(float deadtime){if(_period < deadtime) _period = deadtime; _deadtime = deadtime; InitializeValve(_period,_deadtime);};
vsluiter 0:80993ec26dfd 30 private:
vsluiter 0:80993ec26dfd 31 float _pwm;
vsluiter 0:80993ec26dfd 32 float _period;
vsluiter 1:926025d703f4 33 float _deadtime;
vsluiter 1:926025d703f4 34 void InitializeValve(float period, float deadtime);
vsluiter 0:80993ec26dfd 35 FastPWM _open;
vsluiter 0:80993ec26dfd 36 FastPWM _close;
vsluiter 0:80993ec26dfd 37 };
vsluiter 0:80993ec26dfd 38
vsluiter 0:80993ec26dfd 39 Valve::Valve(PinName open, PinName close): _open(open), _close(close)
vsluiter 0:80993ec26dfd 40 {
vsluiter 1:926025d703f4 41 InitializeValve(0.15,0.006);
vsluiter 1:926025d703f4 42 }
vsluiter 1:926025d703f4 43
vsluiter 1:926025d703f4 44 void Valve::InitializeValve(float period, float deadtime)
vsluiter 1:926025d703f4 45 {
vsluiter 1:926025d703f4 46 _period = period;
vsluiter 1:926025d703f4 47 _deadtime = deadtime;
vsluiter 0:80993ec26dfd 48 _open.period(_period);
vsluiter 0:80993ec26dfd 49 _close.period(_period);
vsluiter 0:80993ec26dfd 50 _open.pulsewidth(0);
vsluiter 0:80993ec26dfd 51 _close.pulsewidth(0);
vsluiter 0:80993ec26dfd 52 }
vsluiter 0:80993ec26dfd 53
vsluiter 1:926025d703f4 54
vsluiter 1:926025d703f4 55 void Valve::write(float pwm, bool direct_mode)
vsluiter 0:80993ec26dfd 56 {
vsluiter 1:926025d703f4 57 //input conditioning
vsluiter 0:80993ec26dfd 58 if(pwm > 1)
vsluiter 0:80993ec26dfd 59 pwm = 1;
vsluiter 0:80993ec26dfd 60 if(pwm < -1)
vsluiter 0:80993ec26dfd 61 pwm = -1;
vsluiter 1:926025d703f4 62
vsluiter 1:926025d703f4 63 if(direct_mode)
vsluiter 0:80993ec26dfd 64 {
vsluiter 1:926025d703f4 65 if(pwm > 1)
vsluiter 1:926025d703f4 66 pwm = 1;
vsluiter 1:926025d703f4 67 if(pwm < -1)
vsluiter 1:926025d703f4 68 pwm = -1;
vsluiter 1:926025d703f4 69 if(pwm > 0)
vsluiter 1:926025d703f4 70 {
vsluiter 1:926025d703f4 71 _close.pulsewidth(0);
vsluiter 1:926025d703f4 72 _open.pulsewidth(_period*pwm);
vsluiter 1:926025d703f4 73 }
vsluiter 1:926025d703f4 74 else
vsluiter 1:926025d703f4 75 {
vsluiter 1:926025d703f4 76 _open.pulsewidth(0);
vsluiter 1:926025d703f4 77 _close.pulsewidth(_period*pwm*-1);
vsluiter 1:926025d703f4 78 }
vsluiter 0:80993ec26dfd 79 }
vsluiter 0:80993ec26dfd 80 else
vsluiter 0:80993ec26dfd 81 {
vsluiter 1:926025d703f4 82 if(pwm > 0.001)
vsluiter 1:926025d703f4 83 {
vsluiter 1:926025d703f4 84 _close.pulsewidth(0);
vsluiter 1:926025d703f4 85 _open.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm) );
vsluiter 1:926025d703f4 86 }
vsluiter 1:926025d703f4 87 else if(pwm < 0.001)
vsluiter 1:926025d703f4 88 {
vsluiter 1:926025d703f4 89 _open.pulsewidth(0);
vsluiter 1:926025d703f4 90 _close.pulsewidth(_deadtime+ ((_period-_deadtime)*pwm*-1));
vsluiter 1:926025d703f4 91 }
vsluiter 1:926025d703f4 92 else
vsluiter 1:926025d703f4 93 {
vsluiter 1:926025d703f4 94 _open.pulsewidth(0);
vsluiter 1:926025d703f4 95 _close.pulsewidth(0);
vsluiter 1:926025d703f4 96 }
vsluiter 0:80993ec26dfd 97 }
vsluiter 0:80993ec26dfd 98 }
vsluiter 0:80993ec26dfd 99
vsluiter 1:926025d703f4 100 class PIDcontroller
vsluiter 1:926025d703f4 101 {
vsluiter 1:926025d703f4 102 public:
vsluiter 1:926025d703f4 103 PIDcontroller(float p, float i, float d, float dt);
vsluiter 1:926025d703f4 104 float control(float setpoint, float measurement);
vsluiter 1:926025d703f4 105 private:
vsluiter 1:926025d703f4 106 float _p;
vsluiter 1:926025d703f4 107 float _i;
vsluiter 1:926025d703f4 108 float _d;
vsluiter 1:926025d703f4 109 float _dt;
vsluiter 1:926025d703f4 110 float _integrator;
vsluiter 1:926025d703f4 111 float _previous_error;
vsluiter 1:926025d703f4 112 };
vsluiter 1:926025d703f4 113
vsluiter 1:926025d703f4 114 PIDcontroller::PIDcontroller(float p, float i, float d, float dt)
vsluiter 1:926025d703f4 115 {
vsluiter 1:926025d703f4 116 _p = p;
vsluiter 1:926025d703f4 117 _i = i;
vsluiter 1:926025d703f4 118 _d = d;
vsluiter 1:926025d703f4 119 _dt = dt;
vsluiter 1:926025d703f4 120 _previous_error = 0;
vsluiter 1:926025d703f4 121 _integrator = 0;
vsluiter 1:926025d703f4 122 }
vsluiter 1:926025d703f4 123
vsluiter 1:926025d703f4 124 float PIDcontroller::control(float setpoint, float measurement)
vsluiter 1:926025d703f4 125 {
vsluiter 1:926025d703f4 126 float output = 0;
vsluiter 1:926025d703f4 127 float error = setpoint - measurement;
vsluiter 1:926025d703f4 128 _integrator += error*_dt;
vsluiter 1:926025d703f4 129 output = error * _p;
vsluiter 1:926025d703f4 130 output += _integrator * _i;
vsluiter 1:926025d703f4 131 output += ((error-_previous_error)/_dt)*_d;
vsluiter 1:926025d703f4 132 _previous_error = error;
vsluiter 1:926025d703f4 133 return output;
vsluiter 1:926025d703f4 134 }
vsluiter 1:926025d703f4 135
vsluiter 1:926025d703f4 136 float getPressure_Pa(void)
vsluiter 1:926025d703f4 137 {
vsluiter 1:926025d703f4 138 float voltage = pressure.read()*3.3;
vsluiter 1:926025d703f4 139 float sensor_output = (voltage*2)-0.2;//offset compensation
vsluiter 1:926025d703f4 140 float pressure = sensor_output*10000;
vsluiter 1:926025d703f4 141 return pressure;
vsluiter 1:926025d703f4 142 }
vsluiter 1:926025d703f4 143
vsluiter 0:80993ec26dfd 144 int main() {
vsluiter 1:926025d703f4 145 PIDcontroller pid(0.01,0,0,0.2);
vsluiter 0:80993ec26dfd 146 Valve v1(V1_OPEN, V1_CLOSE);
vsluiter 0:80993ec26dfd 147 Valve v2(V2_OPEN, V2_CLOSE);
vsluiter 0:80993ec26dfd 148 Valve v3(V3_OPEN, V3_CLOSE);
vsluiter 0:80993ec26dfd 149 Valve v4(V4_OPEN, V4_CLOSE);
vsluiter 0:80993ec26dfd 150 while(1) {
vsluiter 1:926025d703f4 151 float pid_out;
vsluiter 0:80993ec26dfd 152 static float time = 0;
vsluiter 1:926025d703f4 153 wait(0.2);
vsluiter 1:926025d703f4 154 time +=0.2;
vsluiter 1:926025d703f4 155 pid_out = pid.control(slider.readPercentage()*10000, getPressure_Pa());
vsluiter 1:926025d703f4 156 v1.write(pid_out,false)
vsluiter 1:926025d703f4 157 v2.write(0);//sin(time));
vsluiter 1:926025d703f4 158 v3.write(0);
vsluiter 1:926025d703f4 159 v4.write(0);//sin(time),false);
vsluiter 1:926025d703f4 160 scope.set(0,getPressure_Pa());
vsluiter 1:926025d703f4 161 scope.set(1,slider.readPercentage());
vsluiter 1:926025d703f4 162 scope.set(2,pid_out);
vsluiter 1:926025d703f4 163 scope.send();
vsluiter 0:80993ec26dfd 164 }
vsluiter 0:80993ec26dfd 165 }