1

The code is for a DC motor position control PID system. I need the angle I need to move to "setpoint" and change it throughout the process. Though it is only changing once and then nothing happens if I insert a new value unless I upload the code again. I need help in finding the problem, thank you.

Code:

double count = 0; //set the counts of the encoder
double angle = 0;//set the angles
boolean A,B;
byte state, statep;

double pwm = 9;// this is the PWM pin for the motor for how much we move it to correct for its error
const int dir1 = 6;//these pins are to control the direction of the motor (clockwise/counter-clockwise)
const int dir2 = 7;

double    setpoint = 0;
double Kp = 0.2;// you can set these constants however you like depending on trial & error
double Ki = 0.25;
double Kd = 1;

float last_error = 0;
float error = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
String readString;

void setup() {
   Serial.begin(9600);
  pinMode(2, INPUT);//encoder pins
  pinMode(3, INPUT);
  attachInterrupt(0,Achange,CHANGE);//interrupt pins for encoder
  attachInterrupt(1,Bchange,CHANGE); 

  pinMode(pwm, OUTPUT);
  pinMode(dir1, OUTPUT);
  pinMode(dir2, OUTPUT);



}

void loop(){



   if (Serial.available()) { //Check if the serial data is available.
    delay(3);                  // a small delay
    char c = Serial.read();  // storing input data
    readString += c;         // accumulate each of the characters in readString
  }

  if (readString.length() >0) { //Verify that the variable contains information

   Serial.println(readString.toInt());  //printing the input data in integer form
    setpoint = readString.toInt();   // here input data is store in integer form

  }
   PIDcalculation();// find PID value  

  Serial.print("         ");
Serial.print(setpoint);
Serial.print("         ");

  if (angle > setpoint+0.5) {
    digitalWrite(dir1, LOW);// Forward motion
    digitalWrite(dir2, HIGH);
  } 
else if  (angle < (setpoint-0.5)) {
    digitalWrite(dir1, HIGH);//Reverse motion
    digitalWrite(dir2, LOW);
  }
  else{
      digitalWrite(dir1, LOW);//Reverse motion
    digitalWrite(dir2, LOW);
  }
  analogWrite(pwm, pidTerm_scaled);

  Serial.println("WHEEL ANGLE:");
  Serial.print(angle);  // here input data is store in integer form;
  delay(100);

}

void PIDcalculation(){


  angle = (0.9 * count);//count to angle conversion
  error = setpoint - angle;

  changeError = error - last_error; // derivative term
  totalError += error; //accumalate errors to find integral term
  pidTerm = (Kp * error) + (Ki * totalError) + (Kd * changeError);//total gain
  pidTerm = constrain(pidTerm, -155, 155);//constraining to appropriate value
  pidTerm_scaled = abs(pidTerm);//make sure it's a positive value

  last_error = error;
}

void Achange() //these functions are for finding the encoder counts
{
  A = digitalRead(2);
  B = digitalRead(3);

  if ((A==HIGH)&&(B==HIGH)) state = 1;
  if ((A==HIGH)&&(B==LOW)) state = 2;
  if ((A==LOW)&&(B==LOW)) state = 3;
  if((A==LOW)&&(B==HIGH)) state = 4;
  switch (state)
  {
    case 1:
    {
      if (statep == 2) count++;
      if (statep == 4) count--;
      break;
    }
    case 2:
    {
      if (statep == 1) count--;
      if (statep == 3) count++;
      break;
    }
    case 3:
    {
      if (statep == 2) count --;
      if (statep == 4) count ++;
      break;
    }
    default:
    {
      if (statep == 1) count++;
      if (statep == 3) count--;
    }
  }
  statep = state;

}

void Bchange()
{
  A = digitalRead(2);
  B = digitalRead(3);

  if ((A==HIGH)&&(B==HIGH)) state = 1;
  if ((A==HIGH)&&(B==LOW)) state = 2;
  if ((A==LOW)&&(B==LOW)) state = 3;
  if((A==LOW)&&(B==HIGH)) state = 4;
  switch (state)
  {
    case 1:
    {
      if (statep == 2) count++;
      if (statep == 4) count--;
      break;
    }
    case 2:
    {
      if (statep == 1) count--;
      if (statep == 3) count++;
      break;
    }
    case 3:
    {
      if (statep == 2) count --;
      if (statep == 4) count ++;
      break;
    }
    default:
    {
      if (statep == 1) count++;
      if (statep == 3) count--;
    }
  }
  statep = state;

}
  • your code is unreadable as it is ..... please use the {} button to format the code ...... just select all of the code text and click {} – jsotola Apr 8 '19 at 13:57
  • Since you have all that Serial calling in your sketch, please copy and paste the serial output, that you get to the question. Also specify what exactly you entered into the serial monitor to get to these results. – chrisl Apr 9 '19 at 6:34

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.