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); }