My robot starts to behave weird and doing meaningless rotations during autonomous navigation (local planner or odom problem I guess)

asked 2021-09-24 16:32:12 -0600

Kareem Magdy gravatar image

I have assembled a simple mobile robot to accomplish autonomous navigation using the navigation stack. I am using Jetson Nano (Ubuntu 18.04 & ROS Melodic), YD Lidar, and encoded dc motors. Basically following this documentation: https://www.youtube.com/watch?v=Uz_i_...

I have 2 Arduino Uno running my motor driver and communicating with Jetson Nano through two I2C buses to receive PID setpoint from ROS and to send the change in degrees of the motors in degrees back to ROS (supposed to take that change and compute odometry within ROS). Here is the code uploaded to each Arduino:

#include <PinChangeInt.h>
#include <Wire.h>
#include <PID_v1.h>
#define encodPinA1      2                       // Quadrature encoder A pin
#define encodPinB1      3                       // Quadrature encoder B pin
#define M1              5                       // PWM outputs to L298N H-Bridge motor driver module
#define dir             4

double kp =1, ki =20 , kd =0;             // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
unsigned long lastTime,now;
volatile long encoderPos = 0,last_pos=0,lastpos=0;
PID myPID(&input, &output, &setpoint, kp, ki, kd,DIRECT);  
void setup() {
  pinMode(encodPinA1, INPUT_PULLUP);                  // quadrature encoder input A
  pinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input B
  pinMode(M1, OUTPUT);
  pinMode(dir, OUTPUT);
  attachInterrupt(0, ai1, FALLING);               // update encoder position

  TCCR1B = TCCR1B & 0b11111000 | 1;                   // To prevent Motor Noise

  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);

  Wire.begin(9);                // join i2c bus with address #9 for Right Motor
  Wire.onRequest(requestEvent); // register events
  Wire.onReceive(receiveEvent);
  Serial.begin (9600); 

}

void loop() {
   now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=500 )
   {
      input = (-360.0*1000*(encoderPos-last_pos)) /(1856.0*(now - lastTime));
      lastTime=now;
      last_pos=encoderPos;
   }

  myPID.Compute();                                    // calculate new output
  pwmOut(output, setpoint);                                     // drive L298N H-Bridge module
  delay(10);
}

void pwmOut(int out, int set) {                                // to H-Bridge board
  if ((out > 0) && (set > 0)) {
    //myPID.SetOutputLimits(200, 255);
    digitalWrite(dir, LOW);
   // analogWrite(M1, 0);                             // drive motor CW
    analogWrite(M1, out);
  }
  else if ((out < 0) && (set < 0)) {
    //myPID.SetOutputLimits(-200, -255);
    digitalWrite(dir, HIGH);
    analogWrite(M1, abs(out));
  }
  else {
    analogWrite(M1, 0);  
    output = 0;
  }
}

void ai1() {
  // ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
  // Check with pin 2 to determine the direction
  if(digitalRead(encodPinB1)==HIGH) {
  encoderPos++;
  }else{
  encoderPos--;
  }
}

void requestEvent() {
  int8_t s;

  s= (-360.0*(encoderPos-lastpos))/1856.0; //change in position in degrees of the wheel
  lastpos=encoderPos;
  Wire.write(s); // respond with message of 6 bytes
  //Serial.println(s);
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany)
{
  uint8_t a,b;
  a = Wire.read();
  b = Wire.read();
  setpoint= (double)((b<<8)|a);
  Serial.println((uint8_t)a);
  Serial.println((uint8_t)b);

  //Serial.println(setpoint);
}

I have a doubt in the Arduino code in the request event fn where did that 1856.0 come from, I think it is related to the encoder PPR. My hardware interface node along with launch files and config files are given below:

#include <mobile_robot_autonomous_navigation/robot_hardware_interface.h>

//namesapce ...
(more)
edit retag flag offensive close merge delete

Comments

Hi Kareem

A lot of details in your question but it makes it hard to help. I suggest you start with a series of small tests, to narrow down the problem and raise questions on the specific problems you are facing. For example test your robot functionality of movement with teleoperation and make sure that with your control the robot is moving as expected, that way you may discard odometry issues, etc. Then do tests for the navigation, until you find what is not working as expected. From there you narrow it a little more down and make one change at the time to see that your hypothesis is correct. Too many changes at once make it very hard to troubleshoot and you might forget what did you change as well.

osilva gravatar image osilva  ( 2021-09-27 09:05:04 -0600 )edit

Hi Osilva, thanks for the reply. I already did these small test the conclusion are that the amcl, odom, global planner, teleoperation are working fine. The problem is either with the dwa local planner (tends to rotate in place instead of following the path) or my URDF please take a look at this question by me, maybe you would be able to help: https://answers.ros.org/question/3874...

Kareem Magdy gravatar image Kareem Magdy  ( 2021-09-27 09:44:04 -0600 )edit

I added a comment to your prior question. Hope it helps.

osilva gravatar image osilva  ( 2021-09-27 11:28:56 -0600 )edit