task main () {
int _color = 0;
string _tmp;
int red = 0;
int green = 0;
int blue = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "COLOR V2");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and CS to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
while (true) {
// Read the currently detected colour from the sensor
_color = HTCS2readColor(HTCOLOR);
// If colour == -1, it implies an error has occurred
if (_color < 0) {
nxtDisplayTextLine(4, "ERROR!!");
nxtDisplayTextLine(5, "HTCS2readColor()");
wait1Msec(2000);
StopAllTasks();
}
// Read the RGB values of the currently colour from the sensor
// A return value of false implies an error has occurred
if (!HTCS2readRGB(HTCOLOR, red, green, blue)) {
nxtDisplayTextLine(4, "ERROR!!");
nxtDisplayTextLine(5, "HTCS2readRGB()");
wait1Msec(2000);
StopAllTasks();
}
nxtDisplayCenteredTextLine(0, "Color: %d", _color);
nxtDisplayCenteredBigTextLine(1, "R G B");
nxtEraseRect(0,10, 99, 41);
nxtFillRect( 0, 10, 30, 10 + (red+1)/8);
nxtFillRect(35, 10, 65, 10 + (green+1)/8);
nxtFillRect(70, 10, 99, 10 + (blue+1)/8);
StringFormat(_tmp, " %3d %3d", red, green);
nxtDisplayTextLine(7, "%s %3d", _tmp, blue);
wait1Msec(100);
}
}
//----------------------------------------------------------------------------START TASK MAIN---------------------------------------------
task main()
{
// clearDebugStream();
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |drop ball in small goal and move to parking
2 parking |drop ball in small goal and move to parking//values not tested
3 parking |score in center goal
4 parking |knocks down kickstand
5 parking |messes up oponents rolling goals
6 |
7 parking |get in way of opponent 120 goal
8 |
9 |
10 |
11 ramp |drop ball in small goal and move to parking(with compass)
12 parking |drop ball in small goal and move to parking//values not tested
13 |
14 |
15 anywhere|go straight 6 feet (can get of ramp)
21 ramp |drop ball in small goal and move to parking (BACKWARDS)
3|lightblue//program1
3|darkblue
5|darkyellow
7|pink
7|red//program3
7|orange
13|lightgreen
14|white//program11
14|tan
14|brown
14|lightyellow
17|purple
17|black//default(program15)
*/
//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
servo[gate]=gateup;
// gostraight
sFRS = 161;
sBRS = 154;
sFLS = 132;
sBLS = 131;
servo[frontRS]= sFRS;
servo[backRS]= sBRS;
servo[frontLS]= sFLS;
servo[backLS]= sBLS;
wait1Msec(500);
//servo[leftmandible] = outL;
//servo[rightmandible] = outR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
servo[mouth]= mouthup;
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
turnServos(1,0);
clearDebugStream();
while(true){godirection(1,120);}
}
//----------------------------------------------------------------------------START TASK MAIN---------------------------------------------
task main()
{
// clearDebugStream();
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |drop ball in small goal and move to parking
2 parking |drop ball in small goal and move to parking//values not tested
3 parking |score in center goal
4 parking |knocks down kickstand
5 parking |messes up oponents rolling goals
6 |
7 parking |get in way of opponent 120 goal
8 |
9 |
10 |
11 ramp |drop ball in small goal and move to parking(with compass)
12 parking |drop ball in small goal and move to parking//values not tested
13 |
14 |
15 anywhere|go straight 6 feet (can get of ramp)
21 ramp |drop ball in small goal and move to parking (BACKWARDS)
3|lightblue//program1
3|darkblue
5|darkyellow
7|pink
7|red//program3
7|orange
13|lightgreen
14|white//program11
14|tan
14|brown
14|lightyellow
17|purple
17|black//default(program15)
*/
//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
servo[gate]=gateup;
// gostraight
sFRS = 161;
sBRS = 154;
sFLS = 132;
sBLS = 131;
servo[frontRS]= sFRS;
servo[backRS]= sBRS;
servo[frontLS]= sFLS;
servo[backLS]= sBLS;
wait1Msec(500);
//servo[leftmandible] = outL;
//servo[rightmandible] = outR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
servo[mouth]= mouthup;
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
turnServos(1,0);
clearDebugStream();
/* while(true)
{
if(!HTCS2readRGB(colorport, r, g,b))
{
writeDebugStream("BAD\n");
}
writeDebugStream("%d, %d, %d\n", r,g,b);
//writeDebugStream("----------------\n");
wait1Msec(1000);
}
*/
// wait1Msec(4000);
/*
3|lightblue//program1
3|darkblue
5|darkyellow
7|pink
7|red//program3
7|orange
13|lightgreen
14|white//program11
14|tan
14|brown
14|lightyellow
17|purple
17|black//default(program15)
*/
/* while(true)
{
for(j =0; j<4; j++)//get color sensor values
{
// x=HTCS2readRGB(colorport, r, g,b);
//getColorSensorData(colorport, 0, &colorValues[j]);
r = colorValues[0];
g = colorValues[1];
b = colorValues[2];
// writeDebugStream("R:%d,G:%d,B:%d\n",r,g,b);
//writeDebugStream("color:%d\n",HTCS2readColor(colorport));
}
}*//*
for(int x=0; x<10; x++)//test values a couple of times to make sure we have the right one
{
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;
}
}
*/
// disableDiagnosticsDisplay();
// eraseDisplay();
switch(selectedColor)
{
case 1:
case 3:
nxtDisplayCenteredTextLine(2,"BLUE");
break;
case 7:
nxtDisplayCenteredTextLine(2,"RED");
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
nxtDisplayCenteredTextLine(2,"WHITE");
break;
case 15:
break;
case 16:
break;
case 17:nxtDisplayCenteredTextLine(2,"BLACK");
break;
default:
nxtDisplayCenteredTextLine(2,"BLACK");
break;
}
turnServos(1,0);
waitForStart();
servo[gate]=gatedrive;
switch(selectedColor)
{
case 1:
break;
case 2:
break;
case 3:
program1();
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
writeDebugStream("7");
program3();
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
program16();
break;
case 15:
break;
case 16:
break;
program15();
break;
default:
program15();
break;
}
// program1();
//while(true){godirection(1,120);}
//turnleft90();
//compassturn(180);
//compassturn(180);
//while(true)
// godirection(4,120);
//program1();
//turnServos(4,0);
//while(true);
}
