Complete Mt. Sac Fire Sensing Vex Robot Challenge

After doing the practice we had a little issue with round 1, but still were able to win. Here is a video of our last round win before the finals.

We made it to the finals and here was our decisive win. A lot of the delays between turns and searching were taken out of the code for the final and the Vex still performed really well.

Summary of some of the things we learned.

1. The tires pick up dust, the dust makes acceleration, stopping and turns pretty unreliable. Clean the wheels with a wipe often.
2. The inside of the encoders fill with dust. Remember, one piece of dust blocking one tick will make your turns 4 degrees off because each revolution is 90 ticks. Blow out the inside of the encoders often.
3. The older Vex batteries create inconsistent turns due to differing outputs. We spent and entire day debugging what turned out to be a battery weakness and dust in the encoders problem.
4. The Vex Pic environment does NOT allow floating point calculations. Whaaaaatttttt. You can’t even calculate simple percentages like 120% of X because it just doesn’t work. You have to find interesting ways to make that work with whole numbers.
5. Many of the tasks that the robot will perform require some “settling” time. Ex: When you turn left 90 and stop, if you try to measure the Ultrasonic distance it make give you funky readings. Just put a short wait in (>200msecs) and then measure and you get much more reliable results. AND definitely don’t forget to wait 2 seconds before doing anything. Things are really wonky on startup.
6. Once we started to get a handle on the delays, things like my custom sonval function weren’t needed because we should get a reliable value directly from the sensor.
7. Finding the balance of your motor powers is important. You can build it automatically into your forward and turn functions so that they are more consistent. Ex: The right motor was 15 more powerful than the left during medium speed. It’s best when you want to be precise to just go slow. Part of this may be due to….
8. Weight balance left to right will affect acceleration, deceleration, and turning. We had a really big battery on one side and a smaller battery on the other. I think that is what made it always want to pull to one side.

Finally, here is the magical software that made it all happened, about 500 lines of code. Pretty well commented and very reliable execution.

#pragma config(Sensor, in1,    irsensor1,      sensorReflection)
#pragma config(Sensor, in2,    irsensor2,      sensorReflection)
#pragma config(Sensor, in3,    rightEncoder,   sensorRotation)
#pragma config(Sensor, in4,    leftEncoder,    sensorRotation)
#pragma config(Sensor, in7,    sonarSensor1,   sensorSONAR, int1)
#pragma config(Sensor, in8,    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 TURNSPEED = 55;
int FORWARDSPEED = 60;
int SLOWSPEED = 21;

// Our global variables.
int sensorvals[10];
int runstatus = 0;

int icnt = 0;

int lastlt = 0;
int lastrt = 0;

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;
}
// Now make sure that the current value is not some wierd reading totall out of bounds.
cursenvalmax = sensorvals[0] + 5;
cursenvalmin = sensorvals[0] – 5;

if (cursonar > sensorvals[0] && cursonar > cursenvalmax)
{
// It changed too much over the last value, we only allow a 1.2 change.
cursonar = cursenvalmax;
} else if (cursonar < sensorvals[0] && cursonar < cursenvalmin)
{
// It changed too much to less than the last value, we only allow a .8 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;
}

void sonarnormalize() {
// Normalize our sonar for the first time by getting 20 valueiin1
int icnt = 0;
for (icnt=0;icnt<30;icnt++)
{
sonarval();
wait1Msec(10);
}
}

// Setup our robot environment.
void robosetup()
{
bMotorReflected[port2] = true;
bVexAutonomousMode = true;
SensorValue[fanout] = false;
// Initialize our sensorvalue tracking array.
wait1Msec(1000);
int icnt = 0;
for (icnt=0;icnt<9;icnt++)
{
sensorvals[icnt] = 125;
}
sonarnormalize();

}

void roboclear()
{
SensorValue[rightEncoder] = 0;
SensorValue[leftEncoder] = 0;
}

void gostop()
{
motor[rightMotor] = 0;
motor[leftMotor] = 0;
wait1Msec(1000);
roboclear();
}

void robomove(int motorspeed, int rawdistance) {
gostop();
roboclear();
while (SensorValue[rightEncoder] < rawdistance) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = motorspeed;
motor[leftMotor] = motorspeed;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = motorspeed;
motor[leftMotor] = motorspeed -15;
} else {
motor[rightMotor] = motorspeed -15;
motor[leftMotor] = motorspeed;
}
}
gostop();
}

