0
$\begingroup$

I am developing a robot using esp32, mecanum wheel, stepper motor and mouse sensor. The robot runs 1000mm forward, 10mm sideways (right side without turning), 1000mm backward, 10mm right and cycle continues. This process must follow 1mm accuracy and repeatability. Two mouse sensor is used, one for distances measurement and another for tilt measurement of robot using basic math's (theta= atan(x/y)) to check whether the robot is facing forward or not. both mouse are fairly calibrated to match the values. Now, the problem I am encountering is that mecanum wheel creates error while running straight and specially side ways. This creates large difference in direction after every run and error gets stacked.

#include <AccelStepper.h>
#include <BluetoothSerial.h>
#include <SPI.h>
#include "ADNS2080.h"
#define SCLK 33  // Serial clock pin on the Arduino
#define SDIO 32  // Serial data (I/O) pin on the Arduino
#define convertionToMM(x) ((x / 2277.14) * 25.4)
#define DISTANCE 12000  //300 mm // 113.792
#define INTELLI_MOUSE 3
#define SCALING_1_TO_1 0xE6
#define RESOLUTION_8_COUNTS_PER_MM 3
#define MOTION_ST 0x02
#define RUN_DOWNSHIFT 0x0e
#define MOUSE_CTRL 0x0d
#define MOTION_CTRL 0x41
#define RESET 0x3a

//................................................................................................................
// Define the stepper motor s and the pins the will use
AccelStepper LeftBackWheel(1, 14, 27);   // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 26, 25);  // Stepper2
AccelStepper RightBackWheel(1, 23, 22);  // Stepper3
AccelStepper RightFrontWheel(1, 4, 2);   // Stepper4
BluetoothSerial Bluetooth;               // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
BluetoothSerial SerialBT;
ADNS2080 Optical1 = ADNS2080(SCLK, SDIO);  // Mouse Sensor
bool _supportsIntelliMouseExtensions = false;

volatile uint8_t receiveBuffer[18];
char Dstatu, Dwheel;
signed char Dx, Dy, Mc;
int X = 0, Y = 0, W = 0, D = 0;
char DataWheel[4];
int count = 0, Sum = 0;
float mouse2_x, mouse2_y;
float center_x = 0;
float center_y = 0;
float slopM = 0;
float ang = 0;
int tempAssignment = 0;

int wheelSpeed = 20000;
int index1;
int dataIn, m;
int lbw[50], lfw[50], rbw[50], rfw[50];  // for storing positions/steps
long count_x = 1011.27;
long count_y = 11520.18;

String inputFromBluetooth;
int pos = 0;

bool isForward = false;
bool isRight = true;
bool isManualDeviation = false;
//bool isRestrictX = false;
bool isCorrection = true;
int orgY = 0;
bool isFromReverse = false;
bool isRestrictReverse = true;
bool IsAfterYOffset = false;
int correctionSpeed = 0;
int cycleCount = 0;
float m2_y_temp = 0;
bool m2_reset = false;
float m2_x_temp = 0;
bool m2x_reset = false;
bool x2_reset = false;
unsigned long timeSinceLastOutput = millis();
//.........................................................................................................................
void setup() {
  delay(500);
  // Set initial seed values for the steppers
  LeftFrontWheel.setMaxSpeed(wheelSpeed * 10);
  LeftBackWheel.setMaxSpeed(wheelSpeed * 10);
  RightFrontWheel.setMaxSpeed(wheelSpeed * 10);
  RightBackWheel.setMaxSpeed(wheelSpeed * 10);
  SerialBT.begin("ESP32");
  Serial.begin(115200);
  Bluetooth.begin(115200);  // Default baud rate of the Bluetooth module
  SerialBT.begin(115200);
  if (!Bluetooth.begin("ESP32")) {
    Serial.println("An error occurred initializing Bluetooth");
  }
  Bluetooth.setTimeout(1);

  Optical1.writeReg(RESET, 0x5a);  // force reset
  Optical1.writeReg(MOUSE_CTRL, 0x39);
  Optical1.writeReg(MOTION_CTRL, 0x00);  //Clear Motion Control register

  delay(100);
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}

