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;
}
{}button to format the code ...... just select all of the code text and click{}– jsotola Apr 08 '19 at 13:57