Hello everyone :
: ,
I need to clean and adapt my engine control program by PID (called C engine).
I would like to assign a positive or negative speed to the motor C with a potentiometer and recover the actual speed of the motor C to assign it to a motor D.
Also, recover the output value to control the voltage of the motor C and assign it to a motor B.
The problem is that I had trouble adapting this program.
I made two different versions.
In the first the PID works and I manage to assign the motor B, but I control the motor only in one direction.
In the second, I control the motor C in both directions and I manage to recover the actual speed of the motor C, but the PID no longer works as it should.
I explain it this way in the two programs to better situate the errors:
In the 1st, When I brake, block or rotate the engine in the other direction, the output-value changes as it should.
But I would like to control for myself the direction of rotation of the engine not by the program.
What I tried in the other program, but the output-value changes logically and only when the setpoint is close to 0 but regardless of the direction of the setpoint (-5 to 5 for example).
The first 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 = Setpoint; // to set speed to motor with encoder
int stateB = map (val_output, 0,100, -250,250);// val-outpout entre 0 et 300 (works good)
int stateD = map(-duration, 0, 335, -250,250); // -duration entre 0 et 600 program is set in forward i have only one way detected
// stateD is between -255, -245
//Serial.println (sensorReading);
Serial.print ("stateB :"); Serial.println(stateB);
Serial.print ("stateD :"); Serial.println(stateD);
// an other way to program the COUNT (if problem with interrupt?)
noInterrupts();
latestDurationCount = duration;
interrupts();
abs_duration = abs(latestDurationCount);
advance();//Motor Forward or back(); //Backward (originaly in the program)
// The program choose the direction but i will prefer that direction has been chosen by me (the set point with potar) and the encoder (actual speed and direction)
// advance();//Motor Forward or Backward (originaly in the program)
// Originaly way to program the COUNT of pulse
// abs_duration=abs(duration);
result=myPID.Compute();//PID conversion is complete and returns 1 (here or in void ControlMotor()?)
if(result) // here the COUNT depanding of Motor Way (forward, backward) that's why only one way of pulse id detected
{
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
// When I brake, block or rotate the engine in the other direction, the output-value //changes as it should.
// But I would like to control for myself the direction of rotation of the engine not by //the program.
// What I tried in the other program,
// but the output-value doesn't changes logically and only when the setpoint is close //to 0 but regardless of the direction of the setpoint (from -5 to 5 for example)
}
// 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);
}
I don't have enough space to put the other program, so please, open the other in attachments.
Sorry for my English and my poor level in programmation,
I tried to make my best to be the more explicit.
Regards,
DFrobot5ms_potar3.ino (7.86 KB)