Diff: main.cpp

Revision:

0:8609b4a06a8f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 08 10:44:03 2021 +0000
@@ -0,0 +1,162 @@
+
+
+#include "mbed.h"
+#include <stdlib.h>
+#include <numeric>
+#include <vector>
+#include <iostream>
+#include <fstream> 
+
+PwmOut speed(p23);
+PwmOut steering(p24);
+
+//DigitalOut enable(p20);
+AnalogIn sens_r(p15);
+AnalogIn sens_l(p16);
+AnalogIn feedback(p17);
+
+//////////////]tuing
+#include "C12832.h"
+C12832 lcd(p5, p7, p6, p8, p11);
+float mperiod = 1.0 / 20000;
+float error_sensors = 0;
+float error_steering = 0;
+std::vector<float> total_error;
+float u_drive = 0.5, u_steer = 0.5;
+float integral = 0, derivative = 0;
+float error_control = 0;
+float last_error_s;
+float position;
+float max_turn = 0.635;
+float min_turn = 0.23;
+float mid_point = 0.503;
+float offset = 0.5;
+float p[3] = {1, 0, 0};
+float dp[3] ={1, 1, 1};
+float kp = 1;
+float kd_s = 0.05, ki_s =  -2.19;
+float kp_s = 6.05;
+float temp = 0;
+
+void calcIntegral();
+void pidController();
+float getErr(int t);
+void twiddle();
+////////////
+
+
+int main()
+{
+    speed.period(mperiod);
+    steering.period(mperiod);
+    while(1){
+        pidController();
+    }
+    twiddle();
+    lcd.cls();
+    lcd.locate(1,2);
+    lcd.printf("kpf = %.2f, ki_s = %.2f, kd_s = %.2f", kp_s, ki_s, kd_s);
+    return 0;
+    
+}
+
+void pidController()
+{
+    error_sensors = sens_r.read() - sens_l.read();
+    position = mid_point + error_sensors;
+    if(position > max_turn) position = max_turn;
+    if(position < min_turn) position = min_turn;
+    error_steering = position - feedback.read();
+    derivative = (error_steering - last_error_s)/mperiod; //derivative
+    last_error_s = error_steering;
+    u_drive = 0.8 - kp*abs(error_sensors);
+    calcIntegral();
+    u_steer = offset - kp_s*error_steering - ki_s*integral - kd_s*derivative;
+    
+    if (u_drive > 0.65) u_drive = 0.65; //to avoid damaging motors7
+    else if(u_drive < 0.6) u_drive = 0.6;
+    if (u_steer >= 0.7) u_steer = 0.7; //avoid locking motors
+    if (u_steer <= 0.3) u_steer = 0.3; //avoid locking motors
+    speed = u_drive;
+    steering = u_steer;
+}
+
+float checkLimits(float val, int type){
+    if(type){
+        if(val > 0.8) return 0.8;
+        else if(val < 0.2) return 0.2;
+        return val;
+    }
+    if(val > max_turn) return max_turn;
+    else if(val < min_turn) return min_turn;
+    return val;
+}
+
+void calcIntegral(){
+    //for integral part
+    if (error_control < 10)
+    {
+        total_error.push_back(error_steering);
+        error_control++;
+    }
+    else
+    {
+        total_error.erase(total_error.begin()); //removes vec first element
+        total_error.push_back(error_steering);  //add current err as last to vec
+    }
+    integral = 0; //reset accumulated error
+    for (int i = 0; i < total_error.size(); i++) integral += total_error[i] * mperiod;  //sum of errors in vector
+}
+
+void twiddle(){  
+    float err = 0;
+    float best_err  = getErr(0); //calc steering er
+    float total_dp;
+    
+    for (int i = 0; i < 3; i++) total_dp += dp[i];
+    while(total_dp > 0.005){
+        for(int i =0; i < 3; i++){
+            p[i]+=dp[i];
+            err = getErr(0);
+
+            if (err < best_err){
+                best_err = err;
+                dp[i] *= 1.1;
+            }
+            else{
+                p[i] -= 2*dp[i];
+                err = getErr(0);
+
+                if(err < best_err){
+                    best_err = err;
+                    dp[i] *= 1.05;
+                }
+                else{
+                    p[i] += dp[i];
+                    dp[i] *= 0.95;
+                }
+            }
+        }
+    }
+    std::ofstream outfile ("test.txt");
+    outfile << "kp = " << p[0] << "\t ki = " << p[1] << "\t kd = " << p[2] << std::endl;
+}
+
+float getErr(int t){
+    //////////apply new vals
+    kp_s = p[0];
+    ki_s = p[1];
+    kd_s = p[2];
+    pidController();
+    error_sensors = sens_r.read() - sens_l.read();
+    position = mid_point + error_sensors;
+    if(position > max_turn) position = max_turn;
+    if(position < min_turn) position = min_turn;
+    error_steering = position - feedback.read();
+
+    lcd.cls();
+    lcd.locate(1,2);
+    lcd.printf("kp:%.2f, ki:%.2f, kd:%.2f \nerr:%.2f", kp_s, ki_s, kd_s, error_steering);
+    return error_steering;
+}
+