void lookforline(){ // looks for the white line
short nRawValues[4];
float tmp;
tmp = 0;
motor[leftWheel] = 10; // move forward
motor[rightWheel] = 10;
while(tmp<3.5){ // keep moving until we find the line...
getColorSensorData(color, colorAtoD,&nRawValues[0]);
tmp = (float)(nRawValues[0]+nRawValues[1]+nRawValues[2])/(3.0*nRawValues[3]);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
}
task sensors()
{
while(true)
{
switch(directions[dirIndex][2])
{
case 1: // if following left line
switch (SensorValue[colorLeft]) //assign numbers to the left color sensor
{ //and put it in the lineSensor variable
case BLACKCOLOR: lineSensor = 1; break;
case BLUECOLOR: lineSensor = 2; break;
case GREENCOLOR: lineSensor = 3; break;
case YELLOWCOLOR: lineSensor = 4; break;
case REDCOLOR: lineSensor = 5; break;
case WHITECOLOR: lineSensor = 6; break;
default: lineSensor = 0; break;
}
switch (SensorValue[colorRight]) //do the same thing to the right color
{ //sensor, but put the number in the stopSensor variable
case BLACKCOLOR: stopSensor = 1; break;
case BLUECOLOR: stopSensor= 2; break;
case GREENCOLOR: stopSensor = 3; break;
case YELLOWCOLOR: stopSensor = 4; break;
case REDCOLOR: stopSensor = 5; break;
case WHITECOLOR: stopSensor = 6; break;
default: stopSensor = 0; break;
}
getColorSensorData(colorLeft, colorAtoD, nAtoDValues); //get the individual
break; //color values from the line sensor
case 2: //same thing as above, but reversed because we are following the
switch (SensorValue[colorRight]) //other side of the line
{
case BLACKCOLOR: lineSensor = 1; break;
case BLUECOLOR: lineSensor = 2; break;
case GREENCOLOR: lineSensor = 3; break;
case YELLOWCOLOR: lineSensor = 4; break;
case REDCOLOR: lineSensor = 5; break;
case WHITECOLOR: lineSensor = 6; break;
default: lineSensor = 0; break;
}
switch (SensorValue[colorLeft])
{
case BLACKCOLOR: stopSensor = 1; break;
case BLUECOLOR: stopSensor = 2; break;
case GREENCOLOR: stopSensor = 3; break;
case YELLOWCOLOR: stopSensor = 4; break;
case REDCOLOR: stopSensor = 5; break;
case WHITECOLOR: stopSensor = 6; break;
default: stopSensor = 0; break;
}
getColorSensorData(colorRight, colorAtoD, nAtoDValues );
break;
}
red = nAtoDValues[0]; //take the color values from an array,
green = nAtoDValues[1]; //and put them in variables
blue = nAtoDValues[2];
if(lineSensor != 1) // if not seeing black, record that color
{
lineColor = lineSensor;
}
switch(lineColor) // depending on what color line we are seeing, use only some
{ //of the values, so that a low color count
case 3: lineReading = green; break; // doesn't lower the overall count
case 4: lineReading = (red + green) / 2; break;
case 2: lineReading = blue; break;
case 5: lineReading = red; break;
default: lineReading = (red + green + blue) / 3; break;
}
wait1Msec(10);
}
}
void kindOfAllOfAutonomous() // program to go to the IR beacon and drop the block and get on the bridge
{
float sL, sR; // strenght left and right
motor[leftWheel] = 30; // move forward
motor[rightWheel] = 30;
sL = strengthLeft(); // compute strengths of IR sensors
sR = strengthRight();
while(sL/sR<2.0 && sL/sR>0.5 ){ // if the strength are within a factor of 2 of each other
sL = strengthLeft();
sR = strengthRight();
}
if(sL>sR) // see which side the IR is...
{ // it is on the left!
while(HTIRS2readACDir(irLeft) != 5) // while we're not in front of the beacon.
{
sL = USreadDist(sonarLeft);
if(sL<50){
motor[leftWheel] = 30+ 0.5*(35-sL) ; // try to keep 20cm distance
motor[rightWheel] = 30- 0.5*(35-sL) ;
}
}
// we should now be in front of the beacon and the beacon is on the left
motor[slide] = -100;
wait1Msec(1500); //CALIBRATE
motor[slide] = 0;
turn(-90); // turn left
motor[leftWheel] = 15; // get closer to basket
motor[rightWheel] = 15;
wait1Msec(750); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[block] = -100; // spit it out...
wait1Msec(3000);
motor[block] = 0;
// retrace our steps...
motor[leftWheel] = -15;
motor[rightWheel] = -15;
wait1Msec(750); //CALIBRATE same as above
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[slide] = -100; // move our slide down
wait1Msec(1500);
motor[slide] = 0;
turn(90); //turn right
sL = USreadDist(sonarLeft);
while(sL<50){
motor[leftWheel] = -30- 0.5*(20-sL) ; // try to keep 20cm distance
motor[rightWheel] = -30+ 0.5*(20-sL) ;
sL = USreadDist(sonarLeft);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(-90); // turn left and go for the line...
motor[leftWheel] = 30; //move forward
motor[rightWheel] = 30;
float cs = 0; // find the line...
short nRawValues[4];
while(cs<3.5){
getColorSensorData(color, colorAtoD,&nRawValues[0]);
cs = (float)(nRawValues[0]+nRawValues[1]+nRawValues[2])/(3.0*nRawValues[3]);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(90); // turn right
motor[leftWheel] = 30;
motor[rightWheel] = 30;
wait1Msec(3000); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
else // the beacon is on the right
{
while(HTIRS2readACDir(irRight) != 5) // while we're not in front of the beacon.
{
sR = USreadDist(sonarRight);
if(sR<50){
motor[leftWheel] = 30- 0.5*(20-sR) ; // try to keep 20cm distance
motor[rightWheel] = 30+ 0.5*(20-sR) ;
}
}
// we should now be in front of the beacon and the beacon is on the right
motor[slide] = 100;
wait1Msec(1500); //CALIBRATE
motor[slide] = 0;
turn(90); // turn right
motor[leftWheel] = 15; // get closer to basket
motor[rightWheel] = 15;
wait1Msec(750); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[block] = -100; // spit it out...
wait1Msec(3000);
motor[block] = 0;
// retrace our steps...
motor[leftWheel] = -15;
motor[rightWheel] = -15;
wait1Msec(750); //CALIBRATE same as above
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[slide] = -100; // move our slide down
wait1Msec(1500);
motor[slide] = 0;
turn(-90); //turn left
sR = USreadDist(sonarRight);
while(sR<50){
motor[leftWheel] = -30+ 0.5*(20-sR) ; // try to keep 20cm distance
motor[rightWheel] = -30- 0.5*(20-sR) ;
sR = USreadDist(sonarRight);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(90); // turn right and go for the line...
motor[leftWheel] = 30; //move forward
motor[rightWheel] = 30;
float cs = 0; // find the line...
short nRawValues[4];
while(cs<3.5){
getColorSensorData(color, colorAtoD,&nRawValues[0]);
cs = (float)(nRawValues[0]+nRawValues[1]+nRawValues[2])/(3.0*nRawValues[3]);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(-90); // turn left
motor[leftWheel] = 30;
motor[rightWheel] = 30;
wait1Msec(3000); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
//we are done...
}
task main()
{
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |knock down poles
2 parking |knock down poles
3 |
4 |
5 |
6 |
7 |
8 |
10 |
11 |
12 |
13 |
14 |
15 anywhere|go straight 5 seconds
*/
wait1Msec(2000);
disableDiagnosticsDisplay();
short colorValues[4];//these are the scanned values
int j;
long r;
long g;
long b;
// wait1Msec(500);
int minDifference = 32767; // MAX_INT
int selectedColor = 0;//this is the color closest to the scanned color
while(true)
{
for(j =0; j<4; j++)//get color sensor values
{
getColorSensorData(colorPort, 0, &colorValues[j]);
r = colorValues[0];
g = colorValues[1];
b = colorValues[2];
}
for(int i = 0; i < COLOR_COUNT; i++)//here we find the closest color in our database
{
difference(//passing in the database values and the scanned values to find the difference
colorTags[i][0],
colorTags[i][1],
colorTags[i][2],
r, g, b);
long diff = thediff;
if (diff <= minDifference) {//if the difference between the color in the database that we are currently checking and the scanned color
minDifference = diff; //is less than the smallest difference between the other database colors, then the database color we are
selectedColor = i; //currently checking is the closest match so far
}
nxtDisplayCenteredTextLine(0, "%d, %d", diff, minDifference);//print all the values
nxtDisplayCenteredTextLine(1, "%d,%d,%d", colorValues[0], colorValues[1], colorValues[2]);
nxtDisplayCenteredTextLine(4, "%d,%d,%d,", colorTags[i][0], colorTags[i][1], colorTags[i][2]);
}
nxtDisplayCenteredTextLine(2, "%s", colornames[selectedColor]);
nxtDisplayCenteredTextLine(3, "Is this the color?");
if(nNxtButtonPressed==2)
{
break;
}
}
waitForStart();
switch(selectedColor)
{
case 1:
program1();
break;
case 2:
program2();
break;
/*case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
break;*/
case 15:
program15();
break;
default:
program15();
break;
}
}
