Diff: main.cpp

Revision:

2:29daa03e4f62

Parent:

1:b862262a9d14

Child:

3:750afb8580dd

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Mon Oct 21 12:03:31 2019 +0000
@@ -1,23 +1,88 @@
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "BiQuad.h"
+#include <iostream>
+#include "FastPWM.h"
+#include "QEI.h"
+#include <math.h>
+//#include "HIDScope.h"
+
+
+Serial pc(USBTX, USBRX);
+volatile float width_percent;
+
+float k_p = 17.5;  
+float k_i = 1.02;
+float k_d = 23.2;
+float t_s = 0.0025; //sample time in sec
+
+
+//inputs for LPF; still to fill in!
+float b1;
+float b2;
+float b3;
+float b4;
+float b5;
+
+
 
-DigitalOut led(LED_RED);
+float getError(){
+    float a; //error
+    cin >> a;
+    return a;
+    }
+    
+    
+/*PID controller code
+Proportional part general formula: u_k = k_p * e 
+Integral part general formula: u_i = k_i * integral e dt
+Derivative part general formula: u_d = k_d * derivative e */
+
+
 
-MODSERIAL pc(USBTX, USBRX);
+/*float PIDControl(float error){
+    static float error_integral = 0;
+    static float error_prev = error;
+    static BiQuad LPF (b1, b2, b3, b4, b5); 
+    
+    //proportional
+    float u_k = k_p * error;
+    
+    //integral
+    error_integral = error_integral + error * t_s;
+    float u_i = k_i * error_integral;
+    
+    //differential
+    float error_derivative = (error - error_prev) / t_s;
+   // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
+   // float u_d = k_d * filtered_error;
+    float u_d = k_d * error_derivative; //this is very sensitive to noise, hence the LPF above
+    error_prev = error;
+    
+    return u_k + u_i + u_d;
+    }
+    
+
+*/
+
 
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
+    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
     
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
-    }
+    startmain:
+    pc.printf("Please input your error.\r\n");
+    float er = getError();
+    pc.printf("Your error is: %f\r\n",er);
+    pc.printf("-------\r\n-------\r\n");
+    wait(0.5);
+    goto startmain;
+
+    
+    
+    //float outcome = PIDControl(error);
+    //pc.printf("The outcome is %f\n",outcome);
+   // pc.printf("Your input is: %f\n", error);
+
 }