Wheels rotates in different direction (Bluetooth Self Balancing Robot)

I read this issue: two-wheeled differential-drive robot rotation (one or two wheels) - #2 by HazardsMind

It looks similar with my problem but far different I think. I don't know much about different rotation (Wheel A and B), but I don't find any different in my code.

#include "I2Cdev.h"
#include <Wire.h>
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>
#include <digitalIOPerformance.h>                //library for faster pin R/W
//#include <Ultrasonic.h>

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

#define d_speed 1.5
#define d_dir 3


#define IN1 11
#define IN2 10
#define IN3 9
#define IN4 3

char content = 'P';
int MotorAspeed, MotorBspeed;
float MOTORSLACK_A = 28;                   // Compensate for motor slack range (low PWM values which result in no motor engagement)
float MOTORSLACK_B = 28;
#define BALANCE_PID_MIN -210 // Define PID limits to match PWM max in reverse and foward
#define BALANCE_PID_MAX 210

MPU6050 mpu;

const int rxpin = 6;       //Bluetooth serial stuff
const int txpin = 5;
SoftwareSerial blue(rxpin, txpin);

//Ultrasonic ultrasonic(A0, A1);
//int distance;

// MPU control/status vars
bool dmpReady = true;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector



/*********Tune these 4 values for your BOT*********/
double setpoint = 180.04; //set the value when the bot is perpendicular to ground using serial monitor.
double originalSetpoint;
// Read the project documentation on circuitdigest.com to learn how to set these values

#define Kp  21.5 //Set this first
#define Kd  0.1 //Set this secound
#define Ki  80 //Finally set this

#define RKp  21.5 //Set this first
#define RKd 0.49 //Set this secound
#define RKi  80 //Finally set this

/******End of values setting*********/



double ysetpoint;
double yoriginalSetpoint;
double input, yinput, youtput, output, Buffer[3];

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
PID rot(&yinput, &youtput, &ysetpoint, RKp, RKi, RKd, DIRECT);

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
  mpuInterrupt = true;
}


void setup() {

    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    
  Serial.begin(115200);
  blue.begin(9600);
  blue.setTimeout(40);
  init_imu();           //initialiser le MPU6050
  initmot();            //initialiser les moteurs
  originalSetpoint = 180.04;  //consigne
  yoriginalSetpoint = 0.1;
  setpoint = originalSetpoint ;
  ysetpoint = yoriginalSetpoint ;
}



void loop() {
    getvalues();
    Bt_control();
    printval();
}







void init_imu() {
  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  Wire.begin();
  TWBR = 24;
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  devStatus = mpu.dmpInitialize();


  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(120);
  mpu.setYGyroOffset(46);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1688);

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    //setup PID
    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(10);
    pid.SetOutputLimits(-255, 255); // -255, 255
    rot.SetMode(AUTOMATIC);
    rot.SetSampleTime(10);
    rot.SetOutputLimits(-20, 20);  // -20, 20
  }
  else
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}


void getvalues() {
  // if programming failed, don't try to do anything

  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
    new_pid();
  }
  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (mpuIntStatus & 0x02)
  {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
    mpu.dmpGetGravity(&gravity, &q); //get value for gravity
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

    input = ypr[1] * 180 / M_PI + 180;
    yinput = ypr[0] * 180 / M_PI;
  }
}


void new_pid() {
  //Compute error
  pid.Compute();
  rot.Compute();
  // Convert PID output to motor control
  MotorAspeed = compensate_slack(youtput, output, 1);
  MotorBspeed = compensate_slack(youtput, output, 0);
  motorspeed(MotorAspeed, MotorBspeed);            //change speed
}
//Fast digitalWrite is implemented


void Bt_control() {
  if (blue.available()) {
    content = blue.read();
    if (content == 'F')
      setpoint = originalSetpoint - d_speed;//Serial.println(setpoint);}            //forward
    else if (content == 'B')
      setpoint = originalSetpoint + d_speed;//Serial.println(setpoint);}            //backward
    else if (content == 'L')
      ysetpoint = constrain((ysetpoint + yoriginalSetpoint - d_dir), -180, 180); //Serial.println(ysetpoint);}      //left
    else if (content == 'R')
      ysetpoint = constrain(ysetpoint + yoriginalSetpoint + d_dir, -180, 180); //Serial.println(ysetpoint);}        //right
    else if (content == 'S') {
      setpoint = originalSetpoint;
    }
  }
  else { content = 'P';}
}

void initmot() {
  //Initialise the Motor outpu pins
  pinMode (IN4, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN2, OUTPUT);
  pinMode (IN1, OUTPUT);

  //By default turn off both the motor
  analogWrite(IN4, LOW);
  analogWrite(IN3, LOW);
  analogWrite(IN2, LOW);
  analogWrite(IN1, LOW);
}

