Diff: main.cpp

Revision:

1:926025d703f4

Parent:

0:80993ec26dfd

Child:

2:e7421003d1c9

--- a/main.cpp	Fri Jun 26 15:12:05 2015 +0000
+++ b/main.cpp	Mon Jun 29 14:00:34 2015 +0000
@@ -1,71 +1,165 @@
 #include "mbed.h"
 #include "FastPWM.h"
-
+#include "HIDScope.h"
+#include "tsi_sensor.h"
 
-#define V1_OPEN  D2
-#define V1_CLOSE D3
-#define V2_OPEN  D4
-#define V2_CLOSE D5
-#define V3_OPEN  D6
-#define V3_CLOSE D7
-#define V4_OPEN  D8
-#define V4_CLOSE D9
+#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);
+    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)
 {
-    _period = 0.05;
+    InitializeValve(0.15,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)
+
+void Valve::write(float pwm, bool direct_mode)
 {
+    //input conditioning
     if(pwm > 1)
         pwm = 1;
     if(pwm < -1)
         pwm = -1;
-    if(pwm > 0)
+        
+    if(direct_mode)
     {
-        _close.pulsewidth(0);
-        _open.pulsewidth(_period*pwm);
+        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
     {
-        _open.pulsewidth(0);
-        _close.pulsewidth(_period*pwm*-1);
+        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 += ((error-_previous_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.01,0,0,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);
-    //PwmOut v1(V1_OPEN);
-    //v1.period(0.2);
     while(1) {
+        float pid_out;
         static float time = 0;
-        wait(0.02);
-        time +=0.05;
-        v1.write(sin(time));
-        v2.write(sin(time));
-        v3.write(sin(time));
-        v4.write(sin(time));
+        wait(0.2);
+        time +=0.2;
+        pid_out = pid.control(slider.readPercentage()*10000, getPressure_Pa());
+        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();
     }
 }