Lab sesion 4: Self-balancing robot

Mechanical considerations

The mechanical structure of the robot plays a major role in this experiment. The most important mechanical design guideline that we have followed is:

Create a stable structure that allows the robot to react faster to changes in the environment.

In order to achieve the objectives of stability and fast reaction, we have considered symmetry, weight, wheels and sensor position. These factors are explained in the following sections.

Robot symmetry and weight distribution

The robot symmetry is a key factor in this case. Since the robot is going to self maintain balanced just with two contact points with the surface, it should be constructed in a way in which the center of mass is in the center of the structure. Optimally, the lower the center of mass in this vertical line, the better. An ideal solution would be to analyze the weight of all the lego bricks involved in the construction and its distribution, in order to construct an optimized structure. However this can easily take a considerable amount of time, so we have decide to take as a base the structure proposed in [3].

Wheel position and type of wheels

The lego robot proposed in [3] can be seen in the figure above. It is using the wide curved wheels that appear on the left in the next picture. The wheel selection is highly important, since they are the contact point with the surface. As general considerations, it is worth to remark that:

• The wider the wheels the better, since the contact surface with the floor is greater.

• The greater the radius of the wheel the better. This has further implications and it is connected with the motor performance. Consider a wheel with radius r1 and a second wheel with radius r2 so r1 <>

• The greater distance between the wheels, the better. If the separation between the wheels is bigger, this will help distributing the weigh of the robot and it will gives more stability. With an optimized weight distribution this will not have that much repercussion, but in our case, it introduces an important improvement.

Taking in account previous factors we decide to try with different wheel types available at the lab (see following picture). An ideal wheel should have the width of the wheel type that appears on the center and the radius of the wheels on the right. Finally, we choose to construct our robot with the big wheels, since they were making the robot to react faster. This turned out to be a very important decision.

Sensor position

The light sensor is used to determine the distance to the surface. It is important to consider where the sensor is going to be positioned in order to improve robot performance. Before doing that, the basic mechanical movement of the robot should be analyzed.
The movement could be resumed with the following diagram. The optimal position gamma is in normal position (90º). Two angular limits (alpha and alpha prima) express the maximum inclination from which the robot is going to be able to restore the 90º position, thus to balance. If the inclination exceed those limits, the robot will fall. With an even distribution of weight and a constant environment, both limits should be the same. In our case we will assume they are.

Now, the question is ¿how can the sensor be positioned so a change in the inclination is detected as early as possible? ¿how can we get the best out of the sensor resolution? The answer is positioning the sensor as far as possible from the vertical central line of the robot. By doing so, the sensor will be getting close or far from the floor before the inclination of main robot body is beyond the recovery angles. Due to this reason we have modified the robot structure in order to position the sensor properly. We constructed in a way in which is possible to change the sensor distance to compare the robot performance in different cases. This arrangement can be seen in the following picture.

Final robot structure

Taking in account the previously exposed mechanical considerations, we have created the self-balancing robot. The final results can be seen in the following pictures:

Hardware considerations

Fully charged batteries

It is important to have fully charged batteries while operating the self-balancing robot. It is just under those conditions were the motors are able to deliver the appropriate power to move the robot fast enough.
Sensor types
We have identified three other kind of sensors that potentially can offer a better performance of the robot.
An high precision ultrasonic sensor could be used in order to measure the distance to the floor, rather than sensing the light reflection. This would allowed the robot to operate in any environment regardless the environmental light conditions. Of course, by using an ultrasonic sensor, other constraints are introduced: no other ultrasonic sources should be operating nearby (other robots, bats ...) otherwise they could cause erroneous readings.
The most reliable option would be to use mechanical sensors: gyroscopes or accelerometers. Both type of sensors will allow the robot to measure the inclination in a very precise way. Again, as in the ultrasonic case, other kind of constraints are introduced: if gyros or accelerometers are used, the robot should not be operated in an environment with a strong magnetic field.
The best sensing strategy would be to combine different kind of sensors in the same robot by applying the principles of redundancy and dual control.

Software considerations

As a starting point we have taken the PID based controller proposed in [insert reference]. This controller is using integers instead of floats. One of the first modifications we did was to change all the integer parameters to float data types, in order to have a more precise calculations.
Since we have altered the physical structure of the robot, the same PID values are not valid anymore, so we had to determine empirically the most appropriate values to control the robot.

The initial parameter were the following ones:

static final int KP = 28;
static final int KI = 4;
static final int KD = 33;
static final int SCALE = 18;

And a power value base of 55, used in the following expression:

int power = Math.abs(pid_val);
power = 55 + (power * 45) / 100;


