Diff: main.cpp

Revision:

1:01aa18e7c4e5

Parent:

0:bd171e51f222

--- a/main.cpp	Mon Mar 08 23:08:25 2021 +0000
+++ b/main.cpp	Mon Mar 15 22:12:10 2021 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include <string>
-#include <sstream>
+#include <vector>
+#include <iostream>
 
 using namespace std;
 
@@ -12,9 +13,10 @@
 PwmOut BlueLED(LED3);
 
 //Control System Variables
-#define TI 0.001 //1kHz Sample Time
-#define KP 0.2078f //Proportional Gain
-#define KD 1.6231f //Integral Gain
+#define TI 0.02 //50Hz Sample Time
+#define KP 2.6f //Proportional Gain    0.2
+#define KD 0.135f //Derivative Gain    //1.6
+# define M_PI           3.14159265358979323846
 
 //All inital values and variables declared
 float max=0.1f;
@@ -27,7 +29,9 @@
 float leftDat=0.0f;
 float rightDat=0.0f;
 float average;
-        
+
+std::vector<float> V;
+
 float errorSignal;
 float errorLast;
         
@@ -35,25 +39,23 @@
 
 Ticker sampler;
 
-void sample()
+void sample(void)
 {
     leftDist=leftWheelDSense.read();
     rightDist=rightWheelDSense.read();
-    leftDat=(12.185*leftDist)-3.83;
-    rightDat=(-15.77*rightDist)+4.75;
-    average=(leftDat+rightDat)/2;
+    average=(leftDist-rightDist);
     errorLast=errorSignal;
-    errorSignal=average;
-    return;
+    errorSignal=(13.421*average+3.1051)*(1.0f/100.0f);
+    V[1]=V[0];  //Store the last value
+    V[0]=errorSignal;
+    
+          //  pc.printf("Made it erhe %f\r\n\r\n",leftDist);
 }
 
 int main() {
         
         //Control System Variables
-        # define TI 0.001 //1kHz Sample Time
-        # define KP 0.7299f //Proportional Gain
-        # define KD 5.1302f //Integral Gain
-        # define M_PI           3.14159265358979323846
+        
         
         //All inital values and variables declared
         float max=0.1f;
@@ -62,19 +64,16 @@
         float range=max-min;
         pwmWheelAngle.period(0.02f);
         float currVal=0.0f;
-        float leftDist=0.5f;
-        float rightDist=0.5f;
-        float leftDat=0.0f;
-        float rightDat=0.0f;
         float duty=mid;
         float reference=0;
         
+        V = std::vector<float>(2,0);
+        
         float errorSignal = 0.0f;
         float errorCurr = 0.0f;
         float errorLast = 0.0f;
         float errorChange = 0.0f;
-        sampler.attach(&sample,TI);
-        float controllerOutput=3.14/2;
+        float controllerOutput=M_PI/2;
         
         int state=0;  // 0=off    1=on    2=wait;
         
@@ -90,8 +89,12 @@
         
         state=1;
         
-        
+        sampler.attach(sample,TI);
     while(1) {   //main loop that cycles forever
+    
+    //Import Values
+    errorSignal=V[0];
+    errorLast=V[1];
         
     //State check
         if(state==0){
@@ -109,8 +112,8 @@
         controllerOutput=KP*errorCurr+KD*errorChange;
         
     //Output Value Derivation
-        currVal=(controllerOutput*-1)+M_PI;
-        duty=min+(currVal*range);
+        currVal=(controllerOutput*-1)+(M_PI/2);
+        duty=min+((currVal/M_PI)*range);
         if(duty<min){
             duty=min;
         }
@@ -122,8 +125,11 @@
     //Changes PWM signal to appropriate value (steering control)
         if(state==1){
             pwmWheelAngle.write(duty); //uses potentiometer value
+            pc.printf("Duty at %f\r\n",duty);
+            pc.printf("Curr Val at %f\r\n",currVal);
+            pc.printf("Average is %f\r\n\r\n",V[0]*(100));
         }
         
-        wait(0.0001);
+        wait(0.001);
     }
 }