Diff: main.cpp

Revision:

2:e7ce63ada1d2

Parent:

1:712b9b5470d4

Child:

3:08488e544b3e

diff -r 712b9b5470d4 -r e7ce63ada1d2 main.cpp
--- a/main.cpp	Tue Jul 05 09:18:48 2016 +0000
+++ b/main.cpp	Tue Jul 05 11:57:09 2016 +0000
@@ -1,48 +1,77 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "FastAnalogIn.h"
+#include "PID.h"
+
+PID I(10.0, 0.01, 0.0, 0.01);
 
 AnalogOut Iset(PTE30);
 FastAnalogIn Iget(PTE20);
 AnalogIn Vbat(PTB0);
-int flop = 0;
-void adjust() {
-    //flop = (flop + 1) % 4;
-    Iset.write(flop == 0? 0.2/4.61 : 0);
-}
+bool active = true;
 float k = 0.01, iget = Iget.read(), vbat = Vbat.read(), vbat_rest = Vbat.read();
 void filter() {
-    iget = k * Iget.read() + (1 - k) * iget;
-    vbat = k * Vbat.read() + (1 - k) * vbat;
+    if (active) {
+        iget = k * Iget.read() * 32.9697 + 0.0001666 + (1 - k) * iget;
+        vbat = k * Vbat.read() * 36.7538 + (1 - k) * vbat;
+    }
     if (Iset.read() == 0)
-        vbat_rest = k * Vbat.read() + (1 - k) * vbat;
+        vbat_rest = k * Vbat.read() * 36.7538 + (1 - k) * vbat_rest;
 }
 void rest(const void *) {
     while (true) {
         Thread::wait(5000);
-        float x = Iset;
-        Iset.write_u16(16);
-        Thread::wait(400);
-        Iset.write_u16(0);
-        Thread::wait(100);
-        Iset = x;
+        if (Iset.read() > 0) {
+            active = false;
+            Iset.write_u16(16);
+            Thread::wait(400);
+            Iset.write_u16(0);
+            Thread::wait(100);
+            active = true;
+        }
+    }
+}
+int flop = 0;
+void adjust() {
+    //flop = (flop + 1) % 4;
+    //Iset.write(flop == 0? 0.2/4.61 : 0);
+    if (active) {
+        I.setProcessValue(iget);
+        Iset = I.compute();
     }
 }
+
+Serial pc(USBTX, USBRX);
+void ui(const void*) {
+    while (true) {
+        Thread::wait(100);
+        while (pc.readable()) {
+            pc.getc();
+            I = I + 0.1;
+        }
+    }
+}
+
 int main()
 {
-    Ticker t;
-    t.attach(&adjust, 3.0);
+    pc.baud(115200);
+
+    I.setInputLimits(0, 100);
+    I.setOutputLimits(0, 1);
+    I = 0;
+    I.setMode(AUTO_MODE);
+
     Ticker t2;
     t2.attach_us(&filter, 1000);
+    Ticker t;
+    t.attach(&adjust, I.getInterval());
     Thread t3(&rest);
-    Iset.write(0.0);
-    Serial pc(USBTX, USBRX);
-    pc.baud(115200);
+    Thread t4(&ui);
 
     while (true) {
         //pc.printf("Iset %f    Iget %6d %f    Vbat %6d %f    \r", Iset.read(), Iget.read_u16(), Iget.read(), Vbat.read_u16(), Vbat.read());
-        pc.printf("Iset %f      Iget %8d %f %f", Iset.read(), Iget.read_u16(), Iget.read() * 36.140224, iget * 36.140224);
-        pc.printf("      Vbat %8d %f %f %f\r", Vbat.read_u16(), Vbat.read() * 36.7538, vbat * 36.7538, vbat_rest * 36.7538);
+        pc.printf("Iset %f      Iget %8d %f %f", Iset.read(), Iget.read_u16(), Iget.read() * 32.9697 + 0.0001666, iget);
+        pc.printf("      Vbat %8d %f %f %f\r", Vbat.read_u16(), Vbat.read() * 36.7538, vbat, vbat_rest);
         Thread::wait(500);
     }
 }