Used to control a DC steering motor powered by a Cytron MD30C. Currently this is a testing program to determine the PID values. To make that process faster this program reads 3 potentiometers to allow changing the Kp, Ki and Kd values. The current MCU is a TTGO LoRa ESP32 OLED board. The motor is producing a high pitched whining sound. Further r…

ttgo_cytron_pid_v3

/*
* Calculate the error between current direction and the desired direction
* convert the error to a value 0-255 to drive the Cytron motor controller connected to a steering motor.
* Ref: https://www.youtube.com/channel/UCjiVhIvGmRZixSzupD0sS9Q
* 
* Thank you Jeff Sampson for guidance and corrections
*/

#include <Arduino.h>

///////////////////////Functions Below///////////////////////
float get_angle(int n);
double computePID(float inp);
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
void steerVehicle();
void checkSafety();
void printInfo();
void getInputParms();
void blink_LED();
void left_to_right();
/////////////////////////////////////////////////////////////

///////////////////////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
int ledState = LOW;         // ledState used to set the LED
//variables for testing to the right and back again
int test_sw = 0;  // turn the wheel all the way to the left before starting this test
unsigned long test_start_time = 0; 
float test_duration = 0;
///////////////////////////////////////////////////////////

/////////////////// PID variables ///////////////////////
float kp=0; 
float ki=0.0; 
float kd=0; 
unsigned long currentTime, previousTime;
float elapsedTime;
float error;
float lastError;
float output, setPoint;
float cumError, rateError;
///////////////////////////////////////////////////////

///////////////////Steering variables///////////////////////
float safety_margin_pot = 400; // reduce this once I complete field testing
float left_limit_pot = 3499 - safety_margin_pot;  // the actual extreme limit is 3499
float left_limit_angle = -45; // this is a guess - change based on field testing
float right_limit_pot = 431 + safety_margin_pot;  // the actual extreme limit is 431
float right_limit_angle = 45;  // this is a guess - change based on field testing
float steering_target_angle = 0;
float steering_target_pot = 0;
float steering_actual_angle = 0;
float steering_actual_pot = 0;
float steer_effort = 0;
float tolerance = 0.4;  // need to adjust this based on angles
const int motor_power_limit = 150;
/////////////////////////////////////////////////////////////

/////////////////////Loop Timing variables///////////////////////
const long steerInterval = 50;  // 100 10 HZ, 50 20Hz, 20 = 50 Hz
const long infoInterval = 500;  // 100 = 1/10 of a second (i.e. 10 Hz) 3000 = 3 seconds
const long safetyInterval = 1000; 
const long paramsInterval = 50;
unsigned long prev_safety_time = 0; 
unsigned long prev_time_steer = 0;
unsigned long prev_time_printinfo = 0;
unsigned long prev_time_params = 0;
////////////////////////////////////////////////////////////////

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);
  pinMode(LED_BUILTIN, OUTPUT);
  if (test_sw == 1){
    Serial.println("pausing before test starts...");
    delay(10000);
    Serial.println("starting...");
    test_start_time = millis();
    }
}

void loop() {
  unsigned long currentMillis = millis();
  if ((currentMillis - prev_time_params)    >= paramsInterval) {getInputParms();}
  if (test_sw == 1){left_to_right();}
  if ((currentMillis - prev_time_steer)     >= steerInterval)  {steerVehicle();}
  if ((currentMillis - prev_safety_time)    >= safetyInterval) {checkSafety();}
  if ((currentMillis - prev_time_printinfo) >= infoInterval)   {printInfo();}  
}  // end of main loop

void getInputParms(){
    steering_actual_pot = get_angle(1);
    steering_target_pot = analogRead(target_pot_pin);
    steering_target_angle = mapfloat(steering_target_pot, 0, 4095, left_limit_angle, right_limit_angle);
    setPoint = steering_target_angle; 
    kp = mapfloat(analogRead(Kp_pot_pin), 0, 4095, 13, 39);
    ki = mapfloat(analogRead(Ki_pot_pin), 0, 4095, 0.001, 0.1);
    kd = mapfloat(analogRead(Kd_pot_pin), 0, 4095, 2000, 4000);
    prev_time_params = millis();
}

