The Competition

So, all that work and for various reasons, I was unable to make the drive to Penn State Saturday morning. This was very disappointing. However, I’m not going to stop working on it. There’s always next year, plus I have an available bedroom that can be converted into a testing stage for the firefighting competition.

The Night Before the Competition

As usual, this is filled with lots of last minute work. I hacked together a fan that should be able to put out any candle at a distance of about 6 inches using a propeller from a toy RC that had suffered way too many crashes into trees and had been cannibalized with the intention of building some sort of flying robot (which never happened). I had to hack into one of my NXT connector wires. . . ah well.

I tested the fan running with the robot standing still. It barely affects the robot at all. Now, all I have to do by morning is code up the planner. Pretty much the robot will travel on predefined paths through all the rooms. Once it finds the candle, it is to backtrack to a certain point, then follow predefined paths back to the home circle. The challenge is threefold:

1. Code the basic algorithm.
2. Test algorithm.
3. Create paths for competition.

I’ve already more or less outlined paths, however, the planner needs to be able to handle different states, different path types (paths that enter and exit rooms, when to be on the alert for candle detection, how to backtrack and return home, and probably a couple other things.

FieryTessaBot does Line Following

Unfortunately, only well on control surfaces. Here’s the background. Today, the Pitt robotics club was having a line following competition. Given that FieryTessaBot already has a light sensor, I figured some firefighting development time could be diverted towards line following. Besides, it can only help (my mind tends to solve one problem especially when I’m working on another). So, I grabbed another light sensor and worked on line following.

Now, my testbed was the NXT fold out oval that comes in the standard kit for testing line following robots. I also used it to calibrate the light sensors (though I knew further calibration would be needed under actual competition conditions). So, things went well in testing. Rarely, the robot would lose the line due to a too steep change in body angle (balancing) so there was code to attempt to compensate for that.

Actual competition time and things fall apart. On the (relatively) dirty parts of the floor with low amounts of overhead lighting, it ran well. On the clean (and very glossy) floors with lots of lighting, the reflections were too much for the light sensors to do a good job.

Adequate shielding would require that the sensors could either be kept really close to the ground or I build some sort of skirt around FieryTessaBot. So, as much as I’d have liked to kick ass, this goes down as just an adequate performance.

Making 90 degree turns

So, as I tested FieryTessaBot’s ability to turn to a given angle I discovered some bugs in my code. Once again, the type conversion problem hit me, this time in specifying the heading command from the PID loop. Before, I’d been testing it’s ability to drive straight, which managed to mask the issue. Anyway, I corrected, derived new PID gains, and then rechecked driving straight and wall following.

With turning, I could immediately see there would be issues. Since I only allow the robot angle to be within pi and -pi and wrap it around, things could get hairy if anytime I wanted a turn, I just changed the setpoint by 90 degrees. I will also need to wrap that angle around too. Then I got a better idea, since turning is a special occurance, I can have a special function to handle turns.

It will do the following:

• Assume robot starts at a heading of zero degrees.
• Set desired angle to 90 (or -90) degrees.
• Monitor error and turn command values until they approach zero (this means that we’ve completed the turn).
• Set robot heading back to zero.

This should handle any issues with turning. I’ve already decided that to calculate position, I’ll have some external monitor that determines whether I’m heading in one of the cardinal directions and uses the accumulated wheel distances in that direction to localize the robot.

Wall Following and Position Tracking

So, it appears that while angle calculations remain accurate, position does not. Note that I’m calculating position and angle using time stamped wheel count data in Matlab. Angle comes out more or less accurately, position does not. The reason is that the angle is changing too fast.  It would seem the best way to calculate position is to track roughly distance traveled at a given desired heading.

So:

• x = x0 + 0.5*(wL + wR)*sin(desired_hdg)
• y = yo + 0.5*(wL + wR)*cos(desired_hdg)

Now, the next step is to handle making 90 degree turns.

Position Tracking FIXED!

So, with suspicion falling on the sin and cos approximations, I decided it was time to examine the code line by line. . .and I immediately saw a problem. I’d implemented my angle corrections correctly in my matlab code on the PC. But I’d made a slight mistake on the code for FieryTessaBot. However, the true source of confusion was the data types available for user data to be sent via the logger. You can send signed 8 bit values.

That means values between -128 and 127. Since I had to convert angles to degrees to send, whenever my angle exceeded those values, I’d get weird results when I compared them with ones generated on the computer. Similar issue with x and y values. They’d be correct up to a certain point. I’d noticed a couple days ago that I needed to add or subtract 256 to make things correct, but it didn’t click in my mind until just now as I was looking at the code and thinking about type conversions.

Well. That’s done. Next up – driving straight and 90 degree turns. These two are very important for my robot. Even if I can’t accurately measure the distance I’ve traveled, I can use my other sensor data with the prior knowledge of my direction of travel and orientation to more or less accurately localize myself at discrete points.

I plan to utilize a PID controller to maintain angle or goto a desired heading (angle). Now that I’m in such a great mood, I’ll utilize this extra energy to think of what I want FieryTessaBot to do in the talent show!

A bit more details about the bot:

• Two ultrasonic distance measuring sensors – One forward facing to detect the forward walls and possibly the candle, one side facing for primarily wall following.
• One rate gyro – just used for balancing.
• nxtOSEK nxtway-gs API for balancing – runs every 4ms.
• My position and heading calculator – runs every 100ms: I’d determined through trail and error with wheel data in Matlab. I may run it twice or three times at fast depending on the performance of maintaining angle.
• One light sensor – used to detect doorways (white stripe) and candles (sits on white circle).
• Lego Servo motors – pwm controlled with built in optical encoders that give 360 counts/revolution.

What’s even better about developing PID heading control is that I can continue to log data. With just a command to drive forward, my response will essentially be the impulse response of the system. Then, I can analyze the plot of heading to make decisions as to how to set the proprotional, integral, and derivative gains. This will be a lot more robust that just observational trial and error! Of course, this will also require me to implement a logger that transmits appropriate data types so I don’t spend another two days figuring out a simple issue.

I expect that once I have working angle control, I can exploit it to do wall following by adding some constant to the integral error based on how far away I am from how close I want to follow the wall.

Ok, so I implemented P control of the angle. The following are plots of the robot’s position and angle. The desired angle is 90 degrees. As you can see, it fluctuates within 8 degrees of desired heading which means it more or less drives straight. Of course with full PID control, this would be a lot smoother (Visually, the robot drives straight, however, from the position plot, it appears the angle changes too quickly for the x and y position to keep up. Also, the third plot is a polar plot of the angle . . . just another visualization).

Hello world!

This is the chronicle of my entry to the Penn State Abington robot firefighting competition. It is modeled after the Trinity College robot firefighting competition but relaxes the rules a bit. . . and is free.  And this year is a couple weeks after Trinity which gives me time to actually get a robot more or less ready.

So, FieryTessaBot is a Lego Mindstorms NXT based balancing robot. The firmware has been flashed to run nxtOSEK, a port of a RTOS that provides APIs that expose the motors, sensors, LCD, and other peripherals of the NXT. I have a strange fascination with balancing robots. The issue with building your own out of random parts is that it takes longer to both build and program. That has been my downfall in years past. Not that I couldn’t mitigate this by working in the off season. . . but I’ve got other things to do as well.

So, what makes this a great choice for me? Well, Legos are quick to put together, there exist code for balancing robots on the Lego platform including specific code (pretty much an API) for one with nxtOSEK. Finally, with nxtOSEK, I can code in ANSI C!!

So, I’ve started. Built and tested the base balancing robot (nxtway_gs). Next, I need to implement the following:

1. On robot (relatively) accurate tracking of position and heading.
2. Robot ability to travel in a straight line.
3. Robot ability to make 90 degree turns.
4. Robot ability to wall follow.
5. Robot ability to detect doorway and candle (white strips/circles) via light sensor.

In fact, I’ve already implemented some form of wall following without doing #1. So far, #1 is tricky. Using the bluetooth data logger (another nxtOSEK API), I can get wheel counts and time. On my laptop, I’ve run that through the motion equations and get a pretty good picture of the robot’s motion. Then I tried to pretty much copy and paste that into the robot.

No go. Well, not quite. Once I worked out the bugs, it actually gives me pretty good values. But the strain of using sin and cos on floats on the NXT is apparently too much. It apparently locks up the NXT (which is running the balancing program, and bluetooth program as well). So, I found some sin and cos approximations, added a fix (since they were only good from -pi to pi), and tried that. Tests show that sometimes I get the correct angle, but my position is off. Or my angles are incorrect which totally throws off position.

Unfortunately, given the limited amount of information I can log over bluetooth (w/o making changes to the logger API), it’s slow going determining the exact cause of the issue. So far, this is what I think based on my tests and problems with code:

• GNUARM either does not do implicit type promotion or nxtOSEK has configured that not to occur.
• Comparisons (ie >, <, ==) between floats need to be done in some other manner.

Anyway, I’ll try to update this blog every couple days. Now that I have access to a digital camera (it will take me months to finally decide on one for myself), I can post up pictures (and probably screenshots of my matlab analysis).