Hi
I’m trying to control a motor with PID algorithm in both ways.
But only one works.
Can anyone explain to me why? or How can I do? :o :o
My program below
//This is the DFRobot TT Micro DC geared motor with encoder.
//It is a motor with a 120:1 gearbox and an integrated quadrature encoder that provides
// a resolution of 16 pulse single per round giving a maximum output of 1920 within one round.
//The sample code for driving one way motor encoder
#include <PID_v1.h>
const byte encoder0pinA = 2;//A pin -> the interrupt pin 0
const byte encoder0pinB = 3;//B pin -> the digital pin 3
byte encoder0PinALast;
double latestDurationCount;// if we change the kind of COUNTING pulse
double duration,abs_duration;//the number of the pulses
boolean Direction;//the rotation direction
boolean result;
double val_output;//Power supplied to the motor PWM value. (in Volt)
double Setpoint;// assignated speed (when setpoint = 30) => val-outpout and duration(pulse)== (41 to 43) when no couple in the motor
double Kp=0.6, Ki=5, Kd=0; // Set Kp, Ki and Kd "to set the strenght of distortion of the error"
PID myPID(&abs_duration, &val_output, &Setpoint, Kp, Ki, Kd, DIRECT);
// PORT POUR KIT L298N 1st (bleu foncé)
//Ports de commande du moteur B (helice avec tige)
int motorPinB1 = 4;
int motorPinB2 = 7;
const int enablePinB = 6; // Commande de vitesse moteur, to Output ENB the first
/*//Ports de commande du moteur A (pas dispo)
int motorPinA1 = 2; //
int motorPinA2 = 4; //
const int enablePinA = 5; // Commande de vitesse moteur,to Output ENA the first
*/
// PORT POUR KIT L298N 2nd (Bleu clair)
//Ports de commande du moteur C supplied with 12 volt.
int motorPinC1 = 13; //13 pin to set H BRIDGE of L298N
int motorPinC2 = 12; //12 pin to set H BRIDGE of L298N
const int enablePinC= 11; //11 // Commande de vitesse moteur, to Output ENA pour L298 the second
//Ports de commande du moteur D
int motorPinD1 = 8;
int motorPinD2 = 9;
const int enablePinD = 10; //10 Commande de vitesse moteur, to Output ENB pour L298 the second
void setup()
{
Serial.begin(9600);//Initialize the serial port
/* // Configuration des ports en mode "sortie" pour moteur A
pinMode(motorPinA1, OUTPUT);
pinMode(motorPinA2, OUTPUT);
pinMode(enablePinA, OUTPUT);
*/
// Configuration des ports en mode "sortie" pour moteur B
pinMode(motorPinB1, OUTPUT);
pinMode(motorPinB2, OUTPUT);
pinMode(enablePinB, OUTPUT);
// Configuration des ports en mode "sortie" C
pinMode(motorPinC1, OUTPUT); //L298N Control port settings direction of motor C (originaly L298P)
pinMode(motorPinC2, OUTPUT); //L298N Control port settings direction of motor C
pinMode(enablePinC, OUTPUT); // powerRate to control speed of motor C
// Configuration des ports en mode "sortie" D
pinMode(motorPinD1, OUTPUT);
pinMode(motorPinD2, OUTPUT);
pinMode(enablePinD, OUTPUT);
//Originaly to Set the output value of the PID // (read between 42 and 43 pulses when setpoint=30)
Setpoint=0;
myPID.SetMode(AUTOMATIC);//PID is set to automatic mode
myPID.SetSampleTime(5);//Set PID sampling frequency was 100ms. Carefull! Put 10 or 5ms
EncoderInit();//Initialize the module
}
void loop()
{
// read the sensor value:
int sensorReading = analogRead(A0);
// SCALING EXPERIMENT with value outpout and/or pulse (duration)
// map it to a range from -value to value
Setpoint = map(sensorReading, 0, 1023, -255, 255);
int stateC = val_output; // to set PID speed (negative or positive) to motor C (with encoder) in void ControlMotor()
int stateB = map (val_output, 0,100, -250,250);// val-outpout entre 0 et 100 (works good) but in one way
int stateD = map(duration, 0, 335, -250,250); // duration entre 0 et 600 program is set in forward i have only one way detected
//Serial.println (sensorReading);
Serial.print ("stateB :"); Serial.println(stateB);
Serial.print ("stateD :"); Serial.println(stateD);
// A way to program the COUNT (if problem with interrupt?)
noInterrupts();
latestDurationCount = duration;
interrupts();
abs_duration = abs(latestDurationCount);
// A manner to control way of motor and trying to adapt PID process in the 2 way
if (Setpoint<0)
{ //back();
Direction = LOW; // or false?
val_output=-val_output;
}
else if (Setpoint>0)
{ //advance();
Direction = HIGH; // or true?
}
// Advance(); // Forward
// back(); //(Backward originaly in the program)
// Originaly The program choose the direction but i prefer the direction is choosen by the setpoint
// an other way to program the COUNT of pulse
// abs_duration=abs(duration);
result=myPID.Compute();//PID conversion is complete and returns 1
if(result)
{
Serial.print("Pulse: ");
Serial.println(duration);
duration = 0; //Count clear, wait for the next count
Serial.print("val_output"); Serial.println(val_output);// volt to MOTOR
Serial.println(result);
Serial.println(Direction);
// When I brake, block or rotate the engine in the other direction, the output-value //. changes as it should
// BUT ONLY when Setpoint>O and Direction=1
// (result) is always=1 to manage PID
}
// object Control Motor Sens et Vitesse du mouvement
// ControlMotor(stateA, motorPinA1, motorPinA2, enablePinA);
ControlMotor(stateB, motorPinB1, motorPinB2, enablePinB);
ControlMotor(stateC, motorPinC1, motorPinC2, enablePinC);
ControlMotor(stateD, motorPinD1, motorPinD2, enablePinD);
}
void ControlMotor(int speed, const byte pin1, const byte pin2, const byte enablePin)
{
if (speed > 0) // avant // CW
{
digitalWrite(pin1, HIGH);
digitalWrite(pin2, LOW);
analogWrite(enablePin, speed);
}
else if (speed < 0) // arrière // CCW
{
digitalWrite(pin1, LOW);
digitalWrite(pin2, HIGH);
analogWrite(enablePin, -speed);
}
else // Stop (freinage)
{
digitalWrite(pin1, HIGH);
digitalWrite(pin2, HIGH);
analogWrite(enablePin, 0);
}
}
void EncoderInit()
{
Direction = true;//default -> Forward
pinMode(encoder0pinB, INPUT);
attachInterrupt(0, wheelSpeed, CHANGE);
}
void wheelSpeed()
{
int Lstate = digitalRead(encoder0pinA);
if((encoder0PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder0pinB);
if(val == LOW && Direction)
{
Direction = false; //Reverse
}
else if(val == HIGH && !Direction)
{
Direction = true; //Forward
}
}
encoder0PinALast = Lstate;
if(!Direction) duration++;
else duration--;
}
/*void advance()//Motor Forward
{
digitalWrite(motorPinC1,HIGH);
digitalWrite(motorPinC2,LOW);
analogWrite(enablePinC,val_output);
}
void back()//Motor reverse
{
digitalWrite(motorPinC1,LOW);
digitalWrite(motorPinC2,HIGH);
analogWrite(enablePinC,val_output);
}
void Stop()//Motor stops
{
digitalWrite(enablePinC, LOW);
}
*/
Thanks