The first change we introduced was to readjust the integral constant to a higher value. This was not a good approach. The integral term has got a direct implication in the angle that defined the recovery angle. If the integral term is higher, the recovery angle threshold is lower, so it is much harder to control the robot. We decided to keep this value constant after some trials. The second trial was to change the SCALE value, that determines to which extent the pid value is going to influence the power change to compensate the inclination.

float pid_val = (float)(KP * error + KI * int_error + KD * deriv_error) / SCALE;

So the scale value was redefined to 10, a value that we kept since the robot was balancing well, as it can be seen in the following video.

The last improvement we introduce was to change the power base figure. The power base is used to determine the required power depending on the PID value as follows:

int power = (int) Math.abs(pid_val);
power = 35 + (power * 45) / 100; // NORMALIZE POWER

By changing the power base, the robot movement is slower, so it is easier to balance. The results can be seen in the following video.

Conclusions

After completing this lab sessions, we have realized that it is highly important to create a good robot structure, with the robot purpose in mind. Mechanical aspects of the robot shall be considered during this stage and, even though we are not Mechanical Engineers, the most outstanding considerations are direct application of geometry and common sense.
We found out that is very difficult to determine the appropriate parameters for the PID controller empirically, without applying a systematic method.
Finally, it is important to analyze the relationship between the PID values and the physical repercussion in the actuators. Just by considering this relationship it is possible to improve the robot behaviour.

References

1. Ole Caprani - Lego Lab 4 http://legolab.daimi.au.dk/DigitalControl.dir/NXT/Lesson4.dir/Lesson.html
2. Steve Hassenplug Legway http://www.teamhassenplug.org/robots/legway/
3. Philippe Hurbain NXTway http://www.philohome.com/nxtway/nxtway.htm
4. Brian Bagnall, Maximum Lego NXTBuilding Robots with Java Brains
5. Gyroscope from Hi-technic http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NGY1044
6. Accelerometer from Hi-technic http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NAC1040
7. Final Sejway solution http://code.google.com/p/josemariakim/downloads/detail?name=Sejway.java&can=2&q=#makechanges

Lab session 3: Sound Sensor and Clap Controlled Car

Test of the Sound Sensor

The SoundSensor example is based on the SonicSensorTest, which is using the ultrasonic sensor to measure distance and displaying it on the screen. So in this case an instance of the SoundSensor class is created instead. We set a testing sampling time of 5ms. The source code of this example can be seen below.

SoundSensor s = new SoundSensor(SensorPort.S1);
int soundLevel;
LCD.drawString("Level: ", 0, 0);

while (! Button.ESCAPE.isPressed())
{
soundLevel = s.readValue();
LCD.drawInt(soundLevel,3,7,0);
Thread.sleep(5);
}

We have made some experiments to check the response of the microphone. We check how a constant sound source, moving away from the microphone, was attenuated so a smaller value was displayed on the screen.

One of the experiments we did was to check the response of the sound sensor to a variable high frequency noise. The microphone was maintained at a constant distance to the variable high frequency generator. The sound level displayed on the screen was decreasing while the frequency was increased, even though the amplitude was kept constant.

Sound Controlled Car

The program SountCtrCar.java is able to move the car robot around based on sound. The program is a mimiprogram with a sequential control as described by Fred G. Martin [1] page 190. The program performs a procedural series of steps moving the car robot around. It moves the car forward, turn right then left and finally stops when a loud sound is detected. It does it in a series of steps waiting 0.5 sec between each step. Each step is performed when the sound level is greater than a certain threshold.

The sound sensor is polled in the method waitForLoudSound as fast as possible. Only the preemptive JAVA implementation of the LeJOS NXJ will ensure that other tasks will be running. This busy polling is blocking the operation of the robot and it is not possible to stop the program with ESCAPE. The ButtonListener mechanism is added using a callback method that sets a flag indicating the program must be stopped exiting the busy polling loop in waiting for the loud sound.

Link to final version of SoundCtrCar.java

Clap Controlled Car

Our first intension was to make an algorithm based on Sivan Toledo's, [2], investigations on how the sound sensor can be used to detect claps. His method is based on detecting a pattern of 3. peaks as shown in the figure below.

Sivan Toledo's clap detection

Our approach was to design a general method that would detect a peak within a min and max amplitude in a given time interval defined by a start and end. The method lookForMax returns a peak every time a maximum is found in the input sequence of samples. Every sound sensor reading is performed in intervals of 5 ms given a sample time of 200 Hz.

                  

Code snippet - looking for max. peaks