void loop() {                          //......................................................................................................................
  if (Serial.available() > 0) {        // Check if there's incoming data
    char mouse2_data = Serial.read();  // Read the incoming data from serial monitor,mouse2
    //SerialBT.write(mouse2_data); // Send the data over to the Bluetooth terminal
    switch (mouse2_data) {
      case 'x':

        if (m2x_reset) {

          m2_x_temp = -(Serial.parseFloat() * 1931.01 / 1908.01);
          Bluetooth.print("m2_x_temp");
          Bluetooth.println(m2_x_temp);

          m2x_reset = !m2x_reset;
        }

        if (x2_reset) {

          m2_x_temp = -(Serial.parseFloat() * 1931.01 / 1908.01 - pos);
          Bluetooth.print("m2_x_temp");
          Bluetooth.println(m2_x_temp);

          x2_reset = !x2_reset;
        }

        mouse2_x = m2_x_temp + Serial.parseFloat() * 1931.01 / 1908.01 - 10.25;

        break;

      case 'y':

        if (m2_reset) {

          m2_y_temp = -(Serial.parseFloat() + (mouse2_x * (0.02) / 100));
          Bluetooth.print("m2_y_temp");
          Bluetooth.println(m2_y_temp);

          m2_reset = !m2_reset;
        }
        mouse2_y = m2_y_temp + Serial.parseFloat() + (mouse2_x * (0.02) / 100) - 151.95;
        break;
    }
  }

  if (SerialBT.available()) {
    String data = SerialBT.readStringUntil('\n');  //read incoming data
    SerialBT.println(data);                        //print data
  }
  Optical1.writeReg(0x22, 0x40);
  Optical1.writeReg(RUN_DOWNSHIFT, 255);
  Optical1.writeReg(0x22, 0x00);

  // Check for incoming data
  count_x += Optical1.dx();  // Read the dX register and in/decrease X with that value
  count_y += Optical1.dy();  // Same thing for dY register.....

  center_x = (float)convertionToMM(count_x) - (float)11.28;
  center_y = (float)(convertionToMM(count_y) - 0.2 * convertionToMM(count_x) / 100) - (float)128.5;
  slopM = (mouse2_x - (float)center_x) / (mouse2_y - (float)center_y);
  ang = atan(slopM) * 180 / 3.14;

  //  if (millis() - timeSinceLastOutput >= 1000) {
  //    timeSinceLastOutput = millis();
  //
  //            Bluetooth.print("mousedata x:");
  //            Bluetooth.print(mouse2_x + 10.25);
  //            Bluetooth.print("y:");
  //            Bluetooth.println(mouse2_y + 151.95);
  //            Bluetooth.print("Center Point:");
  //            Bluetooth.print(center_x); Bluetooth.print(" , "); Bluetooth.println(center_y);
  //            Bluetooth.print("cycleCount:");
  //            Bluetooth.print(cycleCount);
  //            Bluetooth.print("   ");
  //            Bluetooth.print("Angle:");
  //            Bluetooth.println(ang);
  //  }

  if (Bluetooth.available() > 0) {
    inputFromBluetooth = Bluetooth.readString();

    String distance = getValue(inputFromBluetooth, ',', 0);
    String speedOfRobot = getValue(inputFromBluetooth, ',', 1);
    //String manualDeviation = getValue(inputFromBluetooth,',',2);
    //String manualDeviationDirection = getValue(inputFromBluetooth,',',3);
    String correctionSpeedStr = getValue(inputFromBluetooth, ',', 2);

    pos = distance.toInt();
    // pos = ((distance.toInt() * 2347) / 25.4); // to convert compare to count x 300mm == 11811
    wheelSpeed = speedOfRobot.toInt();
    correctionSpeed = correctionSpeedStr.toInt();
    if (pos == 0) {
      count_x = 1011.27;
      count_y = 11520.18;
      X = 0;
      Y = 0;
      m = 0;
      m2_reset = !m2_reset;
      m2x_reset = !m2x_reset;
    }

    if (pos > 0) {  //start from here when user will give any value in mm
      m = 2;
      Bluetooth.println("Forward direction start.....");
      tempAssignment = 2;
      isForward = true;
    }
  }

  if (m == 4) {
    moveSidewaysLeft();
  }
  if (m == 5) {
    moveSidewaysRight();
  }
  if (m == 2) {
    moveForward();
  }
  if (m == 7) {
    moveBackward();
  }
  if (m == 3) {
    moveRightForward();
  }
  if (m == 1) {
    moveLeftForward();
  }
  if (m == 8) {
    moveRightBackward();
  }
  if (m == 6) {
    moveLeftBackward();
  }
  if (m == 9) {
    rotateLeft();
  }
  if (m == 10) {
    rotateRight();
  }

  if (m == 0) {
    stopMoving();
  }
  // If button "SAVE" is pressed
  if (m == 12) {
    if (index1 == 0) {
      LeftBackWheel.setCurrentPosition(0);
      LeftFrontWheel.setCurrentPosition(0);
      RightBackWheel.setCurrentPosition(0);
      RightFrontWheel.setCurrentPosition(0);
    }
    lbw[index1] = LeftBackWheel.currentPosition();  // save position into the array
    lfw[index1] = LeftFrontWheel.currentPosition();
    rbw[index1] = RightBackWheel.currentPosition();
    rfw[index1] = RightFrontWheel.currentPosition();
    index1++;  // Increase the array index1
    m = 0;
    tempAssignment = 0;
  }
  if (m == 14) {
    //    runSteps();
    if (dataIn != 14) {
      stopMoving();
      memset(lbw, 0, sizeof(lbw));  // Clear the array data to 0
      memset(lfw, 0, sizeof(lfw));
      memset(rbw, 0, sizeof(rbw));
      memset(rfw, 0, sizeof(rfw));
      index1 = 0;  // index1 to 0
    }
  }
  //y correction during x travel....................................................................................................
  //m==5 sideway right m==4 sideway left
  if (!isManualDeviation) {
    float result = orgY - center_y;
    if ((m == 2 || m == 7) && pos > 0 && result >= 2) {
      m = 4;  //to left
      tempAssignment = 4;
    }

    if (m == 4 && center_y >= 0) {
      if (ang > 5.36) {
        //isForward = !isForward;
        m = 9;  //left rotate
        tempAssignment = 9;
        Bluetooth.println("Left rotate.....");
      }
      if (ang < 2.36) {
        //isForward = !isForward;
        m = 10;  //right rotate
        tempAssignment = 10;
        Bluetooth.println("Right rotate.....");
      }

      if (ang >= 2.36 && ang <= 5.36) {
        //         count_y = 10579.98;
        //         m2_reset=!m2_reset;
        Bluetooth.println("Tilt corrected from right.....");
        m = isForward ? 2 : 7;
        tempAssignment = m;
        isManualDeviation = false;
      }
    }

    //To right Y deviation...........................................................
    if ((m == 2 || m == 7) && pos > 0 && result <= -2) {
      m = 5;  // 5 to right
      tempAssignment = 5;
    }
    if (m == 5 && center_y <= 0) {
      if (ang > 5.36) {
        m = 9;  //lefft rotate
        tempAssignment = 9;
        Bluetooth.println("Left rotate.....2");
      }
      if (ang < 2.36) {
        m = 10;  //right rotate
        tempAssignment = 10;
        Bluetooth.println("Right rotate.....2");
      }
      if (ang > 2.36 && ang < 5.36) {
        //        count_y = 10579.98;
        //        m2_reset=!m2_reset;
        Bluetooth.println("Tilt corrected from left.....2");
        m = isForward ? 2 : 7;
        tempAssignment = m;
        isManualDeviation = false;
      }
    }
  }

  //  To manage slope..............................................
  if (IsAfterYOffset) {
    float AR = ang - 3.86;

    if ((m == 4 || m == 5) && abs(AR) > 2 && IsAfterYOffset) {
      Bluetooth.print("AR start:");
      Bluetooth.println(AR);
      if (AR > 1.5) {
        m = 9;  //left rotate
        tempAssignment = 9;
      }

      if (AR < -1.5) {
        m = 10;  //right rotate
        tempAssignment = 10;
      }
    }

    if ((m == 4 || m == 5) && abs(AR) < 2 && IsAfterYOffset) {
      Bluetooth.print("AR not required....");
      Bluetooth.println(AR);
      isForward = !isForward;
      count_y = 11520.18 + 0.2 * (count_x) / 100;
      m2_reset = !m2_reset;
      delay(1000);
      m = isForward ? 2 : 7;
      tempAssignment = m;
      isManualDeviation = false;
      IsAfterYOffset = false;
    }

    if ((m == 9 && ang <= 5.36) || (m == 10 && ang >= 2.36) && IsAfterYOffset) {
      isForward = !isForward;
      Bluetooth.println("AR Offset done.........");
      Bluetooth.println("X direction Start.....");
      count_y = 11520.18 + 0.2 * (count_x) / 100;
      delay(1000);
      m2_reset = !m2_reset;
      m = isForward ? 2 : 7;
      tempAssignment = m;
      isManualDeviation = false;
      IsAfterYOffset = false;
    }
  }

  // *********************************************************************************************************************************
  if ((m == 9 && ang <= 5.36) || (m == 10 && ang >= 2.36) && !IsAfterYOffset) {
    delay(1000);
    Bluetooth.println("Rotation done.....");
    m = isForward ? 2 : 7;
    tempAssignment = m;
    isManualDeviation = false;
  }

  //Reverse to specif distance.................................................................
  if (m == 2 && abs(center_x) > pos) {
    Bluetooth.println("Front Offset start......");
    m = 0;
    Bluetooth.print("mousedata x:");
    Bluetooth.print(mouse2_x + 10.25);
    Bluetooth.print("y:");
    Bluetooth.println(mouse2_y + 151.95);
    Bluetooth.print("Center Poinlt:");
    Bluetooth.print(center_x);
    Bluetooth.print(" , ");
    Bluetooth.println(center_y);
    Bluetooth.print("cycleCount:");
    Bluetooth.print(cycleCount);
    Bluetooth.print("   ");
    Bluetooth.print("Angle:");
    Bluetooth.println(ang);
    delay(1000);
    m = isRight ? 5 : 4;
    tempAssignment = m;
    isManualDeviation = true;
    //    isCorrection = true;
    //    isFromReverse = false;
    IsAfterYOffset = false;
  }

  if (m == 7 && (center_x >= 0.1 && center_x <= 0.2)) {
    Bluetooth.println(".......ZERO DONE.......");
    m2x_reset = !m2x_reset;
    count_y = 11520.18 + 0.2 * (count_x) / 100;
    m2_reset = !m2_reset;
    delay(100);
  }
  //  if (m == 2 && (center_x >= (pos - 0.2) && center_x <= (pos - 0.1))) {
  //    Bluetooth.println(".......Front ZERO DONE.......");
  //    count_y = 11520.18 + 0.2 * (count_x) / 100;
  //    m2_reset = !m2_reset;
  //    x2_reset = true;
  //    delay (100);
  //  }

  if (m == 5 && center_y <= -10) {
    Bluetooth.println("Y Offset done.....");
    Bluetooth.println(center_y);
    Bluetooth.print("mousedata x:");
    Bluetooth.print(mouse2_x + 10.25);
    Bluetooth.print("y:");
    Bluetooth.println(mouse2_y + 151.95);

    Bluetooth.print("Center Point:");
    Bluetooth.print(center_x);
    Bluetooth.print(" , ");
    Bluetooth.println(center_y);
    Bluetooth.print("cycleCount:");
    Bluetooth.print(cycleCount);
    Bluetooth.print("   ");
    Bluetooth.print("Angle:");
    Bluetooth.println(ang);
    IsAfterYOffset = !IsAfterYOffset;
    // count_y = 11520.18 + 0.2 * (count_x) / 100;
    // m2_reset = !m2_reset;
  }

  if (m == 4 && center_y >= 10) {
    Bluetooth.println("Y(mm): ");
    Bluetooth.println("Y Offset done.....");
    Bluetooth.println(center_y);
    Bluetooth.print("mousedata x:");
    Bluetooth.print(mouse2_x + 10.25);
    Bluetooth.print("y:");
    Bluetooth.println(mouse2_y + 151.95);

    Bluetooth.print("Center Point:");
    Bluetooth.print(center_x);
    Bluetooth.print(" , ");
    Bluetooth.println(center_y);
    Bluetooth.print("cycleCount:");
    Bluetooth.print(cycleCount);
    Bluetooth.print("   ");
    Bluetooth.print("Angle:");
    Bluetooth.println(ang);
    isForward = !isForward;
    count_y = 11520.18 + 0.2 * (count_x) / 100;
    m2_reset = !m2_reset;
    m = isForward ? 2 : 7;
    tempAssignment = m;

    isManualDeviation = false;
    //isRestrictX = false;
    isCorrection = false;
    isRestrictReverse = true;
  }

  //Forward from specific distance............................................................................
  if (m == 7 && (center_x) <= 0.1 && isRestrictReverse) {  //&& !isRestrictX
    Bluetooth.println("Back Offset start.....");
    cycleCount++;
    //    isForward = false;
    m = 0;
    delay(100);
    Bluetooth.print("mousedata x:");
    Bluetooth.print(mouse2_x + 10.25);
    Bluetooth.print("y:");
    Bluetooth.println(mouse2_y + 151.95);

    Bluetooth.print("Center Point:");
    Bluetooth.print(center_x);
    Bluetooth.print(" , ");
    Bluetooth.println(center_y);
    Bluetooth.print("cycleCount:");
    Bluetooth.print(cycleCount);
    Bluetooth.print("   ");
    Bluetooth.print("Angle:");
    Bluetooth.println(ang);
    delay(1000);
    m = isRight ? 5 : 4;  //to right or left
    tempAssignment = m;

    isManualDeviation = true;  // to restrict 3 mm while turn right or left
    //    isCorrection = true; // to manage forward and reverse between 5 mm
    //    isFromReverse = true; // to resolve conflict add this flag
    //    isRestrictReverse = false; // to resolve conflict while manage forward and reverse of 5mm
    IsAfterYOffset = false;
  }

  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();
}
//............................................................................................................
void moveForward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveBackward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveSidewaysRight() {
  LeftFrontWheel.setSpeed(-correctionSpeed);
  LeftBackWheel.setSpeed(correctionSpeed);
  RightFrontWheel.setSpeed(-correctionSpeed);
  RightBackWheel.setSpeed(correctionSpeed);
}
void moveSidewaysLeft() {
  LeftFrontWheel.setSpeed(correctionSpeed);
  LeftBackWheel.setSpeed(-correctionSpeed);
  RightFrontWheel.setSpeed(correctionSpeed);
  RightBackWheel.setSpeed(-correctionSpeed);
}
void rotateLeft() {
  // Bluetooth.println("inside11");
  LeftFrontWheel.setSpeed(wheelSpeed/5);
  LeftBackWheel.setSpeed(wheelSpeed/5);
  RightFrontWheel.setSpeed(wheelSpeed/5);
  RightBackWheel.setSpeed(wheelSpeed/5);
}
void rotateRight() {
  LeftFrontWheel.setSpeed(-wheelSpeed/5);
  LeftBackWheel.setSpeed(-wheelSpeed/5);
  RightFrontWheel.setSpeed(-wheelSpeed/5);
  RightBackWheel.setSpeed(-wheelSpeed/5);
}
void moveRightForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveRightBackward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftForward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void stopMoving() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(0);
}
String getValue(String data, char separator, int index) {
  int found = 0;
  int strIndex[] = { 0, -1 };
  int maxIndex = data.length() - 1;

  for (int i = 0; i <= maxIndex && found <= index; i++) {
    if (data.charAt(i) == separator || i == maxIndex) {
      found++;
      strIndex[0] = strIndex[1] + 1;
      strIndex[1] = (i == maxIndex) ? i + 1 : i;
    }
  }

  return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}
