Archives for category: Uncategorized

In my last post I did some experimenting with a low pass filter in Excel. But then there is reality. The US sensor is a slow sensor, I can’t use the sample rate I experimented with. I found out that RobotC, my programming environment, reads the US sensor about every 70 ms. This is a lot slower than the 4 ms I used in my experiment. As a result the filter responds far too slow as you can  see in the first image. FreeRover would hit a wall  ansd still be thinking it was over 20 cm away. If I increase Alpha the filter reacts faster but it will not be as effective as a filter. I needed something else.

The filter described in my last post and above is a first order filter, meaning it uses the current reading and the previous result. Second order filters also use the two previous readings. I started experimenting with a second order filter. Here is an image of the results.

Again, the red line represents raw readings, the green line is the result of a first order filter, the blue line is the result of a second order filter. Thetime axis has changed scale as I use a sample rate of once every 0,070 ms. There are five scenario’s. Scenario 1 and 3 are faulty readings of zero. Scenari 4 is a faulty reading of 255. I won’t go into much detail of these scenario’s, but notice that the second order filter is about as responsive as the first order filter but is more effective in filtering out spike errors.
Scenario 5 is the most interesting one. Here, the robot drives into a wall at a speed of 100 cm/second. This is quite fast for a robot, almost the speed of a walking man. Both filters respond with some delay, this means that the real distance to the wall is less then one might think based on filtered values. Upon hitting the wall the first order filter gives a distance of 5,5 cm, the second order gives 6,3 cm. The size of the lag can be altered by altering alpha and beta but will also depend on the speed of the robot.

I conclude that it is important to know the sensor, the kind of errors it gives and the sampling rate it can handle. It also is important to know the robot, how fast it will go. And it is important to tune the filter. This is best done in a spreadsheet at first.

Here is my code for a second order low pass filter for a US sensor.

```float lowPassUS;

{
int  deltaT=70;
float alpha=0.2, beta=0.18;
float US0=0,US1=0, US2=0;
// Start main loop of the task;
while(true)
{
wait1Msec(deltaT);
// low pass value of US sensor
US0=(float)SensorValue[sonar];
lowPassUS=US1+alpha*(US0-US1)+beta*(US0-US2);
US2=US1;
US1=lowPassUS;
}
}

```

Today I decided to enhance US sensor output. Why? Sometimes I had the impression the car changed behaviour. I suspected this had something to do with faulty readings from the Ultrasonic sensor. I decided to make a low pass filter for the US sensor. This should eliminate random faulty readings. But first I had to understand low pass filters. Coding a low pass filter isn’t too hard. There is an explanation on Wikipedia. Basicly it’s just one line of code:
lowPassUS=lowPassUS+alpha*(SensorValue[sonar]-lowPassUS);
The filtered value is calculated from the raw value and the previous filtered value. There is one parameter, alpha, that controls the filter. I wanted to understand how the value of alpha changes the behaviour of the filter. That is why I did some experiments in Excel. Here is an example. The red line represents unfiltered values, the green filtered values. The horizontal axis represents time.

The alpha factor does one thing, it controls how fast the filter adjusts to a new situation. The smaller the alpha, the quicker the filter reacts. In the plot it means that the green line follows the red line more closely. The downside is that less noise is filtered out.
There is one other thing that is important to realise. Alpha doesn’t control the time the filter takes to adjust, it controls the number of steps (iterations) the filter needs to adjust. This means that there is a relation between sample rate, how often the output from the US sensor is read, and alpha. With a higher sample rate and constant alpha, the filter adjusts quicker in real time. But also, if the sample rate is changes one also needs to consider to change the alpha.

The graph above shows four scenario’s. At t=1 the sampling starts, the filter needs time to adjust. One should give the filter time to settle, or, one should initialize the filtered value to the unfiltered value.
Around t=0,3 I tested another scenario. The reading of the Sensor changes because an object is in sight. In this example it takes about 0,1 second for the filter to adjust to the new situation. Is this quick enough, for me it is.
Around t=0,46 there is an faulty reading, the filter dampens the reading but for a short time (0,06 seconds) the objects seems further than it is.  In my case, The damping needs to be enough so that the filtered value  not exceeds treshhold. I need more real life testing to find out the damping is enough. Also I do know i get faulty 255 values sometimes, but I don’t know if I also get faulty zero values. These will affect behaviour more often, as they can influence collision detection.
Around t=0,6 the last scenario is shown.  Here an object at 60 cm dissappears and and a background object at 90 cm appears. But this time the objects doesn’t dissappear at once but during a short time it is sometimes sen and at other times it is not seen. Here the filter really smoothens out the jitter.

