Diff: main.cpp

Revision:

0:8c9ee304ed9f

Child:

1:c6d269c69853

diff -r 000000000000 -r 8c9ee304ed9f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 29 02:31:22 2018 +0000
@@ -0,0 +1,115 @@
+#include "mbed.h"
+#include <iostream>
+
+using namespace std;
+
+//constants
+const float scalar = 0.6;
+
+DigitalOut myled(LED_GREEN);
+
+//ticker to compute output
+Ticker sample;
+
+//System IO
+//AnalogIn setPoint(PTB0);
+AnalogIn speed(PTB1);
+PwmOut gateDrive(PTD5);
+
+//PI controller****************
+double Input, Output, Setpoint;
+double errSum;
+double ITerm;
+double kp, ki;
+double sampling_frequency;
+double ti;
+//saturation limits
+double outMin;
+double outMax;
+//******************************
+
+///////////////////////////
+Serial bt(PTE0, PTE1); //COM12
+
+void serCb()
+{
+    char x = bt.getc();
+    if (x == 'a')
+    {
+        Setpoint = 0.25;
+    }
+    else if(x == 's')
+    {
+        Setpoint = 0.6;
+    }
+    else
+    {
+        bt.putc(x);
+    } 
+}
+///////////////////////////////
+
+void set_tunings(double KP, double KI)
+{
+    kp = KP;
+    ki = KI;
+}
+
+void update_states()
+{
+    Input = speed.read();
+}
+
+void compute()
+{
+    //proportional
+    double error = Setpoint-Input;
+    
+    //integral
+    errSum +=(error * ti);//potentially the problem
+    ITerm = ki*errSum;
+    if(ITerm > outMax) ITerm = outMax;
+    if(ITerm < outMin) ITerm = outMin; 
+    
+    //compute output
+    Output = kp*error + ITerm;
+    if(Output > outMax) Output = outMax;
+    if(Output < outMin) Output = outMin;    
+}
+
+int main()
+{
+   outMin = 0.0;
+   outMax = 1.0;
+   
+   sampling_frequency = 1e3;
+   ti = 1/sampling_frequency;   
+   set_tunings(0.1414,19.7408);//Kp, Ki   
+   Setpoint = 0.0;
+   sample.attach(&compute, ti);
+   
+    //////////////////////////////////////////////////
+    bt.baud(115200);
+    bt.printf("Press 'a' to go 25 and 's' to go 60%\r\n");
+    bt.attach(&serCb);
+    
+    int counter = 0;
+    //////////////////////////////////////////////// 
+      
+    
+   while(true)
+   {
+    update_states();
+    gateDrive = scalar*Output;
+    
+    ///////////////////////////////////////////////////
+    counter++;
+    if (counter >= 20000)
+    {
+        bt.printf("Wheel speed: %f\r\n", speed.read());
+        bt.printf("Controller output: %f\r\n", Output);
+        counter = 0;
+    }
+    ////////////////////////////////////////////////////
+   }  
+}
\ No newline at end of file