Diff: main.cpp

Revision:

1:c6d269c69853

Parent:

0:8c9ee304ed9f

Child:

2:1fd3a4aff5d3

--- a/main.cpp	Thu Mar 29 02:31:22 2018 +0000
+++ b/main.cpp	Thu Mar 29 02:58:47 2018 +0000
@@ -3,32 +3,25 @@
 
 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);
+#define SCALAR 0.6f
+#define TACH PTB1
+#define SPEED_CONTROL PTD5
+#define BRAKE PTA13
 
-//PI controller****************
-double Input, Output, Setpoint;
-double errSum;
-double ITerm;
-double kp, ki;
-double sampling_frequency;
-double ti;
-//saturation limits
-double outMin;
-double outMax;
-//******************************
+#define TI 0.001f
+#define MINm 0.0f
+#define MAXm 1.0f
+#define KPm 0.1414f
+#define KI 19.7408f
 
-///////////////////////////
+Ticker sample;
+AnalogIn speed(TACH);
+PwmOut gateDrive(SPEED_CONTROL);
+DigitalOut brake(BRAKE);
+
+float Setpoint;
+float errSum;
+
 Serial bt(PTE0, PTE1); //COM12
 
 void serCb()
@@ -47,69 +40,41 @@
         bt.putc(x);
     } 
 }
-///////////////////////////////
 
-void set_tunings(double KP, double KI)
-{
-    kp = KP;
-    ki = KI;
-}
-
-void update_states()
+void compute_PI()
 {
-    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;    
+    float error = Setpoint-speed.read();
+    errSum +=(error * TI);
+    float iTerm = KI*errSum;
+    if(iTerm > MAXm) iTerm = MAXm;
+    if(iTerm < MINm) iTerm = MINm; 
+    float output = KPm*error + iTerm;
+    if(output > MAXm) output = MAXm;
+    if(output < MINm) output = MINm;    
+    if(output < MINm) 
+    {
+        gateDrive.write(MINm);
+        brake.write(1);
+    }
+    else 
+    {
+        brake.write(0);
+        gateDrive.write(output);
+    }           
 }
 
 int main()
-{
-   outMin = 0.0;
-   outMax = 1.0;
+{ 
+    Setpoint = 0.0;
+    errSum = 0.0;
+    sample.attach(&compute_PI, TI);
    
-   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)
+     
+    while(true)
     {
-        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