I wanted free rover to have front wheel drive.
Why? I might want to use odometry, the art of knowing where you are, later on. The path of front wheels is more easy to calculate than the path of the back wheels, so I wanted encoders on my front wheels to tell me how much they have rotated. Also, I wanted to explore the various aspects of front wheel drive.

I could have used a single motor for driving and a differential to divide power to both wheels. But I choose to drive each of the wheels with its own motor.
Why?
I want more power for driving, two motors give more power than one motor. Also, differentials are weak. Also, why use mechanics if there is software.

So, I developed a drive train where aech of the front wheels is driven by a motor. There is a 5/3 gear ratio in the drive train, this will make FreeRover a bit quicker. Both driving motors are on the outside and on top of the car. I think it looks nice.

As i discussed before, both front wheels rotate at different speed while cornering. This has to be reflected in motor speed. It can be calculated how much different the speeds of the motors should be, given the turning radius of the car. Turning radius itself can be calculated from the angle the front wheels make. And this can be calculated from the number of encoder ticks the steering moter is from its center position. The math isn’t comlicated if you know how basic trigonometry.

My program doesn’t set motor speed directly. Instead it uses a function that translates the desired car speed to left and right motor speed and sends this to the motors. The steering position is constantly monitored and when the angle of the steering changes corrections in motor speed are made.

Everything seems to work fine. Without corrections the inner wheel would jitter in corners. With corrections both wheels drive smoothly.

It is time I gave credit to RobotC. It has the opportunity to set motor speed. Normally you would set motor power, you would not know how much speed this would give you. Not in robotC though. This took a lot of work from me.

However there is one thing to consider. My software solution assumes a perfect world. The steering angle must be perfectly corrleted to the radius of the turn and so on. Well, this is not always the case. A car can slip for example. A differential doesn’t suffer from this. It reacts to a real world instead of to a mathematical world. So, unless I take real world measurements in account, my solution is inferior to a differential. For now it doesn’t take the real world into account. I’m still considering some ways to do so. Also, I have not yet decided if it is worth the trouble.

My steering geometry was square. The steering wheels were always parallel to each other. However, if a car is cornering the outside wheel is making a large circle, the inside wheel is making a small circle. So, the steering angle of the outside wheel should be less than that of the inside wheel. This can be achieved with a steering geometry called Ackerman steering geometry. See the image I took from Wikipedia.

I wanted Ackermann steering. This forced me to redesign the steering mechanism and to recalculate some of the cars properties. The biggest challenge turned was to make the design sturdy. On the run I also managed to get rid of some of my previous design errors. Although I introduced some new ones.

As both front wheel have a different angle this could further complicate calculations. To overcome these complications I now base all calculations on a fictional front wheel that is placed exactly between the two real ones and that has an angle that is the average of the to real ones.

Here are two new pictures of FreeRover. One showing the steering mechanism. The other showing FreeRover in its current state. You might notice the red handle bars. These are intentionally red as the remind me to use them while handling the car. Before that I used to grab the driving motors while carrying FreeRover. Sometimes this would cause the cables to disconnect.

I had to compromise on sturdyness and cornering. It turns more slowly than it used to. Was it worth it? I don’t know.

Last week I implemented autocalibration using pid control. I found out it worked well, but only if the miscalibration wasn’t too big. It worked to about half the maximum error. After some attempts to improve tuning I concluded it must be something else.
Pid controllers only work for lineair systems and my steering function isn’t. The output, rotational speed, is not linear related the the input, being number of encoder ticks for the steering motor.

This gave me two questions. Why does it work for small errors? And can i rewrite my steering function to be lineair?

I only have some suspicion about the answers to these questions. So if you have them please post’em.

I think my steering system behaves almost lineair if the steering angle is small. This being my answer to my first question.
I also believe it is possible to write a lineair steering function. But if an error factor is added it will no longer behave lineair. As the error in rotation speed is not lineair to the error in the steering.

So I concluded that my pid function only works for small errors and that is of no use to try to improve it.

And still, i love to see my car calibrate itself all the time. Also, I’ve managed to reduce the distance needed for calibration to aboout 25 cm.

For the steering to work correctly, the steering mechanism has to be calibrated. One needs to know when the steering is centered. The easiest way is to do it by hand, remove the 12 tooth pinion, center the steering and put it back on. I used this method until today.

Today, I had a day of from work and family. I used it to make the car calibrate its steering while driving.