Code snippet - looking for max. peak in interval based on amplitude and time

A state machine would be relevant to detect the clap. 3 states is defined to detect first peak A followed by peak B and C.

State machine detecting clap

After implementing the first version of the above algorithm we made some measurements of a clap using the data logger as described in the exercise. We have imported the text file recordings in Matlab and plotted the results in graphs as show in the following figures.

Below graphs are two recordings for a single clap with a sample rate of 200 Hz.

Single clap 1.

Single clap 2.

Only in the second reading we have a small peak after the clap peak, but it seems to be 1.75 seconds after the clap. We decided to test our algorithm on a double clap instead of a single clap.

Below graphs are two recordings for a double clap with a sample rate of 200 Hz.

Double clap 1.

Double clap 2.

Based on the above readings we changed the algorithm using the general peak detector loopForMaxInInterval. Peak 2 is expected in the time interval between 500 - 2000 ms after the first clap.

The below source code for the final state machine detecting a double clap. In this version we have added timeout detection returning to state 1 if the second peak isn't detected. The state machine could be extended so it would be possible to interrupt the double clap detection adding the ButtonListner mechanism to allow exiting the program with the ESCAPE button.

                                         Code snippet - state machine looking for double clap

We managed being able to start and stop the robot car based on a double clap state machine and peak detector.

The final code for the ClapDetecor can be found here: ClapDetector.java

References

[1], Fred G. Martin, Robotic Explorations

[2], Sivan Toledo, A Clap Counter, Oct 2006

Lab session 2:

After mounting the robot with the ultrasonic sensor on top we compiled and upload the SonicSensorTest.java test program. The purposed of this application was to analyze the response of the ultrasonic sensor provided with the lego box.

The first experiment consisted on checking the values read by the sensor while an object was located close to 254 cm. In this case the original SonicSensorTest application was used with no modifications. The sensor was able to detect an object at that distance if it was a solid surface. In that case we used the cover of the lego box. The sensor was retrieving the proper measurements.

If instead of using a solid surface we were using a jacket, the cloth was absorbing the ultrasonic waves without reflective them, breaking the working principle of the sensor (which was unable to detect the eco of the ultrasonic burst).

What is the time limit for the measurement?

The ultrasonic burst has to travel 2,5 meters until it reaches the object and goes back, covering a total distance of 5 meters. This implies that it will take a minimum time of 14,6 milliseconds (5m / 340.29 m/s = 0.0146 sec). This time is defining the theoretical limit.

In the original SonicSensorTest, the getDistance method is executed every 300 milliseconds, since the thread is executing the method Thread.sleep(300); inside the while loop. If we modify this figure to values closer to the theoretical limit we can analyse the response of the sensor. We found out that if this sleeping period is decreased to 30 ms, which is close to twice the time it takes the ultrasonic burst to reach the object and go back, the maximum reliable distance is decreased to 1 meter. Some additional results can be found in the following table.

Sleeping period [ms]	Maximum reliable distance [ms]
50	1,5
30	1
10	0,5

It can be concluded that

• The lower the sleeping time the faster the measurements turn out to be unreliable.
• There are additional factors that limit the usage of the ultrasonic sensor besides the sound propagation, like the signal adquisition, signal conversion, communication through the I2C bus between the sensor and the processor, etc…

Tracker Beam

The purpose of the tracker beam is to advance until it reaches a distance of 35 cm with an object, which will be kept during program execution.

At the beginning, as explained in the problem statement, the power to the car motors was the controlled variable and the distance was the measured variable. This kind of control is known as proportional control. In the initial application the Minimum power was set to 60 with a gain of 0,5 resulting on 3 to 4 cm oscillations when the robot was around the 35 cm distance.

The behaviour of the car can be modified if different values for the gain and minimum power are set. Some of the tests we run are the following ones:

Minimum power	Gain	Maintained distance [cm]
50	0,5	44
50	1	37

Additional integral control can be added to reduce the error between the maintained distance and the desired one. A third element, the differential term, can be added to control the overshooting, so oscillations would be reduced. If these three elements are combined, the controller is called PID controller (Proportional Integral Differential controller).
Wall Follower

We have implemented the a modified three-state wall follower algorithm. We have defined 4 different distance areas in where we change the behaviour of the car. The precondition for the algorithm is that the car is moving seing the wall at its right side.

Dist Area 4: (2 cm from wall) Car moves backward
Dist Area 3: (6 cm from wal) Car turns left
Dist Area 2: (12 cm from wall) Car moves forward
Dist Area 1: (40 cm from wall) Car turns rigth

