Testing program with PID to control the Cytron MD30

ttgo_cytron_pid_v1

/*
* Calculate the error between current direction and the desired direction
* Use a MAP function to convert the error to a value 0-255
* Use that value to feed into a PID and use the output as the power level for the motor controller
*/

#include <Arduino.h>
/* Credit: https://www.youtube.com/channel/UCjiVhIvGmRZixSzupD0sS9Q
 *
 * Very hard left - 3492
 * soft hard left - 2941
 * middle - 1885, 1900, 1906 - so lets use 1906-1885 = 21/2 = 10 + 1885 = 1895
 * soft hard right - 856
 * very hard right - 368
 * 
 */
//#include <Wire.h>

///////////////////////Functions Below///////////////////////
float get_angle(int n);
double computePID(double inp);
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
float convert_pot_to_angle(int inp);
float remap_Kp(int inp);
/////////////////////////////////////////////////////////////

///////////////////////Inputs/outputs///////////////////////
int steer_angle_pin = 38;   // pin for steer angle sensor
int Kp_pot_pin = 39; // MODE pin position for Kp
int Ki_pot_pin = 36; // THROTTLE pin position for Ki
int Kd_pot_pin = 34; // LED pin position for Kd
int target_pot_pin = 37; // STEERING pin position for Kp
int PWMPin = 25;
int DIRPin = 12; //pin 36, and 12 works; 32 does not work, compiles but signal is misinterpreted
///////////////////////////////////////////////////////////

////////////////////////Variables///////////////////////
//int Read = 0;
float steering_angle = 0;
int mtr_speed = 50;               // range is 0-255, 255=12.8V; 250=12.5V; 101=5V; 50=2.5V; 25=1.2V; 
///////////////////////////////////////////////////////

/////////////////// PID variables ///////////////////////
float kp=0; //Mine was 8
float ki=0.0; //Mine was 0.2
float kd=0; //Mine was 3100
long Kp_pot, Ki_pot, Kd_pot;

unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double output, setPoint;
double cumError, rateError;
///////////////////////////////////////////////////////

///////////////////Steering variables///////////////////////
long left_limit = 3100;  // really the limit is ~3500  this would be say -45 degrees
long right_limit = 700;  // really the limit is ~350 this would be say +45 degrees
long middle_steer = 1895;  // this would be angle 0 degrees
long steer_range = (3500-500);
long steering_target;
int steer_effort = 0;
int tolerance = 50;
///////////////////////////////////////////////////////

void setup() {
  Serial.begin(115200);  
  pinMode(steer_angle_pin,INPUT);
  pinMode(Kp_pot_pin,INPUT);
  pinMode(Ki_pot_pin,INPUT);
  pinMode(Kd_pot_pin,INPUT);
  pinMode(target_pot_pin,INPUT);
  pinMode(PWMPin, OUTPUT);
  pinMode(DIRPin, OUTPUT);
}

void loop() {
    Kp_pot = remap_Kp(analogRead(Kp_pot_pin)); 
    Ki_pot = analogRead(Ki_pot_pin);
    Kd_pot = analogRead(Kd_pot_pin);
    steering_target = convert_pot_to_angle(analogRead(target_pot_pin));
    steering_angle = get_angle(50);    
    kp = Kp_pot;  // note: this is moving an INT into a FLOAT
    ki = Kp_pot/1000;
    kd = Kp_pot/1000;
    Serial.print(", cur_pos: "); Serial.print(steering_angle);
    Serial.print(", steering_target: "); Serial.print(steering_target);
    Serial.print(", steer_effort: "); Serial.print(steer_effort);    
    Serial.print(", steer error: "); Serial.println(error);
    setPoint = steering_target;
    output = computePID(steering_angle);
    steer_effort = mapfloat(output, 0, steer_range, 0, 255);
// if the steering pos > beyond a certain direction - set the motor power to zero
    //clamp the values of steer_effort between -255 to 255
    if (steer_effort < -255){steer_effort = -255;}
    if (steer_effort > 255){steer_effort = 255;}    
    
    if(error < (tolerance*-1)){ 
        digitalWrite(DIRPin, HIGH);   // steer right
        analogWrite(PWMPin, abs(steer_effort)); 
      } 
    else if(error > tolerance){
        digitalWrite(DIRPin, LOW);    // steer left
        analogWrite(PWMPin, steer_effort);
    }
    else {
        analogWrite(PWMPin, 0);       // Turn the motor off  
    }
    delay(500);
  }

float get_angle(int n){  // read the steering angle and average readings (i.e. pot value)
  long sum=0;
  for(int i=0;i<n;i++){
    sum=sum+analogRead(steer_angle_pin); 
    }  
  float adc=sum/n;
  return(adc);
}

double computePID(double inp){     
  // ref: https://www.teachmemicro.com/arduino-pid-control-tutorial/
        currentTime = millis();                                     // get current time
        elapsedTime = (double)(currentTime - previousTime);         // compute time elapsed from previous computation
        error = setPoint - inp;                                     // determine error
        cumError += error * elapsedTime;                            // compute integral
        rateError = (error - lastError)/elapsedTime;                // compute derivative
        double out = ((kp*error) + (ki*cumError) + (kd*rateError)); // PID output               
        Serial.print("p: "); Serial.print(kp);
        Serial.print(", i: "); Serial.print(ki);
        Serial.print(", d: "); Serial.print(kd);
        //Serial.print(", cErr: "); Serial.print(cumError);
        //Serial.print(", error: "); Serial.print(error);
        //Serial.print(", rErr: "); Serial.print(rateError);
        //Serial.print(", Output: "); Serial.println(out);
        lastError = error;                       //remember current error
        previousTime = currentTime;              //remember current time
         return out;                             //have function return the PID output
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min)*(out_max - out_min) / (in_max - in_min) + out_min;
}
float convert_pot_to_angle(int inp){ 
    // the input of the steering target potentiometer is 0-4095; This needs to be converted to 3500 (left) to 500 (right)
    // how to convert 0-4095 to an angle?
    // convert to -45 to 45 (these would be degrees (i.e an angle))
    // conver the angle into the angle_sensor
    int out;
    out = mapfloat(inp, 0, 4095, left_limit, right_limit);
    return out;  
}
float remap_Kp(int inp){ 
    // the input of the steering target potentiometer is 0-4095; This needs to be converted to 3500 (left) to 500 (right)
    int out;
    out = mapfloat(inp, 0, 4095, 0, 20);
    return out;  
}