Integer only refresh when Serial.print?

Other Hardware Motors, Mechanics, Power and CNC
1

Hey,

I'm trying to make work a DC motor with encoder by sending him a target (int) through a I2C protocol. The motor is pluged in the slave, and everything work fine for that part of the project. My probleme now is to make the motor go back (send a target that is smaller than the previous one).

I verified and it seem that the smaller target is correctly sent to the "void evalu" / the fonction that calculate the difference beetween the position and the target, and send the direction and power to an other fonction that control the motor. The strange thing is, when I add a Serial.print(target) in the fonction, it make it work but not smoothly (the motor is in permanent tension).

I'm using a L293D Motor Drive Shield on a arduino Mega 2560 for the slave, and an arduino uno for the master. The base code that I modified to work with my shield is from this link : ArduinoTutorials/MultipleEncoders at main · curiores/ArduinoTutorials · GitHub (SimplePositionPID_Follower and _Leader).

Hope to find some help, I never really learned to code so it's brain game for me. :slight_smile:

#include <util/atomic.h> 
#include <Wire.h>
#include "AFMotor.h" // TEST MOTEUR
AF_DCMotor motor1(2); // création de l'objet "motor1"

/*------------ CLASS ------------*/
class SimplePID{
  private:
    float kp, kd, ki, umax;
    float eprev = 0;
    float eintegral = 0;
    
  public:
    // Default initialization list
    SimplePID() : kp(1), kd(0.0), ki(0.0), umax(255), eprev(0.0), eintegral(0.0) {}
    
    // Set the parameters
    void setParams(float kpIn, float kdIn, float kiIn, float umaxIn){
      kp = kpIn; kd = kdIn; ki = kiIn; umax = umaxIn;
    }

    // Evaluate the signal
    void evalu(int pos, int target, float deltaT, float &pwr, int &dir){
      
      // error
  int e = pos - target;
  // derivative
  float dedt = (e-eprev)/(deltaT);

  // integral
  eintegral = eintegral + e*deltaT;

  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;

  // motor power
   pwr = (int)fabs(u);
  if( pwr > 255 ){
    pwr = 255;
  }
  else {pwr = 0;}
  // motor direction
  dir = 1;
  if(u<0){
    dir = -1;
  }
eprev = e;
}
};

/*------------ GLOBALS AND DEFINITIONS ------------*/
// Define the motors
#define NMOTORS 2
#define M0 0
#define M1 1

const int enca[] = {2,1};
const int encb[]= {13,5};
const int pwm[] = {9,5};
const int in1[] = {8,11};
const int in2[] = {10,12};

// Global variables
long prevT = 0;
int posPrev[] = {0,0};

// positions
volatile int posi[] = {0,0};
volatile long pos[] = {0,0};
volatile long target[] = {0,0};

// PID classes
SimplePID pid[NMOTORS];


/*------------ FUNCTIONS ------------*/
void setMotor(int dir, float pwr, int pwm, int in1, int in2){//int pwVall de base
//  analogWrite(pwm,pwmVal);
//Serial.print(pwr);
  if(dir == 1){
motor1.run(FORWARD);
motor1.setSpeed(pwr);
  }
  else if(dir == -1){
motor1.run(BACKWARD);
motor1.setSpeed(pwr);
  }
  else{
motor1.run(RELEASE);
 }
}

template <int j>
void readEncoder(){
  int b = digitalRead(encb[j]);
  if(b > 0){
    posi[j]++;
  }
  else{
    posi[j]--;
  }
}

void sendLong(long value){
  for(int k=0; k<4; k++){
    byte out = (value >> 8*(3-k)) & 0xFF;
    Wire.write(out);
  }
}

long receiveLong(){
  long outValue;
  for(int k=0; k<4; k++){
    byte nextByte = Wire.read();
    outValue = (outValue << 8) | nextByte;
  }
  return outValue;
}

void requestEvent(){
  sendLong(pos[0]);
  sendLong(pos[1]);
}

void receiveEvent(int howMany){
  target[0] = receiveLong();
  target[1] = receiveLong();
}

void setup() {
  Wire.begin(1); // join I2C
  Wire.onRequest(requestEvent); 
  Wire.onReceive(receiveEvent);
  Serial.begin(9600);
  for(int k = 0; k < NMOTORS; k++){
    pinMode(enca[k],INPUT);
    pinMode(encb[k],INPUT);
    pid[k].setParams(1,0,0,255);
  }
  attachInterrupt(digitalPinToInterrupt(enca[M0]),readEncoder<M0>,RISING);
  attachInterrupt(digitalPinToInterrupt(enca[M1]),readEncoder<M1>,RISING);

  motor1.setSpeed(255);
  motor1.run(RELEASE);//TEST MOTEUR
}

/*------------ LOOP ------------*/
void loop() {

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT; // Store the previous time value

  // Read the position in an atomic block to avoid a potential misread 
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    for(int k = 0; k < NMOTORS; k++){
      pos[k] = posi[k];
    }
  }

  // Loop through the motors
  for(int k = 0; k < NMOTORS; k++){
    int dir;
    float pwr;
    pid[k].evalu(pos[k],target[k],deltaT,pwr,dir); // compute the position
    setMotor(dir,pwr,pwm[k],in1[k],in2[k]); // signal the motor
  }

}

That's the slave code.
Thanks for your time !

2

target[] is updated in the context of your I2C interrupt, so you should protect the reading of its data in the same way as you protected the position.

3

Okay thanks for the tips ! I wasn't aware of this issue.
I changed it to that :

    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    for(int k = 0; k < NMOTORS; k++){
      target[k] = targeti[k];
    }
  }

Knowing that targeti is the value sent through I2C.
The target is correct, but the motor still doesn't move when the target change from 5000 to 10 for example.

4

print the result of your PID computation and see what you are asking the motor to do

5

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.