In all the above distance areas the car motor is powered with at fixed value that ensures a steady slow speed.
When the distance is greater than area 4 it is using a proportional gain based on the distance it measures to the wall. The algorithm has not yet been tested since we ran out of battery and could not get the NXT working again. We will test it first time in the next lab exercise.

Update on Lab session 2 - 16th September

The NXT problem was solved by uploading the lejOS firmware again. Once the brick was reflashed, we continue working with the wall follower problem. We add some modifications to the source code, incorporating the suggestions from the exercises on page 179 [1].

The NQC program is first of all setting the sensors that the robot is going to use. Some small time delays are added so the different peripherals can be initialized. This can be seen in the following lines:

SetSensor(SENSOR_1,SENSOR_LIGHT);
Wait(100); // Charge GP2D12 sensor
SetUserDisplay(value,0); // Display distance (remove for RCX firmware 1.5)

SetSensor(SENSOR_1,SENSOR_TOUCH); // Activate sensor
Wait(5);
SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
value=SENSOR_1; // Get distance
SetSensor(SENSOR_1,SENSOR_LIGHT); // Re-cahrge sensor for next time
Wait(25);

The default action is to move forward left and right motors, so associated values to the defines MFW are assigned to motl and motor. Next step is to check if the robot is close or far away from the wall, and perform different corrections depending on how close or far it is. In the following code snippet it can be seen the actions corresponding to the "close to the wall case".

if (x>=XL1) // Close to the wall
{
if (x>=XL3)
{
motr=MREV; // Very close, turn in place
}
else if (x>=XL2)
{
motr=MOFF; // close enough, turn
}
else if (x>=XL1)
{
motr=MFLT; // a bit too close, shallow turn
}

}

The determined action in the previous conditional blocks is executed for each motor by using two switch statements (one per motor). As an example, it can be seen the first switch statement, that corresponds to the right motor:

switch(motr)
{
case MFWD:
OnFwd(OUT_C);
break;
case MFLT:
Float(OUT_C);
break;
case MOFF:
Off(OUT_C);
break;
case MREV:
OnRev(OUT_C);
break;
}

Implementation and partial modification in java.

In our java application we have defined different parameters that can be adjusted in order to implement different control strategies. These parameters are defining distance, power and sampling time.

int desiredDistance = 0, // cm to wall
minPower = 60,
slowPower = 20, // + minPower
maxPower = 90,
rightPower = 6, // Power difference to turn
leftPower = 3,
sampleTime = 50; // Time between reading sensor

Different thresholds for determining how close the robot is to the wall are defined. Depending on the comparison result between the error value and those thresholds, different actions will be performed.

int CloseWall1 = 30, // Turn Left
CloseWall2 = 20, // Forward
CloseWall3 = 15; // Turn Right

The main control loop is performing a moving average filtering, so it can compensate possible errors in the readings. This is done as follows:

distance = us.getDistance();
distance = distance/2 + last_distance/2;

In the case wall is detected the control strategy will evaluate three different cases, comparing the error figure with the distance to the wall. Depending on the comparison results, different actions will be performed, which could be turn right, move forward or turn let. In the case the error is greater than the expected, the motors will be controlled with the previously defined proportional gain and drive forward.
Finally this routing will be repeated with a period defined by the sampling time.

Final video of wall follower:
http://www.youtube.com/watch?v=TKmqrkfouvI

Lab session 1: Installing and LineFollower

Goals:

• build a simple robot using the provided instructions
• instal the LeJOS NXJ software [1]
• compile and run the program LineFollower.java [2]

---

Building the robot:

It took actually more time than expected to build the robot, as we had too many pieces of some kinds and too few of others... but it's always fun to play with Lego!

---

Installing LeJOS:

Installing Lejos has been quite problematic, in some of our machines.
Installing in Windows 7 was easy, but then the eclipse plugin wouldn't download and run the code in the NXT. It was solved using the "old fashioned way", which was described in a tutorial found in the internet [4].
Afterwards we found out that the eclipse plugin needed some extra configuration (NXJ home path), and then it worked as expected.
Installing in MAC has been, and still is a problem, even though some of our classmates managed to install and run from MAC.

---

LineFollower.java:

We compiled the program and downloaded it to the brick. We made it run over the black line circuit in the lab:

We tried to change the sample time to half of the original example in the main control loop. It seems only to change a little of the oscilation of the robot.

---

References:

[1] LeJOS, LeJOS Homepage
[2] Ole Caprani, LineFollower.java
[3] LeJOS installation instructions, link
[4] LeJOS + Eclipse tutorial, link

2010 Digital Control in a Physical World, Block created for group: José, Maria and Kim