Jose TRINDADE / Mbed 2 deprecated finall_ctrlr

A PID controller to control a buggie car that follows a line based on voltage variation detected by sensors attached to the car

Dependencies:   mbed C12832

main.cpp@0:8609b4a06a8f, 2021-03-08 (annotated)

Committer:
JTrindade
Date:
Mon Mar 08 10:44:03 2021 +0000
Revision:
0:8609b4a06a8f
A PID controller to control a buggie car that follows a line based on voltage variation detected by sensors attached to the car

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JTrindade 0:8609b4a06a8f 1
JTrindade 0:8609b4a06a8f 2
JTrindade 0:8609b4a06a8f 3 #include "mbed.h"
JTrindade 0:8609b4a06a8f 4 #include <stdlib.h>
JTrindade 0:8609b4a06a8f 5 #include <numeric>
JTrindade 0:8609b4a06a8f 6 #include <vector>
JTrindade 0:8609b4a06a8f 7 #include <iostream>
JTrindade 0:8609b4a06a8f 8 #include <fstream>
JTrindade 0:8609b4a06a8f 9
JTrindade 0:8609b4a06a8f 10 PwmOut speed(p23);
JTrindade 0:8609b4a06a8f 11 PwmOut steering(p24);
JTrindade 0:8609b4a06a8f 12
JTrindade 0:8609b4a06a8f 13 //DigitalOut enable(p20);
JTrindade 0:8609b4a06a8f 14 AnalogIn sens_r(p15);
JTrindade 0:8609b4a06a8f 15 AnalogIn sens_l(p16);
JTrindade 0:8609b4a06a8f 16 AnalogIn feedback(p17);
JTrindade 0:8609b4a06a8f 17
JTrindade 0:8609b4a06a8f 18 //////////////]tuing
JTrindade 0:8609b4a06a8f 19 #include "C12832.h"
JTrindade 0:8609b4a06a8f 20 C12832 lcd(p5, p7, p6, p8, p11);
JTrindade 0:8609b4a06a8f 21 float mperiod = 1.0 / 20000;
JTrindade 0:8609b4a06a8f 22 float error_sensors = 0;
JTrindade 0:8609b4a06a8f 23 float error_steering = 0;
JTrindade 0:8609b4a06a8f 24 std::vector<float> total_error;
JTrindade 0:8609b4a06a8f 25 float u_drive = 0.5, u_steer = 0.5;
JTrindade 0:8609b4a06a8f 26 float integral = 0, derivative = 0;
JTrindade 0:8609b4a06a8f 27 float error_control = 0;
JTrindade 0:8609b4a06a8f 28 float last_error_s;
JTrindade 0:8609b4a06a8f 29 float position;
JTrindade 0:8609b4a06a8f 30 float max_turn = 0.635;
JTrindade 0:8609b4a06a8f 31 float min_turn = 0.23;
JTrindade 0:8609b4a06a8f 32 float mid_point = 0.503;
JTrindade 0:8609b4a06a8f 33 float offset = 0.5;
JTrindade 0:8609b4a06a8f 34 float p[3] = {1, 0, 0};
JTrindade 0:8609b4a06a8f 35 float dp[3] ={1, 1, 1};
JTrindade 0:8609b4a06a8f 36 float kp = 1;
JTrindade 0:8609b4a06a8f 37 float kd_s = 0.05, ki_s = -2.19;
JTrindade 0:8609b4a06a8f 38 float kp_s = 6.05;
JTrindade 0:8609b4a06a8f 39 float temp = 0;
JTrindade 0:8609b4a06a8f 40
JTrindade 0:8609b4a06a8f 41 void calcIntegral();
JTrindade 0:8609b4a06a8f 42 void pidController();
JTrindade 0:8609b4a06a8f 43 float getErr(int t);
JTrindade 0:8609b4a06a8f 44 void twiddle();
JTrindade 0:8609b4a06a8f 45 ////////////
JTrindade 0:8609b4a06a8f 46
JTrindade 0:8609b4a06a8f 47
JTrindade 0:8609b4a06a8f 48 int main()
JTrindade 0:8609b4a06a8f 49 {
JTrindade 0:8609b4a06a8f 50 speed.period(mperiod);
JTrindade 0:8609b4a06a8f 51 steering.period(mperiod);
JTrindade 0:8609b4a06a8f 52 while(1){
JTrindade 0:8609b4a06a8f 53 pidController();
JTrindade 0:8609b4a06a8f 54 }
JTrindade 0:8609b4a06a8f 55 twiddle();
JTrindade 0:8609b4a06a8f 56 lcd.cls();
JTrindade 0:8609b4a06a8f 57 lcd.locate(1,2);
JTrindade 0:8609b4a06a8f 58 lcd.printf("kpf = %.2f, ki_s = %.2f, kd_s = %.2f", kp_s, ki_s, kd_s);
JTrindade 0:8609b4a06a8f 59 return 0;
JTrindade 0:8609b4a06a8f 60
JTrindade 0:8609b4a06a8f 61 }
JTrindade 0:8609b4a06a8f 62
JTrindade 0:8609b4a06a8f 63 void pidController()
JTrindade 0:8609b4a06a8f 64 {
JTrindade 0:8609b4a06a8f 65 error_sensors = sens_r.read() - sens_l.read();
JTrindade 0:8609b4a06a8f 66 position = mid_point + error_sensors;
JTrindade 0:8609b4a06a8f 67 if(position > max_turn) position = max_turn;
JTrindade 0:8609b4a06a8f 68 if(position < min_turn) position = min_turn;
JTrindade 0:8609b4a06a8f 69 error_steering = position - feedback.read();
JTrindade 0:8609b4a06a8f 70 derivative = (error_steering - last_error_s)/mperiod; //derivative
JTrindade 0:8609b4a06a8f 71 last_error_s = error_steering;
JTrindade 0:8609b4a06a8f 72 u_drive = 0.8 - kp*abs(error_sensors);
JTrindade 0:8609b4a06a8f 73 calcIntegral();
JTrindade 0:8609b4a06a8f 74 u_steer = offset - kp_s*error_steering - ki_s*integral - kd_s*derivative;
JTrindade 0:8609b4a06a8f 75
JTrindade 0:8609b4a06a8f 76 if (u_drive > 0.65) u_drive = 0.65; //to avoid damaging motors7
JTrindade 0:8609b4a06a8f 77 else if(u_drive < 0.6) u_drive = 0.6;
JTrindade 0:8609b4a06a8f 78 if (u_steer >= 0.7) u_steer = 0.7; //avoid locking motors
JTrindade 0:8609b4a06a8f 79 if (u_steer <= 0.3) u_steer = 0.3; //avoid locking motors
JTrindade 0:8609b4a06a8f 80 speed = u_drive;
JTrindade 0:8609b4a06a8f 81 steering = u_steer;
JTrindade 0:8609b4a06a8f 82 }
JTrindade 0:8609b4a06a8f 83
JTrindade 0:8609b4a06a8f 84 float checkLimits(float val, int type){
JTrindade 0:8609b4a06a8f 85 if(type){
JTrindade 0:8609b4a06a8f 86 if(val > 0.8) return 0.8;
JTrindade 0:8609b4a06a8f 87 else if(val < 0.2) return 0.2;
JTrindade 0:8609b4a06a8f 88 return val;
JTrindade 0:8609b4a06a8f 89 }
JTrindade 0:8609b4a06a8f 90 if(val > max_turn) return max_turn;
JTrindade 0:8609b4a06a8f 91 else if(val < min_turn) return min_turn;
JTrindade 0:8609b4a06a8f 92 return val;
JTrindade 0:8609b4a06a8f 93 }
JTrindade 0:8609b4a06a8f 94
JTrindade 0:8609b4a06a8f 95 void calcIntegral(){
JTrindade 0:8609b4a06a8f 96 //for integral part
JTrindade 0:8609b4a06a8f 97 if (error_control < 10)
JTrindade 0:8609b4a06a8f 98 {
JTrindade 0:8609b4a06a8f 99 total_error.push_back(error_steering);
JTrindade 0:8609b4a06a8f 100 error_control++;
JTrindade 0:8609b4a06a8f 101 }
JTrindade 0:8609b4a06a8f 102 else
JTrindade 0:8609b4a06a8f 103 {
JTrindade 0:8609b4a06a8f 104 total_error.erase(total_error.begin()); //removes vec first element
JTrindade 0:8609b4a06a8f 105 total_error.push_back(error_steering); //add current err as last to vec
JTrindade 0:8609b4a06a8f 106 }
JTrindade 0:8609b4a06a8f 107 integral = 0; //reset accumulated error
JTrindade 0:8609b4a06a8f 108 for (int i = 0; i < total_error.size(); i++) integral += total_error[i] * mperiod; //sum of errors in vector
JTrindade 0:8609b4a06a8f 109 }
JTrindade 0:8609b4a06a8f 110
JTrindade 0:8609b4a06a8f 111 void twiddle(){
JTrindade 0:8609b4a06a8f 112 float err = 0;
JTrindade 0:8609b4a06a8f 113 float best_err = getErr(0); //calc steering er
JTrindade 0:8609b4a06a8f 114 float total_dp;
JTrindade 0:8609b4a06a8f 115
JTrindade 0:8609b4a06a8f 116 for (int i = 0; i < 3; i++) total_dp += dp[i];
JTrindade 0:8609b4a06a8f 117 while(total_dp > 0.005){
JTrindade 0:8609b4a06a8f 118 for(int i =0; i < 3; i++){
JTrindade 0:8609b4a06a8f 119 p[i]+=dp[i];
JTrindade 0:8609b4a06a8f 120 err = getErr(0);
JTrindade 0:8609b4a06a8f 121
JTrindade 0:8609b4a06a8f 122 if (err < best_err){
JTrindade 0:8609b4a06a8f 123 best_err = err;
JTrindade 0:8609b4a06a8f 124 dp[i] *= 1.1;
JTrindade 0:8609b4a06a8f 125 }
JTrindade 0:8609b4a06a8f 126 else{
JTrindade 0:8609b4a06a8f 127 p[i] -= 2*dp[i];
JTrindade 0:8609b4a06a8f 128 err = getErr(0);
JTrindade 0:8609b4a06a8f 129
JTrindade 0:8609b4a06a8f 130 if(err < best_err){
JTrindade 0:8609b4a06a8f 131 best_err = err;
JTrindade 0:8609b4a06a8f 132 dp[i] *= 1.05;
JTrindade 0:8609b4a06a8f 133 }
JTrindade 0:8609b4a06a8f 134 else{
JTrindade 0:8609b4a06a8f 135 p[i] += dp[i];
JTrindade 0:8609b4a06a8f 136 dp[i] *= 0.95;
JTrindade 0:8609b4a06a8f 137 }
JTrindade 0:8609b4a06a8f 138 }
JTrindade 0:8609b4a06a8f 139 }
JTrindade 0:8609b4a06a8f 140 }
JTrindade 0:8609b4a06a8f 141 std::ofstream outfile ("test.txt");
JTrindade 0:8609b4a06a8f 142 outfile << "kp = " << p[0] << "\t ki = " << p[1] << "\t kd = " << p[2] << std::endl;
JTrindade 0:8609b4a06a8f 143 }
JTrindade 0:8609b4a06a8f 144
JTrindade 0:8609b4a06a8f 145 float getErr(int t){
JTrindade 0:8609b4a06a8f 146 //////////apply new vals
JTrindade 0:8609b4a06a8f 147 kp_s = p[0];
JTrindade 0:8609b4a06a8f 148 ki_s = p[1];
JTrindade 0:8609b4a06a8f 149 kd_s = p[2];
JTrindade 0:8609b4a06a8f 150 pidController();
JTrindade 0:8609b4a06a8f 151 error_sensors = sens_r.read() - sens_l.read();
JTrindade 0:8609b4a06a8f 152 position = mid_point + error_sensors;
JTrindade 0:8609b4a06a8f 153 if(position > max_turn) position = max_turn;
JTrindade 0:8609b4a06a8f 154 if(position < min_turn) position = min_turn;
JTrindade 0:8609b4a06a8f 155 error_steering = position - feedback.read();
JTrindade 0:8609b4a06a8f 156
JTrindade 0:8609b4a06a8f 157 lcd.cls();
JTrindade 0:8609b4a06a8f 158 lcd.locate(1,2);
JTrindade 0:8609b4a06a8f 159 lcd.printf("kp:%.2f, ki:%.2f, kd:%.2f \nerr:%.2f", kp_s, ki_s, kd_s, error_steering);
JTrindade 0:8609b4a06a8f 160 return error_steering;
JTrindade 0:8609b4a06a8f 161 }
JTrindade 0:8609b4a06a8f 162