+= Operator Not Functioning As Intended (only sets value to 0)

The situation: I’ve got a very simple robot that I’m using to try and program an odometry system. It consists of three old optical shaft encoders hooked up to 3.25" wheels, as well as an inertial sensor and, of course, a battery and brain. Its sole function is to be a training dummy to get an odometry system working under as stressful of conditions as possible. (Pic below)

Programming the robot is going fine, I’ve calculated all of the various thetas, arc lengths, etc. and have converted from local to global position change. On the final step, adding that change to the current position (robotX += globalXOffset), the code literally just breaks. Instead of adding said value, it simply sets it to 0 (Or, it seems, multiplies by a very small number, such that the result gets rounded to 0.)

Original Code (minus a lot of debugging features):

---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// inert                inertial      16              
// encR                 encoder       A, B            
// encL                 encoder       C, D            
// encB                 encoder       E, F            
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

float wheelCircumference = 10.21; // circumference (inches) of tracking wheels

float robotX = 0; // robot's X coordinate (inches) relative to the field
float robotY = 0; // robot's Y coordinate (inches) relative to the field
float robotH = 0; // robot's heading (degrees) relative to the field

float prevRobotH = 0; // robot's previous heading
float theta = 0;  // robot's change in angle since last iteration
float avgTheta = 0; // average between robot's previous heading and theta
float localXOffset = 0; // local offsets
float localYOffset = 0;
float globalXOffset = 0;  // global offsets
float globalYOffset = 0;

class trackingWheel {
  public:
  float radius=10; // distance (inches) between tracking wheel and tracking center
  float prev; // previous value (degrees) of encoder
  float delta; // distance (inches) encoder has measured since last iteration
  void calcDelta(float encVal){
    delta = wheelCircumference*(encVal-prev)/360;
    prev = encVal;
  }
}wheelL,wheelR,wheelB;

void reposition() {
  // calculate arc lengths for each of the wheels
  wheelL.calcDelta(encL.position(degrees));
  wheelR.calcDelta(encR.position(degrees));
  wheelB.calcDelta(encB.position(degrees));

  // calculate absolute heading and theta (change in heading)
  robotH = (wheelL.prev - wheelR.prev) * wheelCircumference / 360 / (wheelL.radius + wheelR.radius);
  theta = robotH - prevRobotH;
  prevRobotH = robotH;
  
  // calculate the local offsets
  localXOffset = 2 * sin(theta / 2) * (wheelB.delta / theta + wheelB.radius);
  localYOffset = 2 * sin(theta / 2) * (wheelR.delta / theta + wheelR.radius);

  // convert to global offsets
  avgTheta = robotH - theta / 2;
  globalXOffset = ((localYOffset * cos(avgTheta)) - (localXOffset * sin(avgTheta)));
  globalYOffset = (localXOffset * cos(avgTheta) + localYOffset * sin(avgTheta));

  // update position variables (AKA where things go wrong)
  robotX += globalXOffset;
  robotY += globalYOffset;
}

void preAuton() { // initializing sensors
  encL.setPosition(0,degrees);
  encR.setPosition(0,degrees);
  encB.setPosition(0,degrees);
  inert.calibrate();
  wait(100,msec);
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  preAuton();
  while(1) {
    reposition();
    Brain.Screen.clearScreen(); // for "debugging"
    Brain.Screen.setCursor(1,1);
    Brain.Screen.print(robotX);
    wait(10,msec);
  }
}

I’m almost positive some of the math is wrong, but that’s not the point here. By breaking apart each calculation involving robotX and printing the values involved both before and after the operation, I’ve been able to determine that it goes wrong when trying to add globalXOffset to robotX. globalXOffset is usually 0 in testing (makes sense - the wheels aren’t moving), so why code that should add 0 to robotX instead set it to 0?

code by the end of debugging modifications, for reference:

---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// inert                inertial      16              
// encR                 encoder       A, B            
// encL                 encoder       C, D            
// encB                 encoder       E, F            
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

float wheelCircumference = 10.21; // circumference (inches) of tracking wheels

