int getSeeker()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// set the DSP to the new mode
if ( ! HTIRS2setDSPMode(seeker, DSP_1200))
return -1; // Sensor initialized
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(seeker);
if (_dirDC < 0)
return -1; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(seeker);
if (_dirAC < 0)
return -1; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5))
return -1; // I2C read error occurred
if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 ))
return -1; // I2C read error occurred
count ++;
writeDebugStreamLine("D %d %d", count, _dirAC);
//writeDebugStreamLine("0 %d %d", dcS1, acS1);
//writeDebugStreamLine("1 %d %d", dcS2, acS2);
//writeDebugStreamLine("2 %d %d", dcS3, acS3);
//writeDebugStreamLine("3 %d %d", dcS4, acS4);
//writeDebugStreamLine("4 %d %d", dcS5, acS5);
if(_dirAC == 2)// && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor
{
static int debugPrinted = 0;
PlayTone(440<<2, 10);
if (!debugPrinted){
debugPrinted = true;
writeDebugStreamLine("Played sound at %d degrees", gyroVal);
//basketNumber = degreesToBasket(gyroVal);
}
return 1;
}
else if(_dirAC == 3)
return 2;
else if(_dirAC == 4)
return 3;
else
{
return -1*_dirAC;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//IR
void UpdateIR() // brings back information from 1 port, which we can call w/ the variable IRport
{
HTIRS2readAllDCStrength(HTIRS1, dc1S1, dc1S2, dc1S3, dc1S4, dc1S5);
// ^^ reads ports
IRinputs[1][1] = dc1S1; // sets ports to aray
IRinputs[1][2] = dc1S2;
IRinputs[1][3] = dc1S3;
IRinputs[1][4] = dc1S4;
IRinputs[1][5] = dc1S5;
HTIRS2readAllDCStrength(HTIRS2, dc2S1, dc2S2, dc2S3, dc2S4, dc2S5);
// ^^ reads ports
IRinputs[2][1] = dc2S1; // sets ports to aray
IRinputs[2][2] = dc2S2;
IRinputs[2][3] = dc2S3;
IRinputs[2][4] = dc2S4;
IRinputs[2][5] = dc2S5;
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
string tmpString;
// show the user what to do
displayInstructions();
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
// When connected to a SMUX, the IR Seeker V2 can only be
// used in 1200Hz mode.
nxtDisplayTextLine(0, " DC 1200");
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
while (true)
{
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
if (HTSMUXreadPowerStatus(HTSMUX))
nxtDisplayTextLine(7, "Batt: bad");
else
nxtDisplayTextLine(7, "Batt: good");
}
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
while(true)
{
// You can switch between the two different DSP modes by pressing the
// orange enter button
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
nxtDisplayTextLine(0, " DC 1200");
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
}
}
// main task
task main ()
{
// dc and ac directional values.
int _dirDC = 0;
int _dirAC = 0;
// DC and AC values from 5 internal detectors.
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
// initialize the array sTextLines.
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display some header info at top of screen
eraseDisplay();
nxtDisplayTextLine(0, " DC AC");
nxtDisplayTextLine(1, "-----------------");
// loop continuously and read from the sensor.
while(true)
{
_dirDC = HTIRS2readDCDir(HTIRS2);
_dirAC = HTIRS2readACDir(HTIRS2);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
string a = "D";
string b = "0";
string c = "1";
string d = "2";
string e = "3";
string f = "4";
displayText(2, a, _dirDC, _dirAC);
displayText(3, b, dcS1, acS1);
displayText(4, c, dcS2, acS2);
displayText(5, d, dcS3, acS3);
displayText(6, e, dcS4, acS4);
displayText(7, f, dcS5, acS5);
// wait a little before resuming.
wait10Msec(5);
}
}
task main()
{
init();
//waitForStart(); // Wait for the beginning of autonomous phase.
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while((fs1<25&&fs1>1000||fs1<1400)||(fs2<1400||fs2<25&&fs2>1000)){
fs1 = HTFreadSensor(HTFS1);
fs2 = HTFreadSensor(HTFS2);
HTIRS2readAllDCStrength(HTIRS2,dcS1,dcS2,dcS3,dcS4,dcS5);
nxtDisplayCenteredTextLine(1,"IRSensor\n%i\n%i\n%i\n%i\n%i)",dcS1,dcS2,dcS3,dcS4,dcS5);
if(dcS1>5||dcS2>5||dcS3>5||dcS4>5||dcS5>5){
if(dcS3>dcS1&&dcS3>dcS2&&dcS3>dcS4&&dcS3>dcS5){
forward(power);
}
else if(dcS1+dcS2>dcS4+dcS5){
rotateLeft(power);
}
else if(dcS1+dcS2<dcS4+dcS5){
rotateRight(power);
}
else{
stop();
}
}
else{
forward(power/2);
}
}
servoPos=255;
stop();
wait1Msec(1000);
servo[servo1]=servoPos;
reverse(power);
wait1Msec(500);
stop();
}
int getSeeker()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// set the DSP to the new mode
if ( ! HTIRS2setDSPMode(seeker, DSP_1200))
return -1; // Sensor initialized
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(seeker);
if (_dirDC < 0)
return -1; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(seeker);
if (_dirAC < 0)
return -1; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5))
return -1; // I2C read error occurred
if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 ))
return -1; // I2C read error occurred
writeDebugStreamLine("D %d %d", _dirDC, _dirAC);
writeDebugStreamLine("0 %d %d", dcS1, acS1);
writeDebugStreamLine("1 %d %d", dcS2, acS2);
writeDebugStreamLine("2 %d %d", dcS3, acS3);
writeDebugStreamLine("3 %d %d", dcS4, acS4);
writeDebugStreamLine("4 %d %d", dcS5, acS5);
if(acS3 > 100 && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor
return 1;
else
return 0;
}
task main()
{
//while(true){
// nxtDisplayCenteredTextLine(1,"%i",SensorValue[lightSensor]);
//}
//waitForStart();
nMotorEncoder[weightedRingLight]=0;
int threshold=20;
int powerLevel=50;
int lines=0;
int prevColor=0;
int dcS1,dcS2,dcS3,dcS4,dcS5;
while(SensorValue[lightSensor]<threshold){
motor[motorFL] = -powerLevel;
motor[motorFR] = -powerLevel;
motor[motorBR] = -powerLevel;
motor[motorBL] = -powerLevel;
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
nxtDisplayCenteredTextLine(1,"IR%i",dcS3);
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=powerLevel;
motor[motorFR]=powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=powerLevel;
motor[motorFR]=powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel*3;
motor[motorFR]=-powerLevel*3;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel*2;
motor[motorFR]=-powerLevel*2;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel;
motor[motorFR]=-powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// show the user what to do
displayInstructions();
while(true)
{
// You can switch between the two different DSP modes by pressing the
// orange enter button
PlaySound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
nNumbCyles = 0;
++nInits;
while (true)
{
if ((nNumbCyles & 0x04) == 0)
nxtDisplayTextLine(0, "Initializing...");
else
nxtDisplayTextLine(0, "");
nxtDisplayCenteredBigTextLine(1, "IR Seekr");
// set the DSP to the new mode
if (HTIRS2setDSPMode(HTIRS2, _mode))
break; // Sensor initialized
++nNumbCyles;
PlaySound(soundShortBlip);
nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
nxtDisplayCenteredTextLine(6, "Connect Sensor");
nxtDisplayCenteredTextLine(7, "to Port S1");
wait1Msec(100);
}
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
if (_mode == DSP_1200)
nxtDisplayTextLine(0, " DC 1200");
else
nxtDisplayTextLine(0, " DC 600");
while (true)
{
++nNumbCyles;
if (nNxtButtonPressed == kEnterButton)
{
// "Enter" button has been pressed. Need to switch mode
_mode = (_mode == DSP_1200) ? DSP_600 : DSP_1200;
while(nNxtButtonPressed == kEnterButton)
{
// Wait for "Enter" button release
}
break;
}
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
nxtDisplayTextLine(7, "Enter to switch");
}
}
}
task main()
{
initializeRobot();
int _dirDC_left = 0;
int _dirAC_left = 0;
int _dirDC_right = 0;
int _dirAC_right = 0;
int dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left = 0;
int acS1_left, acS2_left, acS3_left, acS4_left, acS5_left = 0;
int dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right = 0;
int acS1_right, acS2_right, acS3_right, acS4_right, acS5_right = 0;
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
waitForStart();
tHTIRS2DSPMode _mode = DSP_1200;
while(true) {
PlaySound(soundShortBlip);
if(HTIRS2setDSPMode(irL, _mode)) {
break;
}
}
while(true) {
PlaySound(soundShortBlip);
if(HTIRS2setDSPMode(irR, _mode)) {
break;
}
}
while (true) {
nxtDisplayTextLine(1, " L R");
_dirDC_left = HTIRS2readDCDir(irL);
if (_dirDC_left < 0)
break; // I2C read error occurred
// read the current irL);
_dirAC_left = HTIRS2readACDir(irR);
if (_dirAC_left < 0)
break; // I2C read error occurred
_dirDC_right = HTIRS2readDCDir(irR);
if (_dirDC_right < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC_right = HTIRS2readACDir(irR);
if (_dirAC_right < 0)
break; // I2C read error occurred
if (!HTIRS2readAllDCStrength(irL, dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(irL, acS1_left, acS2_left, acS3_left, acS4_left, acS5_left))
break; // I2C read error occurred
if (!HTIRS2readAllDCStrength(irR, dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(irR, acS1_right, acS2_right, acS3_right, acS4_right, acS5_right))
break; // I2C read error occurred
displayText(1, "D", _dirAC_left, _dirAC_right);
displayText(2, "0", acS1_left, acS1_right);
displayText(3, "1", acS2_left, acS2_right);
displayText(4, "2", acS3_left, acS3_right);
displayText(5, "3", acS4_left, acS4_right);
displayText(6, "4", acS5_left, acS5_right);
}
}
task main()
{
initializeRobot();
int motorLeft = 0;
int motorRight = 0;
waitForStart();
motor[driveLt]=100;
motor[driveRt]=100;
// IRseek loop
int _dirDC = 0;
int _dirAC = 0;
// DC and AC values from 5 internal detectors.
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
if (HTIRS2setDSPMode(HTIRS2, _mode) == 0)
{
// unsuccessful at setting the mode.
// display error message.
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "Init failed!");
nxtDisplayCenteredTextLine(3, "Connect sensor");
nxtDisplayCenteredTextLine(4, "to Port 1.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task. FTC-TD-0004-Rev-001-IRSeeker Page 12 of 16 9/14/2012 10:54:00 AM
wait10Msec(300);
return;
}
while((dcS3 < IRSEEK_CUTOFF) && (acS3 < IRSEEK_CUTOFF)){
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
}
wait10Msec(IRSEEK_POST_CUTOFF_WAIT);
servo[servo1] = 244;
wait10Msec(50);
servo[servo1] = 5;
wait10Msec(50);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=0;
motor[driveRt]=0;
while (true)
{}
}
task main()
{
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
PlaySound(soundBlip);
wait1Msec((2 * PI) * 1000); //TAUUUU
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
float irval = acS3L;
StringFormat(irvalres, "%3.0f", irval);
WriteText(irFileHandle, IOResult, "Test");
WriteString(irFileHandle, IOResult, irvalres);
WriteByte(irFileHandle, IOResult, 13);
WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
motor[motorLeft] = 0;
motor[motorRight] = 0;
irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
driveToEnd(timeToEnd - roundTime);//drive to end of ramp
}
wait1Msec(300);
//turn left, forward, turn left, forward onto ramp
turnLeft(1.1, 100);
wait1Msec(300);
moveForward(1, 100);
wait1Msec(300);
turnLeft(1.1, 100);
wait1Msec(300);
moveForward(2.7, 100);
wait1Msec(300);
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
while (koth) {
wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
PlaySound(soundDownwardTones);
}//task main
task main()
{
// The IR signal strengh in all 5 directions.
int IRdirA = 0;
int IRdirB = 0;
int IRdirC = 0;
int IRdirD = 0;
int IRdirE = 0;
waitForStart();
initializeRobot();
// The amount of time the robot...
const int forwardTimeAA = 25;
const int turnTimeA = 50;
const int forwardTimeA = 170;
const int turnTimeB = 110;
const int forwardTimeB = 100;
const int liftTimeB = 45;
const int forwardTimeCA = 110; //TODO
const int forwardTimeCB = 40; //TODO
const int turnTimeD = 152;
const int forwardTimeD = 110;
const int liftTimeD = 135;
const int forwardTimeE = 95; //TODO
const int turnTimeF = 112;
const int forwardTimeF = 80;
const int liftTimeF = 47;
const int liftTimeG = 30; //TODO
const int backwardTimeG = 100; //TODO
const int turnTimeG = 70; //TODO
const int forwardTimeG = 20; //TODO
const int liftTimeH = 50; //TODO
const int backwardTimeH = 90; //TODO
const int turnTimeH = 100; //TODO
const int forwardTimeH = 70; //TODO
const int liftTimeI = 30; //TODO
const int backwardTimeI = 130; //TODO
const int turnTimeI = 70; //TODO
const int forwardTimeI = 170; //TODO
const int forwardTimeJ = 50; //TODO
const int turnTimeK = 90; //TODO
const int liftTimeK = 30; //TODO
const int forwardTimeK = 50; //TODO
Move_Forward (forwardTimeAA, g_AccurateMotorPower);
Turn_Left (turnTimeA, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeA, g_AccurateMotorPower);
Time_Wait(50);
HTIRS2readAllDCStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
{
Turn_Right (turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeB, g_AccurateMotorPower);
Move_Forward (forwardTimeB, g_AccurateMotorPower);
Lift_Down (liftTimeG, g_AccurateMotorPower);
Move_Backward (backwardTimeG, g_AccurateMotorPower);
Turn_Right (turnTimeG, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeG, g_AccurateMotorPower);
}
else
{
Move_Forward (forwardTimeCA, g_AccurateMotorPower);
Time_Wait(50);
HTIRS2readAllACStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
Move_Forward (forwardTimeCB, g_AccurateMotorPower);
if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
{
Turn_Right (turnTimeD, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeD, g_AccurateMotorPower);
Move_Forward (forwardTimeD, g_AccurateMotorPower);
Lift_Down (liftTimeH, g_AccurateMotorPower);
Move_Backward (backwardTimeH, g_AccurateMotorPower);
Turn_Right (turnTimeH, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeH, g_AccurateMotorPower);
}
else
{
Move_Forward (forwardTimeE, g_AccurateMotorPower);
Turn_Right (turnTimeF, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeF, g_AccurateMotorPower);
Move_Forward (forwardTimeF, g_AccurateMotorPower);
Lift_Down (liftTimeI, g_AccurateMotorPower);
Move_Backward (backwardTimeI, g_AccurateMotorPower);
Turn_Right (turnTimeI, g_AccurateMotorPower, g_AccurateMotorPower);
Move_Forward (forwardTimeI, g_AccurateMotorPower);
}
}
Move_Forward (forwardTimeJ, g_AccurateMotorPower);
Turn_Right (turnTimeK, g_AccurateMotorPower, g_AccurateMotorPower);
Lift_Up (liftTimeK, g_AccurateMotorPower);
Move_Forward (forwardTimeK, g_AccurateMotorPower);
while (true)
{
PlaySoundFile("moo.rso");
while(bSoundActive == true)
{
Time_Wait(1);
}
}
}
//=============================================================================================================================================
//---------------------------------------------------BEGIN INITIALIZATION CODE-----------------------------------------------------------------
task main() {
//Initialize the display with the program choices
chooseProgram();
switch (PROGID) {
case 1:
FORWARD_SCORE_FORWARD_LINE_1 = true;
linesToFind = 1;
break;
case 2:
FORWARD_SCORE_FORWARD_LINE_2 = true;
linesToFind = 2;
break;
case 3:
FORWARD_SCORE_BACKWARD_LINE_1 = true;
linesToFind = 1;
break;
case 4:
FORWARD_SCORE_BACKWARD_LINE_2 = true;
linesToFind = 2;
break;
case 5:
useDummyAutonomous();
break;
case 6:
//useOriginalAutonomous();
PlaySoundFile("Woops.rso");
break;
}
//---------------------------------------------------------END INITIALIZATION CODE-------------------------------------------------------------
//=============================================================================================================================================
//if (PROGID == 1 || PROGID == 2 || PROGID == 3 || PROGID == 4) {
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
//PlaySound(soundBlip);
//wait1Msec((2 * PI) * 1000); //TAUUUU
//wait1Msec(10000);//wait 10 seconds for other teams who **might** have better autonomous code
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
servo[servoLift] = 123;
servo[servoSweep] = 199;
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
//float irval = acS3L;
//StringFormat(irvalres, "%3.0f", irval);
//WriteText(irFileHandle, IOResult, "Test");
//WriteString(irFileHandle, IOResult, irvalres);
//WriteByte(irFileHandle, IOResult, 13);
//WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
//if it doesnt find the beacon, dont bother returning to start if it has been set to
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
if (time1[T1] < 2000) {
wait1Msec(600);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
//irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
//Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
driveToEnd(-80, timeToEnd - roundTime);//drive to end of ramp
}
if (FORWARD_SCORE_BACKWARD_LINE_1 || FORWARD_SCORE_BACKWARD_LINE_2) {
driveToEnd(80, roundTime);
}
}
wait1Msec(300);
//=======================================================================================================================================
//------------------------BEGIN 90 DEGREE TURNS------------------------------------------------------------------------------------------
//HTGYROstartCal(HTGYRO);
ClearTimer(T3);
while (true) {
wait1Msec(20);
//if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degFirstTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
//---------------LINE DETECTOR--------------------------
LSsetActive(LEGOLS);
while (linesFound < linesToFind) {
motor[motorLeft] = -50;
motor[motorRight] = -50;
wait1Msec(10);
if (LSvalNorm(LEGOLS) >= WHITE_LINE_VALUE) {
linesFound++;
}
if (linesFound >= linesToFind) { //ever-present failsafe
break;
LSsetInactive(LEGOLS);
}
}
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degSecondTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
} else {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = -60;
motor[motorRight] = 60;
if (heading <= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
}
break;
}
}
//==================================================================================
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
float heading = 92;
HTGYROstartCal(HTGYRO);
while (koth) {
//wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
MissionImpossible();
/*
}//END MAIN IF PROGIDS THING
else if (PROGID == 5) {
useDummyAutonomous();
}
*/
}//task main
// main task
task main ()
{
int last = 0;
int stage = 1;
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// show the user what to do
displayInstructions();
while(true)
{
// You can switch between the two different DSP modes by pressing the
// orange enter button
PlaySound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
nNumbCyles = 0;
++nInits;
while (true)
{
if ((nNumbCyles & 0x04) == 0)
nxtDisplayTextLine(0, "Initializing...");
else
nxtDisplayTextLine(0, "");
nxtDisplayCenteredBigTextLine(1, "IR Seekr");
// set the DSP to the new mode
if (HTIRS2setDSPMode(HTIRS2, _mode))
break; // Sensor initialized
++nNumbCyles;
PlaySound(soundShortBlip);
nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
nxtDisplayCenteredTextLine(6, "Connect Sensor");
nxtDisplayCenteredTextLine(7, "to Port S2");
wait1Msec(100);
}
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
if (_mode == DSP_1200)
nxtDisplayTextLine(0, " DC 1200");
else
nxtDisplayTextLine(0, " DC 600");
while (true)
{
++nNumbCyles;
if (nNxtButtonPressed == kEnterButton)
{
// "Enter" button has been pressed. Need to switch mode
_mode = (_mode == DSP_1200) ? DSP_600 : DSP_1200;
while(nNxtButtonPressed == kEnterButton)
{
// Wait for "Enter" button release
}
break;
}
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
//displayText(7, "Last", dcS5, acS5);
//nxtDisplayTextLine(7, "Last: "+last);
/*
if(acS3 >= 50){
int speed = 20;
motor[DFR] = -speed;
motor[DFL]= -speed;
motor[DBR] = -speed;
motor[DBL]= -speed;
}
else if(acS4+acS5 >= 10){
//Right
int speed = 20;
motor[DFR] = speed;
motor[DFL]= -speed;
motor[DBR] = speed;
motor[DBL]= -speed;
}
else if(acS1+acS2 >= 10){
//Left
int speed = 20;
motor[DFR] = -speed;
motor[DFL]= speed;
motor[DBR] = -speed;
motor[DBL]= speed;
}
else{
motor[DFR] = 0;
motor[DFL]= 0;
motor[DBR] = 0;
motor[DBL]= 0;
}
*/
while(stage == 1){
int acS1, acS2, acS3, acS4, acS5 = 0;
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
if(acS1 > acS2-100&&last>10){
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = -20;
last = -1;
stage = 2;
wait1Msec(1000);
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = 0;
}
else{
if(last<acS1)
last = acS1;
int speed = 90-3*(acS2 + acS3);
if (speed < 9) speed = 9;
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = speed;
}
}
while(stage == 2){
//Right
//stage_done = 1;
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
int speed = 20-acS3;
if(speed < 9) speed = 9;
motor[DFR] = -speed;
motor[DFL]= speed;
motor[DBR] = -speed;
motor[DBL]= speed;
if(acS3>last)
last = acS3;
if(last-acS3 > 1&&last!=0){
motor[DFR] = 0;
motor[DFL]= 0;
motor[DBR] = 0;
motor[DBL]= 0;
stage = 3;
wait1Msec(1000);
}
// nxtDisplayTextLine(0, "Last: "+last);
// nxtDisplayTextLine(1, "acS3: "+dcS3);
wait1Msec(500);
}
}
}
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// show the user what to do
displayInstructions();
while(true)
{
PlaySound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
nNumbCyles = 0;
++nInits;
while (true)
{
if ((nNumbCyles & 0x04) == 0)
nxtDisplayTextLine(0, "Initializing...");
else
nxtDisplayTextLine(0, "");
nxtDisplayCenteredBigTextLine(1, "SMUX");
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
if (HTSMUXscanPorts(HTSMUX))
break;
++nNumbCyles;
PlaySound(soundShortBlip);
nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
nxtDisplayCenteredTextLine(6, "Connect SMUX");
nxtDisplayCenteredTextLine(7, "to Port S1");
wait1Msec(100);
}
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
// When connected to a SMUX, the IR Seeker V2 can only be
// used in 1200Hz mode.
nxtDisplayTextLine(0, " DC 1200");
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
while (true)
{
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(msensor_S1_1);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(msensor_S1_1);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(msensor_S1_1, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(msensor_S1_1, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
if (HTSMUXreadPowerStatus(HTSMUX))
nxtDisplayTextLine(7, "Batt: bad");
else
nxtDisplayTextLine(7, "Batt: good");
}
}
}
task main()
{
//Initiate Robot
init();
//Raise Arm
while(nMotorEncoder[armR]<5){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[armR]);
}
stop();
lsVal = LSvalNorm(LEGOLS);
while(lsVal>85){
lsVal = LSvalNorm(LEGOLS);
forward(10);
}
stop();
wait1Msec(500);
ClearTimer(T1);
while(time1[T1]<1000){
HTIRS2readAllDCStrength(HTIRS2,dcS1,dcS2,dcS3,dcS4,dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
nxtDisplayCenteredTextLine(1,"IR:");
nxtDisplayCenteredTextLine(2,"%i",acS1);
nxtDisplayCenteredTextLine(3,"%i",acS2);
nxtDisplayCenteredTextLine(4,"%i",acS3);
nxtDisplayCenteredTextLine(5,"%i",acS4);
nxtDisplayCenteredTextLine(6,"%i",acS5);
}
if(acS2<20&&acS4<20&&acS3>20)
goto column2;
else if(acS2>acS4)
goto column3;
else
goto column1;
////////First Column/////////////
column1:;
stop();
nxtDisplayCenteredTextLine(1,"Column One:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);
////////Second Column/////////////
column2:;
lsVal = LSvalNorm(LEGOLS);
while(time1[T1]<2000){
if(lsVal<50){
forward(15);
}
else{
left(20);
}
lsVal = LSvalNorm(LEGOLS);
}
stop();
//forward(20);
//wait1Msec(1000);
//stop();
//forward(30);
//wait1Msec(250);
//stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Two:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
////////Third Column//////////////
column3:;
stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Three:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
}
