TRYING to ADAPT A PID program and CLEANING it.

Hello everyone ::slight_smile: ::slight_smile: ,

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)