When I got the Ultimate Builders set, it came with instructions for a plotter. Mind you, the RCX had no integrated rotation sensors, so it used a lever rotating against a touch sensor. It even used a complicated construction to drive a pneumatic pump with one motor.
It came with a program to write “LEGO”, which I did not understand, because it was huge. And it didn’t work either, but that turned out to be because of a broken sensor… after I took the plotter apart.
I just found the video of a “Lego Master builder” introducing the model.
Anyway, a plotter was on of the things on my list that had to happen properly someday. The one you see here is my third attempt. The first two where too fast, too bulky, and had a very feeble arm holding the pen.
I actually made this model before the NXTbike, but I messed up the building instructions, so I had to do it again. I can’t recommend Lego Digital Designer for any serious Technic building, use LDraw instead.
The first thing I did, even before my first attempt, was writing a virtual plotter, so I could quickly code up the alphabet. I wrote the software in Python, so that I could use the Turtle module for the virtual printer, and nxt-python for the real job. The software can be found on Github.
I made this model before I realized I should minimize the use of non-NXT parts. Ironically, this model uses a few parts from the Ultimate Builder set, but easy workarounds exist for most parts. Check the parts list before you buy.
Unfortunately, he does not show how to actually implement a PID controller, or how to tweak the values of the algorithm, so I thought I’d show you how it’s done.
For my robot, I chose the trike base by HiTechnic, because it is simple, and usable for my next program. The result:
If you are new to NBC, the main thing to remember that an action consist of a line, starting with the action, usually followed by the variable to store the result in, followed by other parameters.
add result 1 2
Another important concept are comments, which start with //. These are my notes about what the code does, to help you understand it.
If you want to know more about NBC, read this tutorial.
// Define to which ports the sensor
// and motors are connected
#define ULTRASONICSENSOR IN_4
#define motors OUT_AC
// Define constants to tweak the algorithm
#define kp 50
#define ki 12
#define kd 2
// And another one to scale the final value
#define scale 10
// target distance in cm
#define target 30
// From here to dseg ends are variable declarations
dseg segment
// Ultrasonic sensor reading
distance word
// The current error
err sdword
// The previous error
errold sdword
// The integral, all accumulated errors
errint sdword
// The deriviate, the expected next error
errdiff sdword
// Final pid value
pid sdword
// Temporary variable for calculations
temp sdword
dseg ends
// This is where the actual code starts
thread main
// Initialize the ultrasonic sensor
SetSensorUltrasonic(ULTRASONICSENSOR)
Forever:
// Read the sensor and store it in distance
ReadSensorUS(ULTRASONICSENSOR, distance)
// Substract the actual distance
// from the target for the current error
sub err target distance // Proportional
// Add the error to the integral
add errint errint err // Integral
mul errint errint 0.8 // multiply by 0.8 for damping
// Sunstract the previous error from error
// so that we get the speed
// at which the error changes
sub errdiff err errold // Derivative
// set the current error as he old error
mov errold err
mul pid err kp // Apply proportional parameter
mul temp errint ki // Apply integral parameter
add pid pid temp
mul temp errdiff kd // Apply derivative parameter
add pid pid temp
div pid, pid, scale // Apply scale
ClearScreen()
NumOut(0,0,pid)
NumOut(0,16,distance)
// saturate over 100 and under -100
brcmp LT, under100, pid, 100
mov pid, 100
under100:
brcmp GT, overMin100, pid, -100
mov pid, -100
overMin100:
// Turn the motors according to the scaled PID value.
OnRev(motors, pid)
jmp Forever
endt
If you have built a robot, and written the PID controller, the last thing you need to do is tweak the parameters on the lines that start with #define.
kp is multiplied by the proportial, this is where you start. Set the other two to zero, and this one to any value.
If the robot does not move, increase it. If the robot oscillates wildly, decrease it. Do this until it until it oscillates just a bit.
Now divide kp roughly in half, so that it does not oscillate, but stops to early. Now increase ki until it reaches the target as fast as needed. It will overshoot its target.
Finally, increase kd until it stops on target with as little oscillation as possible. You might need to go back and tweak the other parameters a bit.
I designed this robot to experiment with so-called “single track vehicle dynamics”, or in other words, balancing on a bike.
The challenge with designing a motorcycle like this is keeping he wheelbase short, and positioning the steering motor in a sturdy way.
The program for this robot is based on the principle of “steer into fall”, which means that if the bike leans over to the right, it needs to steer right to correct that.
A problem that I have with this robot is detecting the angle of the robot. The ultrasonic sensor is not precise enough and my floor not uniform enough to use the light sensor, like the NXTway does.
If you have a very uniform floor, you could use my code, but it is probably best to get a HiTechnic gyro with the software from this guy:
If you want to build this model, you need some extra wheels, check the parts list.