double compensate_slack(double yOutput, double Output, bool A) {
  // Compensate for DC motor non-linear "dead" zone around 0 where small values don't result in movement
  //yOutput is for left,right control
  if (A)
  {
    if (Output >= 0)
      Output = Output + MOTORSLACK_A - yOutput;
    if (Output < 0)
      Output = Output - MOTORSLACK_A - yOutput;
  }
  else
  {
    if (Output >= 0)
      Output = Output + MOTORSLACK_B + yOutput;
    if (Output < 0)
      Output = Output - MOTORSLACK_B + yOutput;
  }
  Output = constrain(Output, BALANCE_PID_MIN, BALANCE_PID_MAX);
  return Output;
}


void motorspeed(int MotorAspeed, int MotorBspeed) {

  // Motor A control
  if (MotorAspeed >= 0) {
    analogWrite(IN1, abs(MotorAspeed));
    digitalWrite(IN2, LOW);
  }
  else {
    digitalWrite(IN1, LOW);
    analogWrite(IN2, abs(MotorAspeed));
  }

  // Motor B control
  if (MotorBspeed >= 0) {
    analogWrite(IN3, abs(MotorBspeed));
    digitalWrite(IN4, LOW);
  }
  else {
    digitalWrite(IN3, LOW);
    analogWrite(IN4, abs(MotorBspeed));
  }
}

void printval()
{
  Serial.print(yinput); Serial.print("\t");
  Serial.print(yoriginalSetpoint); Serial.print("\t");
  Serial.print(ysetpoint); Serial.print("\t");
  Serial.print(youtput); Serial.print("\t"); Serial.print("\t");
  Serial.print(input); Serial.print("\t");
  Serial.print(originalSetpoint); Serial.print("\t");
  Serial.print(setpoint); Serial.print("\t");
  Serial.print(output); Serial.print("\t"); Serial.print("\t");
  Serial.print(MotorAspeed); Serial.print("\t");
  Serial.print(MotorBspeed); Serial.print("\t"); Serial.print(content); Serial.println("\t");
}

With the code above, my self-balancing robot turns around like a spinner. I check both wheel => have the same direction. My robot (Jimmy) goes crazy :cry: please heeelp

Your post was MOVED to its current location as it is more suitable.

Are both wheels on the same side of the robot? If not, you answered your own question.
Paul

Sorry, I'm not smart enough. What do you mean by saying "both wheels on the same side of the robot"? wait, I try to upload a real image of Jimmy

IF both wheels turn the same direction, they must be on the same side of the robot? Right? If they turn opposite directions, they must be on opposite sides of the robot. Right?'
Paul


Here's my jimmy. What do you think?

I have reduced the speed and it's already balanced but jitter too fast (even with very small Kd as, 0.014;

Think of yourself in place of the motor for one wheel. Which direction do you turn the wheel to go forward? Now place yourself in place of the other motor. Which direction do you turn the wheel to go forward?
Paul

each of them will turn around right? (when I just place my self in one wheel each)

Clockwise or counter clockwise is the question.
Paul

No I mean like this:

  1. both wheels have the same direction when forward and backward. It's balanced enough.
    2., BUT => he turn around (the BODY of the robot turns around and going forward and backward. I don't like to see him like that.

When I make an ONLY self-balancing robot, he didn't like that. When I use bluetooth with that codes, he goes crazy.

Here's the illustration! Sorry for bad pic.

The floor here is 99.999% flat.

Ok, lets back up a bit.
You have two wheels.
You have two motors.
Where in your code do you set the motor direction of rotation?
Paul

Here? I didn't touch this. enA and enB is in jumper cover. When I open them the wheels not turning. So only 4 pinouts are being used.

I see that you can command the motors to either stop or to run at some speed. But where do you tell the motors to turn the other direction?
Paul

1 Like

I don't know. Let me guess, (sorry, I'm a new in coding).

#define d_dir 3

I have ever changed it to other pinout but no luck.

Ehm ... I found it!

Here ...

void new_pid() {
  //Compute error
  pid.Compute();
  rot.Compute();
  // Convert PID output to motor control
  MotorAspeed = compensate_slack(youtput, output, 1);
  MotorBspeed = compensate_slack(youtput, output, 0);
  motorspeed(MotorAspeed, MotorBspeed);            //change speed

I change MotorBspeed = compensate_slack(youtput, output, 0); which has value "0" into 1 and it shows WOW - my jimmy stop going crazy anymore :slight_smile:

I think the problem is = one wheel has different speed with other wheel. Is that right?
SOLVED HERE !!!!!!!!!!!!

That seems to fit the symptoms!
Good luck,
Paul

1 Like

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