File content as of revision 0:8609b4a06a8f:

#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;
}