$\endgroup$
10
  • 1
    $\begingroup$ Welcome to Robotics Sem Paneliya, but I'm afraid that it is not clear what you are asking. We prefer practical, answerable questions based on actual problems that you face, so it's a good idea to include details of what you want to achieve, what you tried, what you saw & what you expected to see. Please take a look at How to Ask & tour for more information on how stack exchange works and work through the Robotics question checklist to edit your question to make it clearer. $\endgroup$
    – Chuck
    Jun 2, 2023 at 7:04
  • $\begingroup$ You're saying that you have sensors to measure distance and rotation, but you don't mention what you're doing with that information. When you say the mecanum wheel creates error while running straight and specially side ways what do you mean? How much error, and how are you measuring it? Are you acting on the error? It sounds like you need a feedback controller but it's not clear from your post what (if anything) you're doing with the feedback from the mouse sensors. $\endgroup$
    – Chuck
    Jun 2, 2023 at 7:08
  • $\begingroup$ The mouse sensors are giving values to esp32. esp32 runs stepper motor based on that data. For example, if mouse gives 1000mm distance esp32 will stop the motor to run the robot in forward direction and start moving in the sideways(right). mouse sensor are used to tell esp32 the position of robot like feedback system. However that is not matching with robot's practical position. $\endgroup$
    – Sem
    Jun 2, 2023 at 7:31
  • $\begingroup$ However that is not matching with robot's practical position This sounds like a calibration issue to me then. If the mouse says the robot has gone 1000 mm but you measure it and it's actually gone 1200 mm, then you modify the mouse readings by (1200/1000) to convert the reported values to actual values. I would also expect this to be bi-directional, though - meaning if it's wrong moving forward then it would be wrong the same amount moving backwards, so as long as the mouse sensor is reporting consistently you should return to the same starting point if you go out and back. Is this the case? $\endgroup$
    – Chuck
    Jun 2, 2023 at 7:36
  • 1
    $\begingroup$ okay, this could be valuable information. I will consider it and update the status. thank you for the help. $\endgroup$
    – Sem
    Jun 2, 2023 at 7:51

