Hi, I've written a program for controlling the position of a DC motor with PID. I have a predesigned circuit with a dc motor and a IC driver that has one input for motor direction and one for PWM. I should control the position of a handle on a rail that is connected to the motor until it reaches a setpoint distance. During the time that handle is moving if the opto-sensor detects the handle the motor should be turned of and not to control the handle. My problem is that handle starts to move and motor through a PID controls the handle movement, but when opto-sensor detects the handle motor continuously tries to control the handle and isn't zero. Although I put the PWM value zero. I send my codes here. Is there any one here to see what is the problem of my program?
#include <Encoder.h>
#include <Encoder.h>
#define ENCA1 15 // encoderA motor connected to 15th microcontroller pins
#define ENCB1 14 // encoderB connected to 14th microcontroller pins
#define PWM1 6 // analog pin
#define PH1 19 //digital pin, output, sets the direction
#define opto_rev 8 // optocopulers of two ends
#define opto_fwd 10
float a,b;
int direction =0; //direction value 0: CCW, 1: CW. - Stored value
// PID parameters
float kp = 1.6;
float kd = 0.025;
float ki = 0.01;
float controlSignal=0;
float pwmVal=0; // PWM shows the power of motor to control speed
int Error=0;
// set target position
int target = 500;
int prevPos =0; // previous position of motor
volatile int newPos = 0; // new position of motor
float prevT = 0;
float prevError = 0;
float eintegral = 0;
long unsigned int flag=0;
void setup() {
Serial.begin(9600);
// Motor Encoder
pinMode(ENCA1,INPUT);
pinMode(ENCB1,INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA1),readEncoder1,CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCB1),readEncoder2,CHANGE);
pinMode(opto_rev, INPUT);
pinMode(opto_fwd, INPUT);
pinMode(PWM1,OUTPUT);
pinMode(PH1,OUTPUT);
Serial.println("target pos");
}
void loop(){
while (digitalRead(opto_fwd)==LOW){
calculatePID();
setMotorSlider();
prevPos = newPos;
}
pwmVal=0;
analogWrite(PWM1, pwmVal);
// with defining this condition we can see the results in every 500000, since the output prints were two much
if (flag++ == 500000){
Serial.print(target);
Serial.print(",");
Serial.print(pwmVal);
Serial.print(",");
Serial.print(Error);
Serial.print(",");
Serial.println(direction);
Serial.print(newPos);
Serial.print(",");
flag=0;
}
} // main loop ends
// defining the interrupt function for finding motor position
void readEncoder1(){
a=digitalRead(ENCA1);
b=digitalRead(ENCB1);
if(a==b){
newPos++; // :CW direction
}
else{ // :CCW direction
newPos--;
}
}
void readEncoder2(){
a=digitalRead(ENCA1);
b=digitalRead(ENCB1);
if(a !=b){
newPos++; // :CW direction
}
else{ // :CCW direction
newPos--;
}
}
//Determine speed and direction based on the value of the control signal
void setMotorSlider(){
// motor direction
if (controlSignal < 0) { // negative value of control signal means motor direction is counterclockwise
direction = -1;
}
else if (controlSignal > 0){
direction = 1;
}
else{
direction = 0;
}
//-----------------------------
// motor speed
pwmVal = fabs(controlSignal); //PWM values cannot be negative and have to be integers
if (pwmVal > 255){
pwmVal = 255;
}
if (pwmVal <50 && Error !=0){
pwmVal = 50;
}
//------------------------------
if (direction == 1){
digitalWrite(PH1, HIGH);
}
else if (direction == -1){
digitalWrite(PH1, LOW);
}
//Then we set the motor speed
analogWrite(PWM1, pwmVal);
}
void calculatePID(){
//Determining the elapsed time
long currT = micros();
float deltaT = ((float) (currT - prevT))/( 1.0e6 );
if (deltaT > 0){
prevT = currT;
// Error
Error = target-prevPos;
// derivative
float dedt = (Error-prevError)/(deltaT);
// integral
eintegral = eintegral + Error*deltaT;
// control signal
controlSignal = kp*Error + kd*dedt + ki*eintegral;
// store previous error
prevError = Error;
} // if
}