float test1;
float test2;
float robotX = 5; // robot's X coordinate (inches) relative to the field
float robotY = 5; // robot's Y coordinate (inches) relative to the field
float robotH = 0; // robot's heading (degrees) relative to the field

float prevRobotH = 0;
float theta = 0;  // robot's change in angle since last iteration
float avgTheta = 0;
float arcLength = 0;  // length of arc robot has been traveling in since last iteration
float localXOffset = 0;
float localYOffset = 0;
float globalXOffset = 0;
float globalYOffset = 0;


class trackingWheel {
  public:
  float radius=10; // distance (inches) between tracking wheel and tracking center
  float prev; // previous value (degrees) of encoder
  float delta; // distance (inches) encoder has measured since last iteration
  void calcDelta(float encVal){
    delta = wheelCircumference*(encVal-prev)/360;
    prev = encVal;
  }
}wheelL,wheelR,wheelB;

void reposition() {
  Brain.Screen.setCursor(1,1);
  Brain.Screen.print(robotX);
  Brain.Screen.setCursor(2,1);
  Brain.Screen.print(robotY);
  wait(.5,sec);

  wheelL.calcDelta(encL.position(degrees));
  wheelR.calcDelta(encR.position(degrees));
  wheelB.calcDelta(encB.position(degrees));
  robotH = (wheelL.prev - wheelR.prev) * wheelCircumference / 360 / (wheelL.radius + wheelR.radius);
  theta = robotH - prevRobotH;
  prevRobotH = robotH;
  arcLength = wheelR.delta / theta - wheelR.radius;
  localXOffset = 2 * sin(theta / 2) * (wheelB.delta / theta + wheelB.radius);
  localYOffset = 2 * sin(theta / 2) * (wheelR.delta / theta + wheelR.radius);
  avgTheta = robotH - theta / 2;

  //Brain.Screen.clearScreen();
  Brain.Screen.setCursor(4,1);
  Brain.Screen.print(robotX);

  Brain.Screen.setCursor(5,1);
  Brain.Screen.print(robotY);
  wait(.5,sec);

  float testA = cos(avgTheta);// should be 1
  Brain.Screen.setCursor(1,10);
  Brain.Screen.print(testA);
  wait(.5,sec);

  float testB = sin(avgTheta);// should be 0
  Brain.Screen.setCursor(2,10);
  Brain.Screen.print(testB);
  wait(.5,sec);

  float testC = localXOffset * testB;// should be 0
  Brain.Screen.setCursor(3,10);
  Brain.Screen.print(testC);
  wait(.5,sec);

  float testD = localYOffset * testA;// should be 0
  Brain.Screen.setCursor(4,10);
  Brain.Screen.print(testD);
  wait(.5,sec);

  //test1 = ((localYOffset * cos(avgTheta)) - (localXOffset * sin(avgTheta)));
  test1 = testD - testC;// should be 0
  Brain.Screen.setCursor(5,10);
  Brain.Screen.print(test1);
  wait(.5,sec);

  test2 = (localXOffset * cos(avgTheta) + localYOffset * sin(avgTheta));
  Brain.Screen.setCursor(6,10);
  Brain.Screen.print(test2);
  wait(.5,sec);

  Brain.Screen.setCursor(7,1);
  Brain.Screen.print(robotX);
  wait(.5,sec);

  Brain.Screen.setCursor(8,1);
  Brain.Screen.print(robotY);
  wait(.5,sec);

  robotX += 1000000000000*test1;
  Brain.Screen.setCursor(10,1);
  Brain.Screen.print(robotX);
  wait(.5,sec);

  robotY += test2;
  Brain.Screen.setCursor(11,1);
  Brain.Screen.print(robotY);
  wait(.5,sec);
}

