# Create 2 Reading Sensor Values

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);
}

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

i = 0;

while (Serial.available()) {
}

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);
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");
}

}

#

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).

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;

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.

• I am not sure if Op Code 142 and Packet ID 20 is what I am looking for. However, I was looking into it and when I ran code to get a reading, I got nothing when trying to read the data. Roomba.write(142); Roomba.write(43); i = 0; while (Roomba.available()) { sensorData[i++] = Roomba.read(); Serial.print ("BYTE\n"); // <-- should at least print the word BYTE, but does not } delay(100); } P.S. I give on this formatting stuff. Nothing I type works there either...despite copying and pasting from on screen instructions. Jul 10 '16 at 20:24
• ...and before anybody starts getting critical over teh fact that I initially saido packet id 20 and then used ID 43 in my code, the same problem applies. I am not getting any data back at all. Why that is is what I am tying to figure out. I'll do the math after I get the data to do it with. Jul 10 '16 at 20:39
• First of all, I misread something in Arduino's serialSoftware setup that led me to use an improper pin for my RX line. That issue has been resolved. I am now getting sensor data. Yay! Secondly, the 142/20 op code/packet id combination (with a non-moving Roomba) returns a negative value. Since the Roomba is not moving, should the result be 0 until it starts moving? I try to look at other people's code to see how they did it (as I am more of a visual learner) and notice that nobody else accounts for rollover protection. An oversight? Looking at your code, I can see how this might come in handy. Jul 11 '16 at 3:37
• @TonyTzankoff - Formatting doesn't work (nicely) in comments. You can delineate code with the grave accent ( ` ) on either end. Generally, you should edit your question to incorporate new/updated information. Regarding your issue, the documentation says Op code 142 followed by packed ID 20 gets you the elapsed Angle (page 26). You need to accumulate those angles. If you are having a problem with the initial value being something other than zero, then discard it and poll again. The second (and subsequent) values should be zero if it didn't move. Jul 11 '16 at 13:09
• Also I appear to have not linked the manual from which I was quoting. The manual I quoted can be found here. There's not much difference, though. Jul 11 '16 at 13:12