Part 16 – Ultrasonic Rangefinder and other sensors…

One of the first things that I leaned when testing the ultrasonic sensor was, it’s inconsistent. First, you really need to make sure that you let the robot “warm up” when you start your program, two seconds seems to work fine. Doing this allows the ultrasonic to have a chance to stabilize. If you don’t do this you can expect all kinds of weird results from the sensor. This same logic applies to turns as well, whenever the robot moves and then stops, it takes about 1/2 second for the information from the ultrasonic to stabilize.

On our robot, Norman and I decided that mounting the sensor low and keeping it as parallel to the ground as we could was going to give us the best results. Here is what it looked like mounted.

Here is the code that was written to test the sensor. The code was written with a lot of functions because it was going to be used later.

#pragma config(Sensor, in1,    irsensor1,      sensorDigitalIn)
#pragma config(Sensor, in2,    rightEncoder,   sensorRotation)
#pragma config(Sensor, in3,    leftEncoder,    sensorRotation)
#pragma config(Sensor, in5,    sonarSensor1,   sensorSONAR, int1)
#pragma config(Sensor, in6,    fanout,         sensorDigitalOut)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex269, openLoop)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard               !!*//

// We will use this array to store the last 10 sonar values.
//   Since the sonar returns weird variable some times, we need to normalize it.
//   The array will allow us to
int sensorvals[10];
int runstatus = 0;

/*  function: sonarval 
author: scott kerfoot and norman lok
This function is used to get the current sonar value. Instead of just reading the value from the sensor
an array sensorvals is used to store the last 10 sonar values and then we use the average of the last 10 
for the value of the sensor. We do this because of sensor bounce, where the sensor will occasionallly 
return numbers that seem too large or too small based on reality.
*/
int sonarval()
{
// What’s the current sonar value.
int jcnt = 0;
int returnval = 0;
int cursenvalmax = 0;
int cursenvalmin = 0;

// Get our current sonar value.
int cursonar = 0;
cursonar = SensorValue[sonarSensor1];
// Check to make sure the current sonar value is not that weird -1 value.
if (cursonar < 1)
{
cursonar = 125; // We’re far away, it probably returned -1
}
// Now make sure that the current value is not some wierd reading totally out of bounds.
cursenvalmax = sensorvals[0] + 5; // we only allow a +5 variance to the max.
cursenvalmin = sensorvals[0] – 5; // and a -5 variance.

if (cursonar > sensorvals[0] && cursonar > cursenvalmax)
{
// It changed too much over the last value, we only allow a +5 change.
cursonar = cursenvalmax;
} else if (cursonar < sensorvals[0] && cursonar < cursenvalmin)
{
// It changed too much to less than the last value, we only allow a -5 change.
cursonar = cursenvalmin;
}
// Move values 0 to 1, 1 to 2, etc…
for (jcnt=9;jcnt>0;jcnt–)
{
sensorvals[jcnt] = sensorvals[jcnt-1];
}
// Assign our current value to 0.
sensorvals[0] = cursonar;

// Now do our average to return.
cursonar = 0;
for (jcnt=0;jcnt<10;jcnt++)
{
cursonar = cursonar + sensorvals[jcnt];
}
cursonar = cursonar / 10;
// Assign our return value.
returnval = cursonar;
return returnval;
}

// Setup our robot environment.
void robosetup()
{
bMotorReflected[port2] = true;
bVexAutonomousMode = true;
// Initialize our sensorvalue tracking array.
wait1Msec(500);
int icnt = 0;
for (icnt=0;icnt<9;icnt++)
{
sensorvals[icnt] = 125;
}
// Normalize our sonar for the first time by getting 20 values.
for (icnt=0;icnt<50;icnt++)
{
sonarval();
wait1Msec(10);
}

}

/*  funciton: roboclear
author: scott kerfoot and norman lok
roboclear is used to clear the encoders.
*/
void roboclear()
{
SensorValue[rightEncoder] = 0;
SensorValue[leftEncoder] = 0;
}

/*  funciton: robostop
author: scott kerfoot and norman lok
robostop is used to turn off both motors and reset both encoders.
*/
void gostop()
{
motor[rightMotor] = 0;
motor[leftMotor] = 0;
roboclear();
wait1Msec(200);
}

/*  funciton: robomove
author: scott kerfoot and norman lok
robomove is used to move the robot forward at a particular speed for a particular count of the encodersl.
No ultrasonic is used with this move.
*/
void robomove(int motorspeed, int rawdistance) {
gostop(); // make sure we are stopped.
roboclear(); // do a final clear of the encoers.
while (SensorValue[rightEncoder] < rawdistance) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = motorspeed; // we have advanced the same with both wheels.
motor[leftMotor] = motorspeed;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = motorspeed; // Our right wheel has moved too much, slow down the left motor.
motor[leftMotor] = motorspeed -10;
} else {
motor[rightMotor] = motorspeed -10; // Our left wheel has moved too much, slow down the right motor.
motor[leftMotor] = motorspeed;
}
}
gostop();
}