1 Answer 1

0
$\begingroup$

I'll be honest here, this code is really hard to look at. There are lots of magic numbers, the flow isn't clear, and there's not a good use of functions. What follows here are some of the most glaring issues I've got with the code. I'm trying to provide a reasonably detailed response here in the hopes that you can clean up your code. Maybe on cleanup you can find contributing issues to the trouble you're having. As it stands, the code is difficult enough for me to read that I can't really provide any feedback on the control issues. I'd urge you to start with just moving to a target location. Once you can reliably move to a target position you can try queuing target positions to make a series of waypoints. The square pattern would just be a series of waypoints.

On to the code review. A big example of the difficult I have reading the code is the collection of motion functions. When I look a diagram like this (from here):

Mecanum wheel arrangement

I can't seem to get your code to agree with the layouts here, though. For example, your code for forward has the left wheels moving backward and the right wheels moving forward.

Looking at the graphic above, if you add (a) and (c) you get (e) - the forward left motors goes forward in (a), backwards in (c), and so it doesn't move in (e). Continuing around, you can see the forward right motor moves forward, back left moves forward, and the back right cancels. Then this makes sense, that forward + left = forward-left.

Again, when I try to sum your motions, forward and left do not equal forward-left. Even if the wheels weren't arranged the same as shown in the graphic, I would still expect linear combinations to work, that forward and left together should give the same result as the forwardleft motion.

