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;
}
// 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()
{
int middle = 256/2;
servo[servo1] = 0;
getJoystickSettings(joystick);
tHTIRS2DSPMode _mode = DSP_1200;
HTIRS2setDSPMode(IR, _mode);
int dir = HTIRS2readDCDir(IR);
bool buttonpressed = false;
while(true){
if(!buttonpressed)
{
if(joystick.joy1_TopHat == 2)
{
servo[servo1] = zones[dir - 1];
buttonpressed = true;
}
else if(joystick.joy1_TopHat == 6)
{
servo[servo1] = zones[dir + 1];
buttonpressed = true;
}
else if(joystick.joy1_TopHat == 0)
{
servo[servo1] = servo[servo1] - 1;
buttonpressed = true;
}
else if(joystick.joy1_TopHat == 4)
{
servo[servo1] = servo[servo1] + 1;
buttonpressed = true;
}
}
else if (buttonpressed && (joystick.joy1_TopHat != 2 && joystick.joy1_TopHat != 6))
{
buttonpressed = false;
}
if(joy1Btn(1) == 1)
{
servo[servo1] = 128;
}
else if (joy1Btn(2) == 1)
{
servo[servo1] = 0;
}
else if (joy1Btn(3) == 1)
{
servo[servo1] = 255;
}
string hi = "hi";
nxtDisplayString(1, hi);
dir = HTIRS2readDCDir(IR);
nxtDisplayString(2, "%d", dir);
nxtDisplayString(3, "%d", servo[servo1]);
}
}
task main()
{
InitializeTeleop();
//waitForStart();
StartTask(DriveTank);
while(true)
{
nxtDisplayTextLine(2, "%d", TSreadState(touchSensor));
nxtDisplayTextLine(1, "%d", HTIRS2readDCDir(irSensor));
/*
getJoystickSettin
//Goal grabber cogs(joystick);
mmands
if(joy1Btn(8) == 1)
{
GrabGoal();
}
if(joy1Btn(7) == 1)
{
ReleaseGoal();
}
//Collector commands
if(joy2Btn(8) == 1)
{
CollectBalls();
}
else if(joy2Btn(7) == 1)
{
ReleaseBalls();
}
else
{
StopCollector();
}
//Container commands
if(joystick.joy2_TopHat == 0)
{
HoldBalls();
}
else if(joystick.joy2_TopHat == 4)
{
DumpBalls();
}
//Lift Commands
//This is for using the lift without encoders
if(joystick.joy2_y2 > ANALOG_DEAD_ZONE)
{
RaiseLift();
}
else if(joystick.joy2_y2 < -1*ANALOG_DEAD_ZONE)
{
LowerLift();
}
else
{
StopLift();
}
//This is for using the lift with the encoder
if(joy2Btn(1) == 1)
{
MoveLifter(DownPos);
}
else if(joy2Btn(2) == 1)
{
MoveLifter(LowGoalPos);
}
else if(joy2Btn(3) == 1)
{
MoveLifter(MediumGoalPos);
}
else if(joy2Btn(4) == 1)
{
MoveLifter(HighGoalPos);
}
else if(joy2Btn(10) == 1)
{
MoveLifter(CenterGoalPos);
}
*/
}
}
int getIrDirection(short sensor){
int dirDC = HTIRS2readDCDir(sensor);
return dirDC;
}
// 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(){
int offset = 45;
int waitTime = 0;
initializeRobot();
nVolume = 4;
//allow the user to select a delay before te robot begins the autonomous routine
while(nNxtButtonPressed != 3){
if(nNxtButtonPressed == 1){
while(nNxtButtonPressed == 1){}
waitTime+=1;
}
if(nNxtButtonPressed == 2){
while(nNxtButtonPressed == 2){}
waitTime-=1;
}
if(waitTime < 0){waitTime = 0;}
nxtDisplayCenteredBigTextLine(4, "Time: %1.d", waitTime);
//play a sound until the user selects a time
if(!bSoundActive){PlaySound(soundBlip);}
}
waitTime = waitTime*1000;
nVolume = 1;
HTMCsetTarget(compass);
waitForStart();
//wait for the pre-designated time, just in case our alliance partner
//needs time
wait1Msec(waitTime);
HTMCsetTarget(compass);
//move arm to ready position
/*while(!moveTo(arm, 3000, 30, 100, 20)){}
for(int i = 0; i < 3; i++){
motor[arm] = 100;
wait1Msec(500);
motor[arm] = -100;
wait1Msec(600);
}
motor[arm] = 0;
polarDrive(x1,x2,y1,y2,100,offset+0,0);
wait1Msec(300);
polarDrive(x1,x2,y1,y2,-100,offset+0,0);
wait1Msec(300);
polarDrive(x1,x2,y1,y2,0,offset+0,0);
*/
while(!moveTo(arm, 10000, 30, 100, 20)){}
//move along the crates while watching for the beacon
//when beacon encountered, slow down and dump block
bool dropped = false;
while(nMotorEncoder[x2] > -9500){
if(HTIRS2readDCDir(IR) == 6){
if (dropped == false){
stopPolarMot(x1,x2,y1,y2);
polarDrive(x1,x2,y1,y2,60,offset+90,0);
wait1Msec(400);
stopPolarMot(x1,x2,y1,y2);
servo[scoop] = 250;
wait1Msec(1000);
servo[scoop] = 0;
polarDrive(x1,x2,y1,y2,60,offset-90,0);
dropped = true;
}
}
else {
polarDrive(x1,x2,y1,y2,60,offset-90,0);
}
}
nMotorEncoder[x2] = 0;
while(nMotorEncoder[x2] > -4500){
polarDrive(x1,x2,y1,y2,75,offset+0,0);
}
stopPolarMot(x1,x2,y1,y2);
while(HTMCreadRelativeHeading(compass) > -70){
polarDrive(x1,x2,y1,y2,0,0,-75);
}
stopPolarMot(x1,x2,y1,y2);
nMotorEncoder[x2] = 0;
while(nMotorEncoder[x2] < 5000){
polarDrive(x1,x2,y1,y2,50,offset+180,0);
}
stopPolarMot(x1,x2,y1,y2);
}
// 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);
}
}
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Pos 1: 0 -> 0
//Pos 2: 0 -> 5
//Pos 3: 5
Move(-23, 0.6);
wait10Msec(100);
int irPos = 0;
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
{
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
irPos = 1;
else
irPos = 2;
}
else
irPos = 3;
writeDebugStreamLine("%d", irPos);
if (irPos == 3)
{
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
getUltraAngle(false);
Move(-15, 0.5);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(100);
//Move(-34, 0.6);
wait10Msec(1);
score();
}
else if (irPos == 2)
{
Turn(-60);
wait10Msec(1);
Move(-26, 0.4);
wait10Msec(1);
Turn(104);
wait10Msec(1);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
wait10Msec(1);
Turn(90 - getUltraAngle(false));
wait10Msec(1);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(1);
score();
}
else
{
//leave zone
Move(13, 1);
wait10Msec(10);
Turn(28);
wait10Msec(10);
Move(-65, 1);
wait10Msec(10);
Turn(-23);
wait10Msec(10);
Move(-37, 0.9);
wait10Msec(10);
Move(-10, 0.2);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//while(true){}
float Angle = 0;
Angle = getUltraAngle(true);
/*if (Angle == -1)
Move(-5, 0.3);
else if (Angle >= 86 && Angle <= 90)
break;*/
writeDebugStreamLine("%f", Angle);
Turn(Angle - 90);
float dist = -10;
Move(dist, 0.25);
//grab goal
wait10Msec(30);
servo[hook1] = 235;
servo[hook2] = 30;
wait10Msec(150);
Turn(90 - Angle);
wait10Msec(10);
Move(117, 1);
wait10Msec(10);
Turn(-100);
wait10Msec(10);
Move(-25, 0.5);
wait10Msec(10);
//score
motor[lift] = 75;
raising = true;
timeValue = 800;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//Move(3, 0.2);
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
servo[output] = 130;
//release goal
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(90);
}
if (irPos != 1)
{
//KickStand
wait10Msec(200);
servo[output] = 130;
motor[lift] = -75;
bool raising = false;
int timeValue = 4000;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
Turn(90);
wait10Msec(1);
Move(-10, 0.5);
wait10Msec(1);
Turn(80);
wait10Msec(1);
Move(20, 0.9);
}
}
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()
{
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
//---------//
#define forwardSpeed 0.4 /*0.5 sideways speed is good. sets forwards speed. (1 = full sensitivity)*/
#define sideSpeed 0.8 /*sets sideways speed. (1 = full sensitivity)*/
#define slowModeThreshold 20 /*sets "slow mode" threshold.(1-128) (when going below this speed in either
direction DON'T move diagonally. makes driving slowly easier.)*/
#define fastMultiplier 2 /*how much faster to go in fast mode (1 = standard speed.)*/
#define slowMultiplier 0.5 /*how much slower to go in slow mode(1 = standard speed.)*/
#define backboardUp 250
#define backboardDown 15
#define grabberDown 100
#define grabberUp 140
int IRLeftValue = HTIRS2readDCDir(IRLeft);
int IRRightValue = HTIRS2readDCDir(IRRight);
float gyroValue = HTGYROreadCal(gyro);
float gyroValue2 = HTGYROreadRot(gyro);
int ultrasoundValue = USreadDist(ultrasound);
task displaySensorValues()
{
while(true)
{
IRLeftValue = HTIRS2readDCDir(IRLeft);
IRRightValue = HTIRS2readDCDir(IRRight);
gyroValue = HTGYROreadCal(gyro);