void steerVehicle(){
    //Serial.print("e: "); Serial.println(error); 
    /* The next statement is used to convert the analog potentiometer value to an angle, based on the steering 
    characteristics of the vehicle. */
    steering_actual_angle = mapfloat(steering_actual_pot, left_limit_pot, right_limit_pot, left_limit_angle, right_limit_angle);
    steer_effort = computePID(steering_actual_angle);

   /*  Safety clamp:  The max_power_limit could be as high as 255 which 
    would deliver 12+ volts to the steer motor.  I have reduced the highest setting that allows the wheels
    to be moved easily while sitting on concrete (e.g. motor_power_limit = 150 )  */
    if (steer_effort < (motor_power_limit*-1)){steer_effort = (motor_power_limit*-1);}  //clamp the values of steer_effort
    if (steer_effort > motor_power_limit){steer_effort = motor_power_limit;} // motor_power_limit
   
    if(error > 0){    
        //Serial.print("e-r: "); Serial.print(error);  
        //Serial.print("s-r: "); Serial.println(steer_effort);                 
        digitalWrite(DIRPin, HIGH);   // steer right - channel B led is lit; Red wire (+) to motor; positive voltage
        if ((steering_actual_pot > left_limit_pot) || (steering_actual_pot < right_limit_pot)) {steer_effort = 0;}  // safety check
        analogWrite(PWMPin, steer_effort);
        } 

    else if(error < 0){   
        //Serial.print("e-l: "); Serial.print(error); 
        //Serial.print("s-l: "); Serial.println(steer_effort);   
        digitalWrite(DIRPin, LOW); // steer left - channel A led is lit; black wire (-) to motor; negative voltage
        if ((steering_actual_pot > left_limit_pot) || (steering_actual_pot < right_limit_pot)) {steer_effort = 0;}  // safety check
        analogWrite(PWMPin, abs(steer_effort));
        }
    else {
        steer_effort = 0;
        analogWrite(PWMPin, steer_effort);       // Turn the motor off  
        }    
    prev_time_steer = millis();
}  // end of steerVehicle

double computePID(float 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
      float out = ((kp*error) + (ki*cumError) + (kd*rateError)); // PID output               
      lastError = error;                                          // remember current error
      previousTime = currentTime;                                 // remember current time
      return out;                                                 // have function return the PID output
}

void checkSafety(){ //checkSafety
  /* not yet written - placeholder for safety check 
  - check for LoRa radio connectivity (i.e. is the RSSI strong)
  - check for serial communication coming from the Tractor CPU (i.e. laptop)
  */
  prev_safety_time = millis();
}

void printInfo(){
    blink_LED();
    Serial.print("p: "); Serial.print(kp,2);
    Serial.print(", i: "); Serial.print(ki,5);
    //Serial.print(", Ki_pot: "); Serial.print(Ki_pot,2);
    Serial.print(", d: "); Serial.print(kd,2);  
    Serial.print(", actual: "); Serial.print(steering_actual_angle);
    //Serial.print(", act_pot: "); Serial.print(steering_actual_pot);
    //Serial.print(", PID output: "); Serial.print(output);
    Serial.print(", target: "); Serial.print(steering_target_angle);
    Serial.print(", PID error: "); Serial.print(error);     
    Serial.print(", steer_effort: "); Serial.print(steer_effort);    
    Serial.println(" ");
    prev_time_printinfo = millis();
}

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

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

void blink_LED(){
// this helps with debugging to visually show the microcontroller is functioning
    if (ledState == LOW) {ledState = HIGH;} else {ledState = LOW;}  // if the LED is off turn it on and vice-versa:
    digitalWrite(LED_BUILTIN, ledState);  // set the LED with the ledState of the variable:
}
void left_to_right(){
  /*  this is used to see how fast the steering takes to go from full left to full right and back again
  10/7/22 - with Kp at 18.7 and power at 120 it takes 3.9 seconds left to right to left

  Protocal:
  - manually set target to left extreme (i.e. steer the wheels to extreme left)
  - set test_sw in program to "1"
  - recompile and upload
  - use stopwatch and start when wheels start moving
  - Note time reported for the move to the right
  - stop stopwatch when wheels return to left position
  - set test_sw in program to "0"
  - recompile and upload
*/
    setPoint = 45; // the furthest right position
    test_duration = (millis() - test_start_time)/1000;
    /* if actual_pot is close to end (at full right the pot is ~900), then print duration */
    if (steering_actual_pot < 980){
      Serial.println(" ");
      Serial.print("test_sw: "); Serial.print(test_sw);
      Serial.print(", act_pot: "); Serial.print(steering_actual_pot);
      Serial.print(", test_duration: "); Serial.println(test_duration);
      test_sw = 0;
    }
}