I am trying to control position using L298 motor driver and quadrature encoder but the motor just won't stop at my setpoint np matter what PID parameters I use.
Any ideas?
#include <PID_v1.h>
double Setpoint = 0 ; // will be the desired value
double Input = 0;
double Output = 0 ;
//PID parameters
double Kp = 3.8, Ki = 1.7, Kd = 0.15;
//create PID instance
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int in_A = 2;
int in_B = 3;
// Initialize the counter
// The number of pulses per revolution
// depends on the index disc
unsigned int pulsesperturn = 100;
// The total number of revolutions
unsigned int revolutions = 0;
// Initialize the counter
double pulses = 0;
double angle = 0;
//motor para
int Enable = 10;
int Direc1 = 7;
int Direc2 = 8;
//int motor = 10;
int flag = 1; //indicate the direction of motor, forward(counter clockwise) = 1, backward(clockwise) = 0
int position_index = 0;
void count_pulse() {
// This function is called by the interrupt
// If in_b is HIGH increment the counter
// otherwise decrement it
if (digitalRead(in_B)) {
pulses++;
}
else {
pulses--;
}
}
void setup() {
pinMode(in_A, INPUT_PULLUP);
pinMode(in_B, INPUT_PULLUP);
attachInterrupt(0, count_pulse, RISING);
Serial.begin(9600);
pinMode(Enable, OUTPUT);
pinMode(Direc1, OUTPUT);
pinMode(Direc2, OUTPUT);
myPID.SetTunings(Kp, Ki, Kd);
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC);
myPID.SetSampleTime(1);
myPID.SetOutputLimits(-255, 255);
}
void loop() {
revolutions = pulses / pulsesperturn;
Setpoint = 180;
/*if ( position_index == 0){
Setpoint = 0;
}*/
angle = pulses * 3.6;
//motor_speed = 150;
/*
if ( flag == 1 && angle < 30 ) {
forward();
}
if ( flag == 1 && angle > 30 ) {
flag = 0;
hardstop();
}
if ( flag == 0 && angle > 0 ) {
backward();
}
if ( flag == 0 && angle < 0) {
flag = 1;
hardstop();
}
*/
//Serial.print("angle = ");
Serial.println(angle);
//Serial.println(pulses);
Serial.println(revolutions);
//Input = angle;
Input = pulses;
myPID.Compute();
pwmOut(Output);
}
void pwmOut(int out) { // to H-Bridge board
if (out > 0) {
forward( out ); // drive motor CW
}
else {
backward( out ); // drive motor CCW
}
}
void forward( int motor_speed ) {
analogWrite(Enable, motor_speed);
digitalWrite(Direc1, HIGH);
digitalWrite(Direc2, LOW);
}
void backward( int motor_speed ) {
analogWrite(Enable, motor_speed);
digitalWrite(Direc1, LOW);
digitalWrite(Direc2, HIGH);
}
void hardstop() {
digitalWrite(Enable, HIGH);
digitalWrite(Direc1, HIGH);
digitalWrite(Direc2, HIGH);
}
PID_control.ino (2.61 KB)
