Diff: main.cpp

Revision:

0:bd171e51f222

Child:

1:01aa18e7c4e5

diff -r 000000000000 -r bd171e51f222 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 08 23:08:25 2021 +0000
@@ -0,0 +1,129 @@
+#include "mbed.h"
+#include <string>
+#include <sstream>
+
+using namespace std;
+
+Serial pc(USBTX,USBRX);
+AnalogIn wheelAngleControl(PTB0);
+AnalogIn leftWheelDSense(PTB1);
+AnalogIn rightWheelDSense(PTB2);
+PwmOut pwmWheelAngle(PTE20);
+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
+
+//All inital values and variables declared
+float max=0.1f;
+float min=0.05f;
+float mid;
+float range;
+float currVal=0.0f;
+float leftDist=0.5f;
+float rightDist=0.5f;
+float leftDat=0.0f;
+float rightDat=0.0f;
+float average;
+        
+float errorSignal;
+float errorLast;
+        
+int state=0;  // 0=off    1=on    2=wait;
+
+Ticker sampler;
+
+void sample()
+{
+    leftDist=leftWheelDSense.read();
+    rightDist=rightWheelDSense.read();
+    leftDat=(12.185*leftDist)-3.83;
+    rightDat=(-15.77*rightDist)+4.75;
+    average=(leftDat+rightDat)/2;
+    errorLast=errorSignal;
+    errorSignal=average;
+    return;
+}
+
+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;
+        float min=0.05f;
+        float mid=(max+min)/2;
+        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;
+        
+        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;
+        
+        int state=0;  // 0=off    1=on    2=wait;
+        
+        //LED cycles on and off to show proper response
+        BlueLED=0.9f;
+        wait(0.9);
+        BlueLED=1.0f;
+        wait(0.9);
+        BlueLED=0.9f;
+        wait(0.9);
+        BlueLED=1.0f;
+        
+        
+        state=1;
+        
+        
+    while(1) {   //main loop that cycles forever
+        
+    //State check
+        if(state==0){
+            return 0;
+        }
+        else if(state==2){
+            BlueLED=0.80f;
+        } else {
+            BlueLED=1.0f;
+        }
+        
+    //PD Tranfer Function
+        errorChange=(errorSignal-errorLast)/TI; // NO REFERNCE DERIVATIVE, NOT SURE WHAT TO PUT HERE-0.036495
+        errorCurr=errorSignal-reference;
+        controllerOutput=KP*errorCurr+KD*errorChange;
+        
+    //Output Value Derivation
+        currVal=(controllerOutput*-1)+M_PI;
+        duty=min+(currVal*range);
+        if(duty<min){
+            duty=min;
+        }
+        if(duty>max){
+            duty=max;
+        }
+        
+        
+    //Changes PWM signal to appropriate value (steering control)
+        if(state==1){
+            pwmWheelAngle.write(duty); //uses potentiometer value
+        }
+        
+        wait(0.0001);
+    }
+}