Calibration is done everytime the car has to drive a straight line. It then uses it’s gyro and a PID controler to drive straight. If the steering is miscalibrated this will take some time. But once FreeRover drives straight, it stores the current encoder value as steering centre. Thus calibrating the steering.

Writing a PID controller is not very difficult, but it needs to be tuned before it works well. Tuning is difficult. It took me some time to tune the PID controller but now it’s working like a blast.

The autocalibration calibrates within a meter, but well before that it corrects for an offset steering. For FreeRover autocalibration is very usefull because the steering gets lots of strain from collisions, the front wheel drive system and design errors. As a result, it gets miscalibrated very easily.

Here is the robotC code dealing with autocalibration. Note that calibration is done if the absolute value of past errors is below a certain treshhold. This treshhold is then decreased to allow for even bettter calibration. The treshhold is reset every time the car starts driving straight.

```// PID controller using the gyro to tune the car for driving straight
{
float Kp=1.6, Ki=0.008, Kd=0.2, memory=0.6;
float error=0, integral=0, absIntegral=0, derivative, lastError=0, sumError, calibrationOffset=32;
int i, dT=80,x=0, q=0 ;
while (true)
{
wait1Msec(100);
calibrationOffset=32;
q=0;
while(state==STdrive)
{
// Read the gyro 20 times (to eliminate noise) and calculate the error;
sumError=0;
for (i=0;i&lt;20;i++)
{
wait1Msec(4);
}
error=(-sumError/20);

// Test again, for gyro loop took some time
if (state==STdrive)
{
// If the sum of last errors is below a treshold then the car is driving straight steadily.
// Calibrate the center steering
if (q++&gt;20 &amp;&amp; absIntegral&lt;calibrationOffset)
{
nMotorEncoder[steer]=0;
calibrationOffset=calibrationOffset/2;
}
else
{
// Calculate the values for PID
integral=memory*integral+(error*dT);
absIntegral=memory*absIntegral+(abs(error));
derivative=(error-lastError)/dT;
// calculate the steer correction value (it will be picked up by the steering task
steerCorrection=clip(Kp*error+Ki*integral+Kd*derivative,-80,80);
lastError=error;

// Plot the sum of the last errors.
/*
nxtEraseLine(x, 0, x, 50);
nxtSetPixel(x, absIntegral);
if (x++&gt;99) x=0;
nxtDrawLine(x, 0, x, 47);
nxtDisplayTextLine(0, &quot;E:%4.2f&quot;, error);
nxtDisplayTextLine(1, &quot;I:%4.2f&quot;, absIntegral);
*/
}
}
}
}
}

```

Steering of FreeRover is done by means of a rack and pinion system. The rack is made out of 2 worm gears. The pinion is a 12 tooth gear that is connected to a NXT motor through a gear train. I want FreeRover to be symetrical. That is why the NXT motor is placed along the length axis of the car.

There are two things I need to know . First, what is the maximum amount of rotation the NXT motor can make (number of encoder ticks) without the pininion (wheel gear) loosing touch with the rack. And second, what is the angle of the front wheels given the amount of rotation of the steering motor.

The maximum rate of freedom of the steering rod with the 2 worm gears is about 2 studs from the center to either left or right. At its maximum the pinion (12 tooth gear) begins to loose touch with the rack (2 worm gears), beond this point the pinion does not have any contact with the rack and all control over the steering is lost. That is why I need to know how much I can make the NXT motor rotate. Let’s start with the rack. Each worm wheel has 5 teeth (the singel spiral makes 5 turns around the axis), so 5 teeth is the maximum for the rack. The pinion has 12 teeth. It’s maximum rotation is 5/12. The pinion is connected to a 20 teeth gear, it’s maximum is also 5/12. This gear is driven by a 12 tooth gear. The maximum rotation of this gear is 20/12* 5/12. From there on, I used only 24 tooth gears to the motor, these can be ignored. So the maximum rotation of the motor equals 20/12*5/12*360 = 250 encoder ticks. If the steering mechanism is pointing straight forward and I rotate the motor for 250 ticks it will be pointing full left (or right).
A nice tutorial about lego gears can be found here.

Based on this calculation I coded a function that takes an angle as input and drives the steering motor accordingly.

I also want to know what angle the wheels are making given a number of encoder ticks. The length of the steering arm is given (2 studs). The steering rod goes one stud sideways for each 125 encoder ticks of the steering wheel. This gives us the length of two sides of an imaginary triangle, the hypothenuse (2) and the opposite (encoderticks / 125). The angle of the steering wheel equals the arc sinus of encoders ticks / 250.

I made a function that returns the angle of the front wheels.