/*  funciton: robomovesonal
author: scott kerfoot and norman lok
robomovesonar is used to move the robot forward AND make sure we go to within a specific distance. Different than
the robomove which moves a specific distance, this function moves to within a specific number of inches. Because
the normal speed is pretty fast, the delay between when we “stop” the motors and the actual distance will vary.
The motors slow down but don’t quick stop.
*/
void robomovesonar(int motorspeed,int distancego)
{
roboclear(); // clear the counters.
while (sonarval() < 0 || sonarval() > distancego) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = motorspeed; // we have advanced the same with both wheels.
motor[leftMotor] = motorspeed;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = motorspeed; // Our right wheel has moved too much, slow down the left motor.
motor[leftMotor] = motorspeed – 10;
} else {
motor[rightMotor] = motorspeed – 10; // Our left wheel has moved too much, slow down the right motor.
motor[leftMotor] = motorspeed;
}
}
}

/*  funciton: robomovesonal
author: scott kerfoot and norman lok
robomovesonar is used to move the robot forward AND make sure we go to within a specific distance. Different than
the robomove which moves a specific distance, this function moves to within a specific number of inches. Because
the normal speed is pretty fast, the delay between when we “stop” the motors and the actual distance will vary.
The motors slow down but don’t quick stop.
*/
void goforwardsonar(int distancein)
{

// We need to let the sonar settle after turns.
wait1Msec(1000);
// Define some variable we’ll need to move.
roboclear(); // Clear our sensor values.
//// I started writing some code for doing gradual acceleration, this code shows up in the finished product.
//// gradual acceleration to keep from slipping.
//if (sonarval() > 24) {
// for (icnt = 37;icnt < 127;icnt = icnt + 20)
// {
// robomovesonar(icnt,distancein+24);
// wait1Msec(40);
// }
// }
//// go fast in the beginning.
//robomovesonar(127,distancein+24);
// Start off at speed 80 so we don’t get a lot of wheel spin, do this until we get within 12″ of our target.
robomovesonar(80,distancein+12);
// Get within 6 inches, slow down even more.
robomovesonar(50,distancein+6);
// Now are are almost there, move slow for accuracy.
robomovesonar(35,distancein);
gostop();
}

void gobackward(int distance)
{
// Now move backware the requested number of ticks by just doing a negative speed.
robomove(-60,distance);
}

void goforward(int distance)
{
// Now move forwards the requested number of ticks.
// Originally there was a lot more code in here and I should probably go back and refactor this.
robomove(60,300);
}

void runfan() {
// This is a simple test version of the runfan function. Just used to denote that we’re done.
runstatus = 3;
SensorValue[fanout] = true;
wait1Msec(10000);
SensorValue[fanout] = false;
}

void goleft(int degrees) {
// This is where we turn left.
runstatus = 2;
int motorspeed = 50;
// need to figure out what a 90 degree turn looks like.
int clickstoturn = 0;
clickstoturn = degrees-10;

roboclear(); // Clear our sensor values.

while (SensorValue[rightEncoder] < clickstoturn || SensorValue[leftEncoder] < clickstoturn) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = motorspeed;
motor[leftMotor] = motorspeed*-1;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = motorspeed;
motor[leftMotor] = (motorspeed*-1) – 10;
} else {
motor[rightMotor] = motorspeed – 10;
motor[leftMotor] = (motorspeed*-1);
}
}
}

void goright(int degrees) {
// This is where we turn right.
runstatus = 2;
int motorspeed = 50;
// need to figure out what a 90 degree turn looks like.
int clickstoturn = 0;
clickstoturn = degrees- 10;

roboclear(); // Clear our sensor values.

while (SensorValue[rightEncoder] < clickstoturn || SensorValue[leftEncoder] < clickstoturn) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = motorspeed*-1;
motor[leftMotor] = motorspeed;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = motorspeed*-1;
motor[leftMotor] = motorspeed – 10;
} else {
motor[rightMotor] = (motorspeed*-1) – 10;
motor[leftMotor] = motorspeed;
}
}
}

task main()
{

// Run our setup routine
robosetup();
// Go foward until we are 5 inches from the sensor.
goforwardsonar(5);
// Run the fan to denote we got to the 5″ mark. 
runfan();
// Turn around
goright(180);
// Go forward toward the other wall, this time 10″.
goforwardsonar(10);
// Run the fan to denote we got to the mark. 
runfan();
// Turn around left this time.
goleft(180);
// Go forward toward the other wall, this time 7″.
goforwardsonar(7);
// Back up a specific number of clicks on the encoder.
gobackward(220);
// Do a victory spin.
goright(720);

}