task main()
{
int irsearch = 1;
int dirIR1 = 0;
int dirIR2 = 0;
int dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 = 0;
int dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 = 0;
while (irsearch==1)
{
dirIR1 = HTIRS2readACDir(irsensor1);
HTIRS2readAllACStrength(irsensor1, dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 );
//HTIRS2readEnhanced(irsensor1, dirEnh1, strEnh1);
dirIR2 = HTIRS2readACDir(irsensor2);
HTIRS2readAllACStrength(irsensor2, dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 );
//HTIRS2readEnhanced(irsensor2, dirEnh2, strEnh2);
nxtDisplayTextLine(7, "%d,%d,%d,%d,%d", dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2);
nxtDisplayTextLine(6, "%d,%d,%d,%d,%d", dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1);
}
}
task main()
{
eraseDisplay();
if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeeker) {
writeDebugStreamLine("IRL is HTSMUXIRSeeker");
}
else if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeekerNew ) {
writeDebugStreamLine("IRL is HTSMUXIRSeekerNew");
}
else {
writeDebugStreamLine("IRL is unknown");
}
if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeeker) {
writeDebugStreamLine("IRR is HTSMUXIRSeeker");
}
else if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeekerNew ) {
writeDebugStreamLine("IRR is HTSMUXIRSeekerNew");
}
else {
writeDebugStreamLine("IRR is unknown");
}
while(true)
{
eraseDisplay();
byte status = HTSMUXreadStatus(S1);
displayTextLine(1, "Status: %d", status);
if(status & HTSMUX_STAT_BATT == HTSMUX_STAT_BATT) {
displayTextLine (1, "no power to MUX");
PlaySound(soundException);
// PlaySound(soundBeepBeep);
}
int currentIRL = HTIRS2readACDir(IRL);
int currentIRR = HTIRS2readACDir(IRR);
int irl1, irl2, irl3, irl4, irl5 = -1;
int irr1, irr2, irr3, irr4, irr5 = -1;
HTIRS2readAllACStrength(IRL, irl1, irl2, irl3, irl4, irl5 );
HTIRS2readAllACStrength(IRR, irr1, irr2, irr3, irr4, irr5 );
displayTextLine(2, "D %d %d", currentIRL, currentIRR);
displayTextLine(3, "0 %d %d", irl1, irr1);
displayTextLine(4, "1 %d %d", irl2, irr2);
displayTextLine(5, "2 %d %d", irl3, irr3);
displayTextLine(6, "3 %d %d", irl4, irr4);
displayTextLine(7, "4 %d %d", irl5, irr5);
}
}
task updateIRVals() {
while(true)
{
if(!safe)
{
HTIRS2readAllACStrength(leftIRDev,leftFive[0],leftFive[1],leftFive[2],leftFive[3],leftFive[4]);
HTIRS2readAllACStrength(rightIRDev,rightFive[0],rightFive[1],rightFive[2],rightFive[3],rightFive[4]);
left = HTIRS2readACDir(leftIRDev);
right = HTIRS2readACDir(rightIRDev);
safe = true;
}
}
}
// Y is our IR input, X is our dependant variable.
task main()
{
clearDebugStream();
while(true)
{
sum_y = 0; //from _x->_y
sum_xTimesy = 0;
irInputSum = 0;
for (int j = 0; j<20; j++)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
irInputSum += acS3;
}
irInputAvg = (irInputSum/20);
for (int i = 0; i<20 ; i++)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
if (abs(irInputAvg-acS3) > 20)
{
acS3 = irInputAvg;
}
sum_y += acS3; //READINGS: Fill our array with data from the middle node
//sum_y += irInput[i]; //CALCULATE: The sum of each value in the array. (its easiest this way)
sum_xTimesy += (xInput[i] * acS3) ;//EQUATION: Top left half
writeDebugStream("%i,", acS3);
writeDebugStreamLine("0");
Sleep(10);
}//APROOVED
mean_xTimesy = (sum_x * sum_y)/20; //EQUATION: Top right half
slopeLOBF = (sum_xTimesy-mean_xTimesy)/(sum_xSquared - mean_sum_xSquared); //CALCULATE: LOBF slope
if (slopeLOBF<0)
{
PlayTone(1000,13);
}
//y_intLOBF = (sum_y/10) - (5.5 * slopeLOBF); //CALCULATE: LOBF y-intercept -> 5.5 = mean_x
/*
while (y_intLOBF >= y_intTH) //So long as we are above our y-intercept threshold AKA know we are close to the top of the curve
{
if (slopeLOBF <= slopeTH || slopeTH >=) //Wait untill our LOBF slope falls within our calibrated threshold
{
//Make beeping noise perhaps turn left because we have determined the IRSeekerV2 to be facing the IRBeacon
}
}
*/
writeDebugStream("0,");
writeDebugStreamLine("%f, ", slopeLOBF);
Sleep(50);
}
}
void getcenterpos()
{
//default is 2 because #2 routine works for positions 1 & 2
int acS1, acS2, acS3, acS4, acS5;//irsensor values
// int acS1avg=0, acS2avg=0, acS3avg=0, acS4avg=0, acS5avg=0; //these were for testing the irsensor
// int prevacS1, prevacS2, prevacS3, prevacS4, prevacS5; //these were for testing the irsensor
int irthreshold=58.5;//to decide if position 3 or something else
int irthreshold2=10;//to decide if position 2 or 1
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);//used to get ir sensor values
if(acS4>irthreshold)//finding position of centerpiece
{
centerpos=3;
}
else if(acS4>irthreshold2)
{
centerpos=2;
}
else
{
centerpos=1;
}
}
// 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.
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 2.");
// make a noise to get their attention.
PlaySound(soundDownwardTones);
// wait so user can read message, then leave main task.
wait10Msec(300);
return;
}
// initialize the array sTextLines.
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display some header info at top of screen
eraseDisplay();
nxtDisplayTextLine(0, " Piece Of Cake ");
nxtDisplayTextLine(1, "------3763-------");
// loop continuously and read from the sensor.
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
nxtDisplayTextLine(2, "Position %d", _dirAC);
nxtDisplayTextLine(3, "Strength0 %d", acS1);
nxtDisplayTextLine(4, "Strength1 %d", acS2);
nxtDisplayTextLine(5, "Strength2 %d", acS3);
nxtDisplayTextLine(6, "Strength3 %d", acS4);
nxtDisplayTextLine(7, "Strength4 %d", acS5);
// wait a little before resuming.
wait10Msec(5);
}
}
task main()
{
HTIRS2setDSPMode(irsensor, DSP_1200);
HTIRS2setDSPMode(irsensor2, DSP_1200);
while(true) {
int irArray[5];
int irArray2[5];
HTIRS2readAllACStrength(irsensor, irArray[0], irArray[1], irArray[2], irArray[3], irArray[4]);
HTIRS2readAllACStrength(irsensor2, irArray2[0], irArray2[1], irArray2[2], irArray2[3], irArray2[4]);
int p = (irArray[1]-irArray[2])-10;
writeDebugStream("(%d) ",p);
for (int i = p+20; i > 0;i-=2){
writeDebugStream("#");
}
}
}
// 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");
}
}
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;
}
}
float strengthRight(){
int ac1,ac2,ac3,ac4,ac5 = 0; // reading of all sensors to compute "strength"
float s;
HTIRS2readAllACStrength(irRight, ac1, ac2, ac3, ac4, ac5 ); // reading the strength of all the sensors of the left beacon
s = ac1+ac2+ac3+ac4+ac5;
return(s);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the tele-op robot operation Customize as appropriate for
// your specific robot
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot Most tele-op programs will follow the following
// logic:
// 1 Loop forever repeating the following actions:
// 2 Get the latest game controller / joystick settings that have been received from the PC
// 3 Perform appropriate actions based on the joystick + buttons settings This is usually a
// simple action:
// * Joystick values are usually directly translated into power levels for a motor or
// position of a servo
// * Buttons are usually used to start/stop a motor or cause a servo to move to a specific
// position
// 4 Repeat the loop
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings
//
// At the end of the tele-op period the FMS will autonmatically abort (stop) execution of the program
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
bool bAtBeacon(void)
{
int zones[5];
HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]);
clearDebugStream();
for(int i = 0; i < 5; i++)
{
writeDebugStreamLine("zone %d:\t%d", i, zones[i]);
}
return ((zones[1] > 50) && (zones[2] > 50) && (abs(zones[1] - zones[2]) < 10));
}
task infared(){
int _dirAC1 = 0;
//int 1acS1, 1acS2, 1acS3, 1acS4, 1acS5 = 0;
int _dirAC2 = 0;
//int 2ac2S1, 2acS2, 2acS3, 2acS4, 2acS5 = 0;
while(true){
_dirAC1 = HTIRS2readACDir(IRS1);
if (_dirAC1 < 0)
break; // I2C read error occurred
_dirAC2 = HTIRS2readACDir(IRS2);
if (_dirAC2 < 0)
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(IRS1, acS1a, acS2a, acS3a, acS4a, acS5a))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(IRS2, acS1b, acS2b, acS3b, acS4b, acS5b))
break; // I2C read error occurred
}
}
task main() {
float r0 = getIRDir(sensorIR)-8, r1;
if(r0 > 0) rbtArcRight(-7); else rbtArcLeft(20);
rbtMoveFdDist(-10, 5000);
ClearTimer(T1); while(time1[T1] < 2000) {
r1 = getIRDir(sensorIR)-8; int acS[5]; HTIRS2readAllACStrength(sensorIR, acS[0], acS[1], acS[2], acS[3], acS[4]);
if(r0 > 0) {setLeftMotors(acS[4] > acS[3] ? -6 : -50); setRightMotors(acS[4] > acS[3] ? -50 : -8);}
else {setLeftMotors(acS[4] > acS[3] ? -6 : -90); setRightMotors(acS[4] > acS[3] ? -30 : 0);}
} setLeftMotors(0); setRightMotors(0); int cr = (r0 > 0 ? (r1 > 0 ? 1 : 2) : (r1 > 0 ? 3 : 4));
nxtDisplayBigTextLine(3, "%f", cr); for(;;);
}
void searchDirection()
{
int _dirAC = 0;
int trueDir = 0;
int maxSig = 0;
int acS1, acS2, acS3, acS4, acS5;
zeroEncoder();
// IR direction search algorithm
while(_dirAC != 5)
{
// Read the current IR signal directions
_dirAC = HTIRS2readACDir(IRSeeker);
if ((_dirAC < 0))
{
writeDebugStreamLine("Read dir ERROR!");
break; // I2C read error occurred
}
// Read the individual signal strengths of the internal sensors
if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5))
{
writeDebugStreamLine("Read dir ERROR!");
break; // I2C read error occurred
} else {
// find the max sig strength of all detectors
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
// which way to go?
// 0 = no signal found
// 1 = far left (~8 o'clock)
// 5 = straight ahead (~12 o'clock)
// 9 = far right (~4 o'clock)
encoderCount = (nMotorEncoder[Lwheel] + nMotorEncoder[Rwheel]) / 2;
// raw, dirty direction -> nice, workable direction! :-)
trueDir = abs(_dirAC - 5);
// only concerned with moving back/forward
motor[Lwheel] = 20 + 5 * trueDir;
motor[Rwheel] = 20 + 5 * trueDir;
wait1Msec(20);
}
}
// Move foward until the beacon is found.
void MovetoIR()
{
int FindState = 1;
bool FoundIt = false;
nMotorEncoder[motorL] = 0;
nMotorEncoder[motorR] = 0;
wait1Msec(200);
// Start moving.
driveMotors(DRIVE_SPEED, DRIVE_SPEED);
while(!FoundIt)
{
switch (FindState)
{
case 1:
// Look for target
// Get the direction.
_dirAC = HTIRS2readACDir(IRseeker);
// Make 0 straight ahead, all positive, no left or right worry.
_dirAC = abs(_dirAC - 5);
// Get the strength.
nxtDisplayTextLine(1, "IR: %d", _dirAC);
HTIRS2readAllACStrength(IRseeker, acS1, acS2, acS3, acS4, acS5);
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
nxtDisplayTextLine(2, "maxSig: %d", maxSig);
wait10Msec(2);
if (_dirAC >= irGoal)
{
StopMotors();
DistanceToIR = nMotorEncoder[motorR];
FindState++;
}
break;
case 2:
// Look for strongest signal.
FindState++;
break;
case 3:
// Backup a little.
FindState++;
FoundIt = true;
break;
}
}
}
// 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);
}
}
void searchStrength()
{
int acS1, acS2, acS3, acS4, acS5;
int acTemp[3];
bool offTarget = true;
// IR max strength search
while(offTarget)
{
// get input
for (int i = 0; i <= 2; ++i)
{
motor[Lwheel] = 15;
motor[Rwheel] = 15;
wait10Msec(50);
// Read the individual signal strengths of the internal sensors
if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5))
{
writeDebugStreamLine("Read dir ERROR!");
break; // I2C read error occurred
} else {
nxtDisplayCenteredBigTextLine(3, "Sig=%d", acS3);
acS3 = acTemp[i];
}
}
// compare 3 IR strength readings
if ((acTemp[2] > acTemp[1]) && (acTemp[2] > acTemp[0]))
{
motor[Lwheel] = 15;
motor[Rwheel] = 15;
wait10Msec(20);
} else if ((acTemp[2] < acTemp[1]) && (acTemp[2] > acTemp[0])) {
motor[Lwheel] = -13;
motor[Rwheel] = -13;
wait10Msec(20);
} else {
offTarget = false;
stopRobot();
}
wait1Msec(500);
}
}
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;
}
/* Checks to see if we are next to the beacon */
bool bAtBeacon(void) {
/* Declare variables */
int zones[5];
int average = 0;
bool atBeacon = false;
/* Read the infrared seeker into an array */
HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]);
/* Find the average infrared noise level */
for(int i = 0; i < 5; i++) average += zones[i];
average /= 5;
/* If the critical zones are stronger than the noise, we are in range */
if(((zones[1] - average) > 10) && ((zones[2] - average) > 10)) inRange = true;
/* When we are in range and the critical zones are nearly equal (or we have overshot) we are at the beacon */
if(inRange && ((zones[1] - zones[2]) > -15)) atBeacon = true;
return atBeacon;
}
int findIRBeacon_v2()
{
int acS1, acS2, acS3, acS4, acS5, _dirAC = 0;
int max = 0;
bool done;
int dir;
tHTIRS2DSPMode _mode = DSP_1200;
HTIRS2setDSPMode(IRSeeker, _mode);
moveBackwardOn(50);
while (!done) {
HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5);
dir = HTIRS2readACDir(IRSeeker);
if (dir == 5) {
rangeFind();
}
}
return 0;
}
task main()
{
initializeRobot();
// waitForStart(); // Wait for the beginning of autonomous phase.
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
// CONFIGURATION VARIABLES //
int driveTime = 3000; //in MILLISECONDS 1000 MS = 1 second //
int bucketDump = 1600;
int bucketStop = 800;
int L_Flip_Open = 15;
int L_Flip_Close = 150;
int R_Flip_Close = 85;
int R_Flip_Open = 230;
///////////////////////////////////////////////////////
// BEGIN AUTONOMOUS ROUTINE //
///////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
// BEGIN IR ROUTINE
/////////////////////////////////////////////////////////////
int _dirAC = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int maxSig = 0; // the max signal strength from the seeker.
int val = 0; // the translated directional value.
// 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.
wait10Msec(300);
return;
}
eraseDisplay();
// loop continuously and read from the sensor.
while(true)
{
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
// Get the AC signal strength values.
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
// display info
nxtDisplayCenteredBigTextLine(1, "Dir=%d", _dirAC);
nxtDisplayCenteredBigTextLine(4, "Sig=%d", maxSig);
// figure out which direction to go...
// a value of zero means the signal is not found.
// 1 corresponds to the far left (approx. 8 o'clock position).
// 5 corresponds to straight ahead.
// 9 corresponds to far right.
// first translate directional index so 0 is straight ahead.
val = _dirAC - 5;
// calculate left and right motor speeds.
motor[Left_drive] = 30 * val;
motor[Right_drive] = 30 * val;
// wait a little before resuming.
wait10Msec(2);
}
/////////////////////////////////////////////////////////////
// END IR ROUTINE
/////////////////////////////////////////////////////////////
/*
// Drive forward for drivetime @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = -20;
wait1Msec(driveTime);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Set Flippers to Open //
servo[Left_Flipper] = L_Flip_Open;
servo[Right_Flipper] = R_Flip_Open;
wait1Msec(2000);
// Rotate clockwise for 1000ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Perform lift cycle //
nMotorEncoderTarget[bucket] = bucketDump; // target bucket dumping position
motor[bucket] = -25;
while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon
{
// Do not continue.
}
motor[bucket] = 0;
// if(SensorValue(touchBucketZero)== 0)
// {
nMotorEncoderTarget[bucket] = bucketStop; // target bucket drive position
motor[bucket] = 25;
while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon
{
// Do not continue.
}
motor[bucket] = 0;
// Set Flippers to Close //
servo[Left_Flipper] = L_Flip_Close;
servo[Right_Flipper] = R_Flip_Close;
wait1Msec(1000);
// Rotate counterclockwise for 1000ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Drive forward for 1000ms = drivetime/3 @ 20% power //
motor[Left_drive] = 20;
motor[Right_drive] = 20;
wait1Msec(driveTime/3);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Rotate counterclockwise for 500ms @ 20% power //
motor[Left_drive] = -20;
motor[Right_drive] = 20;
wait1Msec(2000);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
// Drive forward for 3500ms @ 20% power //
motor[Left_drive] = 40;
motor[Right_drive] = 40;
wait1Msec(2500);
motor[Left_drive] = 0;
motor[Right_drive] = 0;
////////////////////////////
// End Autonomous Routine //
////////////////////////////
*/
}
task main()
{
int highest_seen_value = 0;
// TODO go forward here
// wait for the cliff in IR readings, then break
while(acS1 < cliff_drop_threshold)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
}
nxtDisplayString(3, "hit cliff");
// start trying to find the spike
bool going_down = false;
while(!going_down)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
int previous_ir_value[10];
int previous_ir_value_difference[9];
// set the new highest seen value
if (highest_seen_value < acS3) highest_seen_value = acS3;
for (int i = 0; i < 10; i++)
{
int number_of_negative_ir_differences = 0;
// add the new data
if (i < 10)
{
// we're in the middle (or start) of the stack, shift lower
previous_ir_value[i] = previous_ir_value[i+1];
} else
{
// i == 10, so we're at the top of the stack
// populate with the new data
previous_ir_value[i] = acS3;
}
// if this is negative, then we're probably going down
previous_ir_value_difference[i] = previous_ir_value[i+1] - previous_ir_value[i];
if (previous_ir_value_difference[i] < 0)
{
number_of_negative_ir_differences++;
}
// determine if we're going down with a threshold of 7/9 or more
if (number_of_negative_ir_differences >= going_down_threshold) going_down = true;
}
}
// now the trend is definitely going down, let's back up and find the highest value
// TODO this will stop slightly before the top
nxtDisplayString(1, "pull me backwards");
// TODO go back here
bool reached_highest_value = false;
while(!reached_highest_value)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
if (highest_seen_value + curve_top_change_threshold > acS3 &&
acS3 < highest_seen_value - curve_top_change_threshold)
{
reached_highest_value = true;
}
}
}
// 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 to read in all sensors to workspace variables
//===================================================
task sensors()
{
float currDir = 0.0; //prevDir = 0.0,
long currtime,prevtime;
LSsetActive(LEGOLS); // set the LEGO light sensor to active mode
//-------------------------
// gyro
//-------------------------
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25); // if not we beep indefinitely
wait1Msec(500);
}
//SMUX_good = true;
while(calibrate != 1){}; // wait for a request to start calibrating the gyro
wait1Msec(300); // short delay to ensure that user has released the button
HTGYROstartCal(HTGYRO); // initiate the GYRO calibration
drift = MyHTCal(gyroCalTime*1000);
Driver_Cal = HTGYROreadCal(HTGYRO); // read the calculated calibration value for saving to file
//---------------------------------------
// write the GYRO calibration data to file for Tele-Op
//---------------------------------------
Delete(sFileName, nIoResult); // delete any pre-existing file
nFileSize = 100; // new file size will be 100 bytes
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create and open the new file
WriteFloat( hFileHandle, nIoResult, drift); // write the current drift value to the file
WriteFloat( hFileHandle, nIoResult, Driver_Cal); // write the driver calibration to the file
Close(hFileHandle, nIoResult); // close the file
//---------------------------------------
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10) // if there is too much spread we beep 5 times to alert the drive team
{
gyroTrue = true;
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2; // this signifies to the main program that calibration has been completed
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
//wait1Msec(1);
//---------------------------------------------------------------------
// Read both sonar sensors and filter out non-valid echo readings (255)
// If there is no echo the filter just retains the last good reading
//---------------------------------------------------------------------
sonarRaw = USreadDist(LEGOUS); // read the rear mounted sensor
if (sonarRaw!=255) sonarLive = sonarRaw; // and copy valid results to workspace
sonarRaw2 = USreadDist(LEGOUS2); // read the side mounted sensor
if (sonarRaw2!=255) sonarLive2 = sonarRaw2; // and copy valid results to workspace
//-------------------------
// LEGO light sensor
//-------------------------
light_normalised = LSvalNorm(LEGOLS); // read the LEGO light sensor
//-------------------------
// HiTechnic IR Sensor
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2); // Read the IR bearing from the sensor
bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334
currDir = (float) bearingAC; // copy into workspace -
/*if (bearingAC == 0) // No IR signal is being detected
{
currDir = prevDir; // so retain the previous reading
}
else // otherwise read all the IR segments
{
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
IR_Bearing=currDir-5; // and setup the main variable for others to use
*/
HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]);
HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]);
//-----------------------------------
// code for the peaks of IR sensor 1
//-----------------------------------
if (bearingAC!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS[i]>maximum)
{peak = i;
maximum = acS[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS[peak] != 0)) // we are not working with extreme value
{
if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS[peak-1]>acS[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS[peak]-acS[peak-1])/ // calculate the bias away from the peak
max(acS[peak], acS[peak-1]));
}
else
{
offset = 25*(1-(float)(acS[peak]-acS[peak+1])/
max(acS[peak], acS[peak+1]));
}
}
}
IR_Bearing = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
//-----------------------------------
// code for the peaks of IR sensor 2
//-----------------------------------
if (bearingAC2!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS2[i]>maximum)
{peak = i;
maximum = acS2[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS2[peak] != 0)) // we are not working with extreme value
{
if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS2[peak-1]>acS2[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/ // calculate the bias away from the peak
max(acS2[peak], acS2[peak-1]));
}
else
{
offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/
max(acS2[peak], acS2[peak+1]));
}
}
}
IR_Bearing2 = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
}
}
task sensors()
{
//-------------------------
// gyro
//-------------------------
long currtime,prevtime;
int acS[5];
while (HTSMUXreadPowerStatus(S3)) // check battery power is on
{
PlayTone(750,25);
wait1Msec(500);
}
SMUX_good = true;
while(calibrate != 1){};
wait1Msec(300);
HTGYROstartCal(HTGYRO);
float drift = MyHTCal(gyroCalTime*1000);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (abs(highest-lowest)>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
newgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
//-------------------------
// IR
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2);
#define max(a, b) (((a) > (b))? (a): (b))
#define min(a, b) (((a) < (b))? (a): (b))
currDir = (float) bearingAC;
if (bearingAC == 0)
{
currDir = prevDir;
}
else
{
if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]))
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
////-------------------------
//// Sonar
////-------------------------
//num = USreadDist(LEGOUS);
//num2 = USreadDist(LEGOUS2);
//if(num != 255) sonarLive = num;
//if(num2 != 255) sonarLive2 = num2;
//-------------------------
// light
//-------------------------
LSsetActive(LEGOLS);
nrm = LSvalNorm(LEGOLS);
}
}
// main task
task main ()
{
int _dirAC1 = 0;
int _dirAC2 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int acS12, acS22, acS32, acS42, acS52 = 0;
int maxSig = 0; // the max signal strength from the seeker.
int maxSig2 = 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 || HTIRS2setDSPMode(ir2, _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 Ports 1 and 4.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task.
wait10Msec(300);
return;
}
eraseDisplay();
// loop continuously and read from the sensor.
while(true)
{
// read the current modulated signal direction
_dirAC1 = HTIRS2readACDir(HTIRS2);
_dirAC2 = HTIRS2readACDir(ir2);
if (_dirAC1 < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
if (_dirAC2 < 0)
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
// Get the AC signal strength values.
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
if (!HTIRS2readAllACStrength(ir2, acS12, acS22, acS32, acS42, acS52 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig2 = (acS12 > acS22) ? acS12 : acS22;
maxSig2 = (maxSig2 > acS32) ? maxSig2 : acS32;
maxSig2 = (maxSig2 > acS42) ? maxSig2 : acS42;
maxSig2 = (maxSig2 > acS52) ? maxSig2 : acS52;
}
// display info
nxtDisplayCenteredTextLine(1, "DirL = %d", _dirAC1);
nxtDisplayCenteredTextLine(4, "DirR = %d", _dirAC2);
nxtDisplayCenteredTextLine(2, "SigL = %d", maxSig);
nxtDisplayCenteredTextLine(5, "SigR = %d", maxSig2);
// figure out which direction to go...
// a value of zero means the signal is not found.
// 1 corresponds to the far left (approx. 8 o'clock position).
// 5 corresponds to straight ahead.
// 9 corresponds to far right.
// first translate directional index so 0 is straight ahead.
// wait a little before resuming.
wait10Msec(2);
}
}
// 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");
}
}
}
// 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()
{
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);
}
}