// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int _dirEnh, _strEnh;
//// 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 Enh");
while (true)
{
//// Read the current non modulated signal direction
//_dirDC = HTIRS2readDCDir(irLeft);
//if (_dirDC < 0)
// break; // I2C read error occurred
//// read the current modulated signal direction
//_dirAC = HTIRS2readACDir(irLeft);
//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(irLeft, dcS1, dcS2, dcS3, dcS4, dcS5))
// break; // I2C read error occurred
//if (!HTIRS2readAllACStrength(irLeft, acS1, acS2, acS3, acS4, acS5 ))
// break; // I2C read error occurred
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(irLeft, _dirEnh, _strEnh))
break; // I2C read error occurred
//displayText3(1, "D", _dirDC, _dirAC, _dirEnh);
//displayText(2, "0", dcS1, acS1);
//displayText(3, "1", dcS2, acS2);
displayText3(4, "2", dcS3, acS3, _strEnh);
//displayText(5, "3", dcS4, acS4);
//displayText(6, "4", dcS5, acS5);
//if (HTSMUXreadPowerStatus(HTSMUX))
// nxtDisplayTextLine(7, "Batt: bad");
//else
// nxtDisplayTextLine(7, "Batt: good");
}
}
int getIrStrengthLeft()
{
int direction, strength;
if(!HTIRS2readEnhanced(irLeft, direction, strength)){
writeDebugStreamLine("something's wrong");
return -1;
}
return strength;
}
//This function assumes that the robot only needs to move left and right to find the beacon:
//Make sure x,y,r is a correct starting position
void seekIR(){
int _dirEnh;
int _strEnh;
float dir;
float time = 0;
HTIRS2readEnhanced(IR, _dirEnh, _strEnh);
time1[T1] = 0;
while (_dirEnh != 6) {
time1[T1] = 0;
HTIRS2readEnhanced(IR, _dirEnh, _strEnh);
if (_dirEnh == 0) {
motor[fr] = 0;
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
PlaySound(soundShortBlip);
}
else if (_dirEnh < 6){
motor[fr] = 30;
motor[br] = -30;
motor[bl] = -30;
motor[fl] = 30;
dir = -1;
}
else if (_dirEnh > 6) {
motor[fr] = -30;
motor[br] = 30;
motor[bl] = 30;
motor[fl] = -30;
dir = 1;
}
time += dir*time1[T1];
}
//Edit this according to basket position
motor[fr] = -30; //Sideways
motor[br] = 30;
motor[bl] = 30;
motor[fl] = -30;
wait1Msec(500);
motor[fr] = -30; //Backwards
motor[br] = -30;
motor[bl] = 30;
motor[fl] = 30;
wait1Msec(1200);
motor[fr] = 0; //Arm up
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
PlaySound(soundBeepBeep);
servo[arml] = ARMUPL;
servo[armr] = ARMUPR;
wait1Msec(1000); motor[fr] = 0; //Arm down
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
motor[fr] = 30; //Forward
motor[br] = 30;
motor[bl] = -30;
motor[fl] = -30;
wait1Msec(1200);
motor[fr] = 0; //Dispose
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
motor[jaw] = 40;
wait1Msec(1500);
motor[jaw] = 0;
motor[fr] = -30; //Backward
motor[br] = -30;
motor[bl] = 30;
motor[fl] = 30;
wait1Msec(1200);
motor[fr] = 0; //Arm down
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
PlaySound(soundBeepBeep);
servo[arml] = ARMDOWNL;
servo[armr] = ARMDOWNR;
wait1Msec(1000);
motor[fr] = 30; //Forward
motor[br] = 30;
motor[bl] = -30;
motor[fl] = -30;
wait1Msec(1200);
motor[fr] = 30; //Sideways
motor[br] = -30;
motor[bl] = -30;
motor[fl] = 30;
wait1Msec(500);
dir = sgn(time);
motor[fr] = dir*30; //Back to coordinates
motor[br] = dir*-30;
motor[bl] = dir*-30;
motor[fl] = dir*30;
wait1Msec(time);
motor[fr] = 0;
motor[br] = 0;
motor[bl] = 0;
motor[fl] = 0;
}
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
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int _dirEnh, _strEnh;
// 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 Enh");
// 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
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2, _dirEnh, _strEnh))
break; // I2C read error occurred
displayText3(1, "D", _dirDC, _dirAC, _dirEnh);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText3(4, "2", dcS3, acS3, _strEnh);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
if (HTSMUXreadPowerStatus(HTSMUX))
nxtDisplayTextLine(7, "Batt: bad");
else
nxtDisplayTextLine(7, "Batt: good");
}
}
//=============================================================================================================================================
//---------------------------------------------------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 _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int _dirEnh, _strEnh;
// 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
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2, _dirEnh, _strEnh))
break; // I2C read error occurred
displayText3(1, "D", _dirDC, _dirAC, _dirEnh);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText3(4, "2", dcS3, acS3, _strEnh);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
nxtDisplayTextLine(7, "Enter to switch");
}
}
}