void goforwardsonar(int distancego,int fwdspeed)
{
// Define some variable we’ll need to move.
roboclear(); // Clear our sensor values.
int adjspeed = 30;
if (fwdspeed < 30) {
adjspeed = 5;
}
int distanceaway = SensorValue[sonarSensor1] – distancego;
if (distanceaway > 12) {
for (icnt=30;icnt < 60;icnt=icnt+5) {
motor[rightMotor] = icnt;
motor[leftMotor] = icnt;
wait1Msec(15);
}
}

while (SensorValue[sonarSensor1] < 0 || SensorValue[sonarSensor1] > distancego) {

if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = fwdspeed+2;
motor[leftMotor] = fwdspeed;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = fwdspeed+adjspeed;
motor[leftMotor] = fwdspeed;
} else {
motor[rightMotor] = fwdspeed;
motor[leftMotor] = fwdspeed+adjspeed;
}
distanceaway = SensorValue[sonarSensor1] – distancego;
if (distanceaway < 5) {
fwdspeed = SLOWSPEED;
}

}
gostop();
roboclear();
// We need to let the sonar settle after turns.
wait1Msec(1000);
}

void gobackward(int distance)
{
// Now move backware the requested number of ticks.
robomove(-60,distance);
}

void goforward(int distance)
{
// Now move backware the requested number of ticks.
robomove(60,distance);
}

void runfan() {
/* function: runfan
creation: 2/14/2017
*/
// Set our runstatus for blowing out the candle.
runstatus = 6;
int fireout = 5;
bool onfire = true;
int firevals[10];
int fireavg = 999;
int icnt = 0;
for (icnt=0;icnt<10;icnt++) {
firevals[icnt]=999;
}
while (onfire) {
// Turn the fan on.
SensorValue[fanout] = true;
// Keep the fan running while we are detecting the flame.
while(fireavg > fireout) {
// Keep the fan on and re-calulate our fireaverage.
fireavg = 0;
for (icnt=9;icnt>0;icnt–) {
firevals[icnt] = firevals[icnt-1];
fireavg = fireavg + firevals[icnt];
}
firevals[0] = SensorValue[irsensor2];
fireavg = fireavg + firevals[0];
fireavg = fireavg / 10;
wait1Msec(500);
}
// We blew out the flame, turn off the fan.
SensorValue[fanout] = false;
// Wait for 4 seconds.
wait1Msec(4000);
// Are we still on fire?
if (SensorValue[irsensor2] <= fireout) {
onfire = false;
}
}

}

void goleft(int degrees) {
// Change our runstatus to a left turn.
runstatus = 2;
// Set our variable for how many clicks in each direction.
int clicksrt = 0;
if (degrees==90) {
clicksrt = 60;
} else if (degrees == 180) {
clicksrt = 133;
} else if (degrees == 360) {
clicksrt = 272;
} else if (degrees == 10) {
clicksrt = 5;
} else {
clicksrt = 32;
}
// Clear the sensorvalues.
roboclear();
// Turn until we get to our clicks…
while (SensorValue[rightEncoder] < clicksrt) {
if (SensorValue[rightEncoder] == SensorValue[leftEncoder]) {
motor[rightMotor] = TURNSPEED;
motor[leftMotor] = -TURNSPEED;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = TURNSPEED+12;
motor[leftMotor] = -TURNSPEED;
} else {
motor[rightMotor] = TURNSPEED;
motor[leftMotor] = -(TURNSPEED+15);
}
}
lastlt = SensorValue[rightEncoder];
lastrt = SensorValue[leftEncoder];
gostop();
sonarnormalize();
}

void goright(int degrees) {
runstatus = 2;
// Set our variable for how many clicks in each direction.
int clicksrt = 0;
if (degrees==90) {
clicksrt = 62;
} else if (degrees == 180) {
clicksrt = 133;
} else if (degrees == 360) {
clicksrt = 272;
} else if (degrees == 10) {
clicksrt = 1;
} else {
clicksrt = 32;
}

// Clear the sensorvalues.
roboclear();
// Turn until we get to our clicks…
while (SensorValue[rightEncoder] < clicksrt) {
if (SensorValue[rightEncoder] == SensorValue[leftEncoder]) {
motor[rightMotor] = -(TURNSPEED);
motor[leftMotor] = TURNSPEED;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = -(TURNSPEED+15);
motor[leftMotor] = TURNSPEED;
} else {
motor[rightMotor] = -TURNSPEED;
motor[leftMotor] = TURNSPEED+15;
}
}
lastlt = SensorValue[rightEncoder];
lastrt = SensorValue[leftEncoder];
gostop();
sonarnormalize();
}

void runbyremote() {
// He gets two minute at a time.
bIfiAutonomousMode = false;
int chan2 = 0;
int chan3 = 0;
int chan5 = 0;
motor[port2]=0;
motor[port3]=0;
ClearTimer(T1);
chan5 = vexRT[Ch5];
// while (time10(T1) < 12000 && vexRT[Ch5] == 0) {
while (true) {
motor[port3] = vexRT[Ch2]*-1;
motor[port2] = vexRT[Ch3]*-1;
//motor[port4] = vexRT[Ch1];
//motor[port5] = vexRT[Ch1]*-1;
if (vexRT[Ch6] > 10) {
motor[port4] = 100;
motor[port5] = -100;
} else if (vexRT[Ch6] < -10) {
motor[port4] = -40;
motor[port5] = 40;
} else {
motor[port4] = 0;
motor[port5] = 0;
}
chan2 = vexRT[Ch2];
chan3 = vexRT[Ch3];
chan5 = vexRT[Ch5];
}
motor[port2]=0;
motor[port3]=0;
bIfiAutonomousMode = true;
}