That said, all of your motion functions could be simplified with the use of named arrays. Consider the following:

const uint forwardLeftIndex = 0;
const uint forwardRightIndex = 1;
const uint rearLeftIndex = 2;
const uint rearRightIndex = 3;
const int forward[4]{1, 1, 1, 1};
const int backwards[4]{-1, -1, -1, -1};
const int left[4]{-1, 1, 1, -1};
const int right[4]{1, -1, -1, 1};
const int clockwise[4]{1, -1, 1, -1};
const int counterclockwise[4]{-1, 1, -1, 1};

Then you can do something like:

void move(int wheelSpeed, int[] direction)
{
    LeftFrontWheel.setSpeed(wheelSpeed * direction[forwardLeftIndex]);
    LeftBackWheel.setSpeed(wheelSpeed * direction[rearLeftIndex]);
    RightFrontWheel.setSpeed(wheelSpeed * direction[forwardRightIndex]);
    RightBackWheel.setSpeed(wheelSpeed * direction[rearRightIndex]);
}

which should let you call the function like:

move(desiredSpeed, forward);

or

move(desiredSpeed, forward + left);

My concern with the code, as written, isn't that the move functions are near duplicates (though they are), it's that the speed isn't passed. Some are using wheelSpeed, some use wheelSpeed/5, some use correctedSpeed. It's not clear from the calling code which speed you're going to get.

