Create 2 Reading Sensor Values

Question

I am trying to solve some Create 2 sensor reading problem that I am having when I came across @NBCKLY's posts (Part 1 and Part 2) that I believe are exactly what I am looking for. I copied his code from the original post into my project and updated the code from the second post as best as I could interpret...but something is not going according to plan.

For example, I am printing the angle to my serial monitor (for now) but I am constantly getting a value of 0 (sometimes 1).

Can @NBCKLY or anybody please check out this code and tell me what I'm doing wrong? I would appreciate it. Thank you very much.

int baudPin = 2;
int data;
bool flag;
int i;
int ledPin = 13;
int rxPin = 0;
signed char sensorData[4];
int txPin = 1;

unsigned long baudTimer = 240000;
unsigned long prevTimer = 0;
unsigned long thisTimer = 0;



void drive(signed short left, signed short right) {
  Serial.write(145);
  Serial.write(right >> 8);
  Serial.write(right & 0xFF);
  Serial.write(left >> 8);
  Serial.write(left & 0xFF);
}

void updateSensors() {
  Serial.write(149);
  Serial.write(2);
  Serial.write(43); // left encoder
  Serial.write(44); // right encoder
  delay(100);

  i = 0;

  while (Serial.available()) {
    sensorData[i++] = Serial.read();
  }

  int leftEncoder = int((sensorData[0] << 8)) | (int(sensorData[1]) & 0xFF);
  int rightEncoder = (int)(sensorData[2] << 8) | (int)(sensorData[3] & 0xFF);
  int angle = ((rightEncoder * 72 * 3.14 / 508.8) - (leftEncoder * 72 * 3.14 / 508.8)) / 235;

  Serial.print("\nAngle: ");
  Serial.print(angle);
  Serial.print("\n");
}



void setup() {
  pinMode(baudPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);

  delay(2000);
  Serial.begin(115200);

  digitalWrite(baudPin, LOW);
  delay(500);
  digitalWrite(baudPin, HIGH);
  delay(100);

  Serial.write(128);
  Serial.write(131);
  updateSensors();
  drive(50, -50);
}



void loop() {
  thisTimer = millis();

  if (thisTimer - prevTimer > baudTimer) {
    i = 0;
    prevTimer = thisTimer;
    digitalWrite(baudPin, LOW);
    delay(500);
    digitalWrite(baudPin, HIGH);
    Serial.print("Pulse sent...\n");
  }

  updateSensors();
}

#

What I am asking is why do I only get an angle of rotation of 0 or 1 degrees when the robot is moving in a circle. The angle should be incrementing while the robot is moving.

The output I am getting on the serial monitor shows a line of what looks like garble which I assume is supposed to be the bytes sent back from the Create which is followed by "Angle: 0 (or 1)" What I was expecting to see was an increasing angle value. (1,2,3...360, and so on).

Answer 1

First, I'll point out that OP code 20 will just give you the angle directly. Remember if you use that code then,

The value returned must be divided by 0.324056 to get degrees.

Regarding your code specifically, it looks like you're trying to do:

$$ \frac{\mbox{right distance} - \mbox{left distance}}{\mbox{wheel base}} \\ $$

This is a rearrangement of the arc length formula:

$$ s = r\theta \\ $$

where $s$ is the arc traversed (difference in wheel distances), $r$ is the radius of the circle (the wheel base), and $\theta$ is the angle traversed, in radians.

So, one of your problems is that you are using an int to define your angle value - int is short for integer, meaning that it can only be whole number. Radians are very small in magnitude compared to degrees. One complete circle is $2\pi$ radians, or about 6.28. Compare this to degrees, which is 360. This means that one radian is about 60 degrees; you won't get any update because of your unit definition.

Your biggest problem is, as NBCKLY asked about, the encoder counts that are returned to you from OP code 43 and 44 are not signed. So, when you your math, you are doing the following:

$$ \frac{ \frac{\mbox{right encoder}}{\mbox{counts per rev}} (\pi d) - \frac{\mbox{left encoder}}{\mbox{counts per rev}} (\pi d)}{\mbox{wheel base}} \\ $$

BUT, your drive commands are equal in magnitude. This means that $\mbox{right encoder}\approx \mbox{left encoder}$ because the encoder counts are not signed. That is, even though your drive command is negative, the counts come back as positive.

So, here's what you can do to fix this**:

1. Multiply the encoder counts by the sign (not sine) of the respective drive command.
2. Use a float for the angle and/or use degrees for the angle by multiplying your existing code by (180/3.1415).

** - A final note on your code: You're going to have trouble doing it the way you are at the moment because you're using total encoder counts for your math. Say you get to 16,000 counts and change direction on that motor. What happens? Well, the way I've written above (which fixes your current issue), you go from +16,000 to -16,000 (or vice-versa). What you should consider doing instead is to accumulate an angle and to evaluate only the elapsed wheel encoder counts. That is, I would do the following (pseudo-code):

float angle;
angle = 0;

<in your updateSensor code>
leftEncoder = <get left encoder counts>;
rightEncoder = <get right encoder counts>;

/* Rollover protection on the encoder */
if leftDriveCommand > 0
    if leftEncoder < prevLeftEncoder
        prevLeftEncoder = prevLeftEncoder - 65535;
    end
elseif leftDriveCommand < 0
    if leftEncoder > prevLeftEncoder
        prevLeftEncoder = prevLeftEncoder + 65535;
    end
end

<do the same thing for the right encoder>

elapsedLeft = leftEncoder - prevLeftEncoder;
prevLeftEncoder = leftEncoder;

elapsedRight = rightEncoder - prevRightEncoder;
prevRightEncoder = rightEncoder;

angleIncrement = (elapsedRight - elapsedLeft)*(pi*d/countsPerRev)/(wheelBase);

angle = angle + angleIncrement;

So in this way you accumulate your angle by looking at how much of an angle has elapsed since the last time you updated your sensor. In this way, you don't have to worry about what happens with encoder counts when you reverse polarity on a drive command. You should also consider what is going to happen when you roll over from 65535 back to 0.

Or again, just use OP code 20 to get the angle directly.