Design
Basic designThe sweeper is built with Lego Mindstorms set. It uses two servo motors, one on each side. The NXT intelligent brick is located in the middle of the vehicle and is placed very low to ease the attachment of a rag. The brick is the brain of the vehicle, it uses measurements from sensors to decide next movement steps. Figure 1 to 3 show the sweeper without a rag, from different views.
Figure 1. Top view of the sweeper
|
Figure 2. Front view of the
sweeper
|
Figure 3. Bottom view of the
sweeper
|
Sensors
The sweeper has two touch sensors in front of the chassis, used to detect obstacles. When any touch sensor is pressed, the sweeper stops, move backwards for a short distance, and then chooses another direction randomly to go forward. On top of the sweeper, there is a compass sensor, used to detect if the sweeper is stuck in a corner or a dead-end street. The compass measures the direction (0-360) of the movement of the sweeper. If the direction has little changes during a period (measures as the number of wheel rotations),
Attachment of cleaning rag
To conduct cleanning job, a rag is attached to the sweeper. Figure 4 to 6 show the sweeper with a rag attached.
Figure 4. Sweeper with rag, top
view
|
Figure 5. Sweeper with rag,
front view
|
Figure 6. Sweeper with rag,
bottom view
|
The following figure shows after cleanning, dust is accumulated on the rag.
Figure 7. Dusty rag
The video below shows how the sweeper runs. Notice how the sweeper gets stuck and then gets out of a dead-end street at 1:47 and 3:25.
The RobotC program
The following program controls the sweeper.
#pragma config(Sensor, S1,
touch1, sensorTouch) // Setup touch sensor
#pragma config(Sensor, S4, touch2, sensorTouch) // Setup touch sensor
#pragma config(Sensor, S3, HTMC, sensorI2CCustom) // Setup compass sensor
#include "drivers/HTMC-driver.h" // Include driver for the compass sensor
// Change the direction of the robot.
void changeDirection()
{
// Move backwards by setting the motor power to minus number.
// motorB and motorC are the two servo motors.
motor[motorB] = -100;
motor[motorC] = -100;
wait1Msec(1000); // Let the robot moves backwards for 1000 milleseconds.
// Randomly select a new direction.
if(random(2)) {
motor[motorB] = 100; // Set motorB to move forward by setting the power to a positive number.
} else {
motor[motorC] = 100;
}
// Let the robot to move for some random period. Since motorB and motorC are set to move in
// opposite directions, this time period decides the final direction of the robot.
wait1Msec(random (1000) + 100);
}
task main()
{
int heading = HTMCreadHeading(HTMC); // Read direction from the compass sensor.
int newHeading;
int headingDelta;
nMotorEncoder[motorB] = 0;
while(1==1) {
while(SensorValue(touch1)==0 && SensorValue(touch2)==0) { // Check if no touch sensor is pressed.
newHeading = HTMCreadHeading(HTMC); // Read direction from the compass sensor
headingDelta = abs(heading - newHeading); // Calculate the difference between an old direction and the new one.
// Display direction information.
eraseDisplay();
nxtDisplayTextLine(1, "Distance : %4d", nMotorEncoder[motorB]); // Read how many degrees motorB has rotated.
nxtDisplayTextLine(2, "Dir delta: %4d", headingDelta);
// Check if direction has not changed much.
if(nMotorEncoder[motorB] > 10000) {
if(headingDelta < 10) {
changeDirection();
}
heading = newHeading;
nMotorEncoder[motorB] = 0;
}
motor[motorB] = 100;
motor[motorC] = 100;
}
nMotorEncoder[motorB] = 0;
heading = HTMCreadHeading(HTMC);
}
}
#pragma config(Sensor, S4, touch2, sensorTouch) // Setup touch sensor
#pragma config(Sensor, S3, HTMC, sensorI2CCustom) // Setup compass sensor
#include "drivers/HTMC-driver.h" // Include driver for the compass sensor
// Change the direction of the robot.
void changeDirection()
{
// Move backwards by setting the motor power to minus number.
// motorB and motorC are the two servo motors.
motor[motorB] = -100;
motor[motorC] = -100;
wait1Msec(1000); // Let the robot moves backwards for 1000 milleseconds.
// Randomly select a new direction.
if(random(2)) {
motor[motorB] = 100; // Set motorB to move forward by setting the power to a positive number.
} else {
motor[motorC] = 100;
}
// Let the robot to move for some random period. Since motorB and motorC are set to move in
// opposite directions, this time period decides the final direction of the robot.
wait1Msec(random (1000) + 100);
}
task main()
{
int heading = HTMCreadHeading(HTMC); // Read direction from the compass sensor.
int newHeading;
int headingDelta;
nMotorEncoder[motorB] = 0;
while(1==1) {
while(SensorValue(touch1)==0 && SensorValue(touch2)==0) { // Check if no touch sensor is pressed.
newHeading = HTMCreadHeading(HTMC); // Read direction from the compass sensor
headingDelta = abs(heading - newHeading); // Calculate the difference between an old direction and the new one.
// Display direction information.
eraseDisplay();
nxtDisplayTextLine(1, "Distance : %4d", nMotorEncoder[motorB]); // Read how many degrees motorB has rotated.
nxtDisplayTextLine(2, "Dir delta: %4d", headingDelta);
// Check if direction has not changed much.
if(nMotorEncoder[motorB] > 10000) {
if(headingDelta < 10) {
changeDirection();
}
heading = newHeading;
nMotorEncoder[motorB] = 0;
}
motor[motorB] = 100;
motor[motorC] = 100;
}
nMotorEncoder[motorB] = 0;
heading = HTMCreadHeading(HTMC);
}
}