The constants are difficult to look at, and there aren't units given for anything. I see (not exhaustive): 0.1, 0.2, 2.36, 3.86, 1.5, 1931.01, 1908.01, 10.25, 151.95, 11.28, 128.5, 2347, 25.4, 1011.27, 11520.18, and 2. It's not clear what all of these are for. I also don't understand some of the variables. There is a count_y and a count_x, which seem like two different things, but then count_y is a function of count_x?

The other difficult magic numbers for me are the possible values of m. If you named these values, like:

const int goForward = 5;

then you could do something like:

if(m == goForward)
{
    move(wheelSpeed, forward);
}

but instead there's code like:

if (m == 7) {
    moveBackward();
}

which makes it look like 7 is backwards, but then there's also code that says:

//Forward from specific distance............................................................................
if (m == 7 && (center_x) <= 0.1 && isRestrictReverse) { 

and then it's not clear if 7 is forward from specific distance or if 7 is backwards and you... move forward from specific distance when you're moving backwards?

There's also debug statements sprinkled throughout the code, and it would clean things up a lot if you just moved that to a printDebug() function:

Bluetooth.print("mousedata x:");
Bluetooth.print(mouse2_x + 10.25);
Bluetooth.print("y:");
Bluetooth.println(mouse2_y + 151.95);

Bluetooth.print("Center Point:");
Bluetooth.print(center_x);
Bluetooth.print(" , ");
Bluetooth.println(center_y);
Bluetooth.print("cycleCount:");
Bluetooth.print(cycleCount);
Bluetooth.print("   ");
Bluetooth.print("Angle:");
Bluetooth.println(ang);

Other things I don't understand are how the inputs are handled. For example:

// Check for incoming data
count_x += Optical1.dx();  // Read the dX register and in/decrease X with that value
count_y += Optical1.dy();  // Same thing for dY register.....

center_x = (float)convertionToMM(count_x) - (float)11.28;
center_y = (float)(convertionToMM(count_y) - 0.2 * convertionToMM(count_x) / 100) - (float)128.5;
slopM = (mouse2_x - (float)center_x) / (mouse2_y - (float)center_y);
ang = atan(slopM) * 180 / 3.14;

I can understand incrementing count_x and count_y, but then why is center_x only depending on count_x but center_y depends on count_y AND count_x? Typically a slope is dy/dx, so why is it the opposite here? What is the difference between the mouse2_x from the Serial Port and count_x or center_x from the Optical1?

If you could work through your code and replace the magic numbers with named constants, push the scaling operations into scaling methods, clean up the function calls, etc., then I think the code would be much, much easier to read, and if it's easier to read then it's easier to see where the behavior isn't what you intended. Ideally (IMO) you would work through the code until motion control happens in one and only one method. That method would accept current and target locations as parameters, and then you could test that one method for approaching targets from different locations, headings, etc.

$\endgroup$
1
  • $\begingroup$ I knew it will be difficult to understand the code. Well the code can be fully changed or completely new if you suggest. My concern is only with results. I started preparing code from basics and this is where i got. As i have very little time to complete this project, i need best way possible. Thank you for the suggestions. $\endgroup$
    – Sem
    Jun 6, 2023 at 12:22

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.