void debugScreen() {
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1,13);
  Brain.Screen.print("prev");
  Brain.Screen.setCursor(1,20);
  Brain.Screen.print("delt");
  Brain.Screen.setCursor(2,1);
  Brain.Screen.print("Left Wheel:");
  Brain.Screen.setCursor(2,13);
  Brain.Screen.print(wheelL.prev);
  Brain.Screen.setCursor(2,20);
  Brain.Screen.print(wheelL.delta);
  Brain.Screen.setCursor(3,1);
  Brain.Screen.print("Right Wheel:");
  Brain.Screen.setCursor(3,13);
  Brain.Screen.print(wheelR.prev);
  Brain.Screen.setCursor(3,20);
  Brain.Screen.print(wheelR.delta);
  Brain.Screen.setCursor(4,1);
  Brain.Screen.print("Back Wheel:");
  Brain.Screen.setCursor(4,13);
  Brain.Screen.print(wheelB.prev);
  Brain.Screen.setCursor(4,20);
  Brain.Screen.print(wheelB.delta);
  Brain.Screen.setCursor(6,4);
  Brain.Screen.print("Robot");
  Brain.Screen.setCursor(6,12);
  Brain.Screen.print("Inertial");
  Brain.Screen.setCursor(7,1);
  Brain.Screen.print("X:");
  Brain.Screen.setCursor(7,4);
  Brain.Screen.print(robotX);
  Brain.Screen.setCursor(7,12);
  Brain.Screen.print(inert.acceleration(xaxis));
  Brain.Screen.setCursor(8,1);
  Brain.Screen.print("Y:");
  Brain.Screen.setCursor(8,4);
  Brain.Screen.print(robotY);
  Brain.Screen.setCursor(8,12);
  Brain.Screen.print(inert.acceleration(yaxis));
  Brain.Screen.setCursor(9,1);
  Brain.Screen.print("H:");
  Brain.Screen.setCursor(9,4);
  Brain.Screen.print(robotH);
  Brain.Screen.setCursor(9,12);
  Brain.Screen.print(inert.heading(degrees)*0.01745);
}

void preAuton() {
  encL.setPosition(0,degrees);
  encR.setPosition(0,degrees);
  encB.setPosition(0,degrees);
  inert.calibrate();
  wait(100,msec);
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  //preAuton();
  while(1) {
    //Brain.Screen.clearScreen();
    Brain.Screen.setCursor(1,25);//should be 5
    Brain.Screen.print(robotX);
    wait(.5,sec);
    reposition();
  }
}

As you can see, the code does the same math, although there’s a fancy debug screen that’s being ignored in favor of a more relevant one. This debug screen prints values of robotX and robotY in pairs down the left side of the brain throughout the code, as well as the value of robotX before the function is called on the far right side of the brain. In between these values are the values of the math portions used, and each of them line up with what was predicted, showing that we really are just trying to add 0.00 to robotX, which was initialized as 5.
Picture for reference: first iteration of the loop:


Each of the pairs down the left side are 5, up until the last one (the one printed right after the += operator is used), which it then is shown as 0. on the next iteration of the code, the rest of the values on the debug screen update to 0:

I also tried not using the += operator, using robotX = robotX + test1; instead. It didn’t matter. Nothing I did accomplished anything but point towards that line of code as the problem. The name of the variable doesn’t matter. Doing robotX += 1; works as one would expect it to, and so does +=.1, +=.01, and +=10000000.

Ultimately, I have no clue why this is happening, and even copying the code to a separate VEXcode file doesn’t fix it. Does anyone have any idea as to why this is happening, and any solutions to fix it? I feel like I’ve tried practically everything (even got help from my brother who programs for a living), so what can I do?

3 Likes

I started odometry a little bit ago so I could be wrong, but when you are figuring out your theta for the heading, what if both (motor.prev) are the same(subtracting x by x)? Then you might be multiplying and dividing by zeros. I use the inertial sensor to find my heading, I put it into a similar algorithm to achieve my odometry. Take this with a grain of salt as I am fairly new to Odom.

Edit: Try printing your theta if you haven’t.
Edit 2: I quoted the wrong thing(I never post)

3 Likes

I think that you’re exactly correct! A theta value of 0 would result in the local offsets trying to divide by 0, possibly resulting in some sort of bit integer type limit, which registers as 0 on the brain. The X position would then add an absurdly large number to itself, causing the X position to overflow, thereby causing it to register as 0 as well. I’ll try adding the case statement for when theta = 0, and mark your answer as the solution if that works! Thanks again!

3 Likes