void findirft() {
int ircurrent = 0;
int irmax = 0;
// This is the variable we’ll use to track if we’re still searching.
bool searching = true;
goleft(10);
searching = true;
irmax = 0;
while (searching) {
wait1Msec(250);
ircurrent = SensorValue[irsensor2];
if (ircurrent < irmax-2) {
// We found it, stop here.
motor[rightMotor] = 0;
motor[leftMotor] = 0;
searching = false;
} else {
irmax = ircurrent;
goright(10);
}
}

}

void leftirdetect() {
// We need to move forward very slow while detecting fire on the left side.
// Define some variable we’ll need to move.
int distancego = 5;
int ircurrent = 0;
// This is the variable we’ll use to track if we’re still searching.
bool searching = true;
// The current max IR found.
int irmax = 0;
// The minimum allowed before we thing we’ve passed the fire.
int irmin = 190;
// Clear our encoders.
roboclear(); // Clear our sensor values.
// Now lets search to see if we can find the fire

while (searching) {
// Now lets see if we have found fire.
// Check to see if we are too close to the wall.
if (SensorValue[sonarSensor1] < distancego) {
// We got to the wall and didn’t see the fire, go back and try again.
gostop();
gobackward(450);
roboclear();
}
ircurrent = SensorValue[irsensor1];
if (ircurrent > irmax || ircurrent < irmin) {
// Our current value is greater than our previous max, we are still increasing.
irmax = ircurrent;
} else if (ircurrent > irmin && ircurrent < irmax-50) {
// Our current value is less than the minimum allowed, we must have passed the fire.
gostop();
searching = false;
}
if (searching) {
if (SensorValue[leftEncoder] == SensorValue[rightEncoder]) {
motor[rightMotor] = SLOWSPEED;
motor[leftMotor] = SLOWSPEED+2;
} else if (SensorValue[rightEncoder] < SensorValue[leftEncoder]) {
motor[rightMotor] = SLOWSPEED+9;
motor[leftMotor] = SLOWSPEED;
} else {
motor[rightMotor] = SLOWSPEED;
motor[leftMotor] = SLOWSPEED+15;
}
}

}
}

void frontirdetect() {
// Define some variable we’ll need to move.
int distancego = 2;

// Clear our encoders.
roboclear(); // Clear our sensor values.
// Let’s very carfully find out where we are going.

// Now lets search to see if we can find the fire
// First we’ll go until we are 8 from the fire.
if (SensorValue[sonarSensor1] > 18) {
goforward(50);
findirft();
}
// Check to see how close we are.
if (SensorValue[sonarSensor1] < 5) {
gobackward(30);
}
goforwardsonar(4,SLOWSPEED);
wait1Msec(1000);
// Now we are going to turn left, then slowly turn right until we find the fire.
findirft();
// At this point we should be facing the fire. Go forward until we are close.
goforwardsonar(distancego,SLOWSPEED);
}

task main()
{

// Run our setup routine
robosetup();
//Go foward until we are 4 inches from the wall.
goforwardsonar(4,FORWARDSPEED);
//Turn left.
goleft(90);
// Move to the beginning of the squares.
goforward(200);
// Detech the flame with our sensative detector
leftirdetect();
// Turn left to face the candle.
goleft(90);
// Move to the candle
frontirdetect();
// Runfan to put out the flame.
runfan();
// Go right to find the wall.
goright(90);
// Go to the wall.
goforwardsonar(6,SLOWSPEED);
// Turn right at the wall.
goright(90);
// Go to the far wall.
goforwardsonar(48,FORWARDSPEED);
// turn right.
goright(90);
// Go forward to the black area.
goforward(300);
//
runbyremote();

//*** Turn Tests.
//goright(10);
//goright(10);
//goright(10);
//goright(10);
//goright(10);
//goright(10);
//goright(10);
//goright(10);
//goright(10);
////goright(90);
//goright(90);
//goleft(90);
//////wait1Msec(2000);
//goright(90);
//goleft(90);
//////wait1Msec(2000);
//goright(90);
//goleft(90);
//////wait1Msec(2000);
//goright(180);
//goright(180);
//goright(90);
//goleft(90);
//goright(45);
//goleft(45);
//goright(90);
//goleft(90);
//goforward(100);
//gobackward(100);

}