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);
}
}
task main(){
calibrate();
while(true){
length = USreadDist(USBack);
rightWidth = USreadDist(USRight);
leftWidth = USreadDist(USLeft);
haveBall = TSreadState(HaveBaller);
frontIRValue = HTIRS2readACDir(IRFront);
currentRelCompass = HTMCreadRelativeHeading(Compass);
HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue);
isWhite = (currentRed > whiteThreshold && currentGreen > whiteThreshold && currentBlue > whiteThreshold);
nxtDisplayTextLine(1, "Com: %4d", currentRelCompass);
nxtDisplayTextLine(2, "IR: %4d", frontIRValue);
nxtDisplayTextLine(3, "Len: %4d", length);
nxtDisplayTextLine(4, "RWid: %4d", rightWidth);
nxtDisplayTextLine(5, "LWid: %4d", leftWidth);
nxtDisplayTextLine(6, "Ball: %4d", haveBall);
if(isWhite){
nxtDisplayTextLine(7, "White: Yes");
}
else{
nxtDisplayTextLine(7, "White: No");
}
if(currentRelCompass < 0 - nbound){
move(CW, turnPower);
}
else if(currentRelCompass > 0 + nbound){
move(CC, turnPower);
}
else{
switch(frontIRValue){
case 0:
move(ST);
break;
case 1:
move(L);
break;
case 2:
move(L);
break;
case 3:
move(L);
break;
case 4:
move(L);
break;
case 5:
if(leftWidth < 45 && leftWidth != 0){
if(length < 20){
move(F);
}
else{
move(R);
}
}
else if(rightWidth < 45 && rightWidth != 0){
if(length < 20){
move(F);
}
else{
move(L);
}
}
else{
if (length > fbound){
move(B);
}
else if(length < 8){
move(F);
}
else{
move(ST);
}
}
break;
case 6:
move(R);
break;
case 7:
move(R);
break;
case 8:
move(R);
break;
case 9:
move(R);
break;
}
wait1Msec(50);
}
}
}
int calc(){
int length = SensorValue[USBack];
int width = SensorValue[USRight];
bool haveBall = TSreadState(HaveBaller);
int frontIRValue = HTIRS2readACDir(IRFront);
int backIRValue = HTIRS2readACDir(IRBack);
int currentRelCompass = HTMCreadRelativeHeading(Compass);
HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue);
nxtDisplayTextLine(3, "Abs: %4d", currentRelCompass);
nxtDisplayTextLine(4, "Dir: %4d", frontIRValue);
if(currentRelCompass < -nbound){
return CC;
}
else if(currentRelCompass > nbound){
return CW;
}
else{
if(!haveBall){
if(IRFront != 0){
switch(frontIRValue){
case 1:
return B;
break;
case 2:
return BL;
break;
case 3:
return L;
break;
case 4:
return FL;
break;
case 5:
return F;
break;
case 6:
return FR;
break;
case 7:
return R;
break;
case 8:
return BR;
break;
case 9:
return B;
break;
}
}
else{
switch(backIRValue){
case 0:
return ST;
break;
case 1:
return BR;
break;
case 2:
return BR;
break;
case 3:
return B;
break;
case 4:
return BL;
break;
case 5:
return BL;
break;
case 6:
return BR;
break;
case 7:
return B;
break;
case 8:
return BL;
break;
case 9:
return BL;
break;
}
}
}
else{
hashtagyoloswagdiem();
return F;
}
}
}
