task main()
{
//waitForStart(); // Wait for the beginning of autonomous phase.
min = SensorRaw[frontLight];
StartTask(UpdateMax);
while(SensorRaw[frontLight] <= min + 50){
motor[fr] = 10;
motor[fl] = 15;
motor[br] = 10;
motor[bl] = 15;
}
motor[fr] = 0;
motor[fl] = 0;
motor[br] = 0;
motor[bl] = 0;
StopTask(UpdateMax);
StartTask(followline);
while(HTIRS2readACDir(frontRightIR) != 5 && HTIRS2readACDir(backRightIR) != 9){
}
StopTask(followline);
irturn();
liftUpAndDownAndTakeDumpAndForward();
getbackontape();
while (true)
{}
}
task main()
{
int i;
int segment;
i = 20;
disableDiagnosticsDisplay();
eraseDisplay();
servo[rightEye] = i;
wait1Msec(500);
while (i < 255) {
servo[rightEye] = i;
wait1Msec(100);
segment = HTIRS2readACDir(irr_right);
nxtDisplayCenteredBigTextLine(4, "%d %d", segment, i);
if (segment == 6) {
break;
}
i++;
}
while(true) {
segment = HTIRS2readACDir(irr_right);
nxtDisplayCenteredBigTextLine(2, "%d %d", segment, i);
}
}
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);
}
}
int read_total()
{
// This function adds together the values of the two IR sensors (1-9, based on the direction of the beacon) for triangulation purposes
int direction1 = HTIRS2readACDir(irSensor1);
int direction2 = HTIRS2readACDir(irSensor2);
int total = direction1 + direction2;
return total;
}
void locateIR () { //Locates IR Beacon
int slowMovement = 25;
while(HTIRS2readACDir(IR_SENSOR1) != 4 && HTIRS2readACDir(IR_SENSOR1) != 6){ //Moves until IR Sensor gets inbetween IR fields 4 and 6
motor [motor_left] = slowMovement;
motor [motor_right] = slowMovement;
}
stops(); //Stops movement
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart();
disableDiagnosticsDisplay();
//Initialize the gyro and turning
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
//Start actual movement code
moveForwardInches(60,43);//initial forwards
turn(g_PidTurn,45,20);
clearEncoders();
wait1Msec(50);
const int totalTics = 6806;//total tics from before IR to end- CHANGED FOR LESSENED AMOUNT OF FORWARDS
const int ticsToCenter = 3663;//tics from start to central beam
const int ticsToSubtract = 1665;//failsafe, may still need testing
//finding IR
while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //finds the beacon zone 4 (rough)
//nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
startBackward(27);
}
stopDrive();
wait1Msec(300);
while(HTIRS2readACDir(IR) != 5 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //slow down to look for basket (fine)
startForward(15);
}
int currentPosition = abs(nMotorEncoder[leftDrive]);
if (currentPosition > ticsToCenter)//check where we are
moveForwardInchesNoReset(20, 6);//move forwards 5 inches (buckets 1 and 2)
else
moveForwardInchesNoReset(20, 3);//forwards 3 inches (buckets 3 and 4)
stopDrive();//stops robot
servo[dumper] = 30;//dumps the block
motor[lift]= 50;//starts the lift up
wait1Msec(700);
motor[lift]= 0;//stops lift
servo[dumper] = servoRestPosition;//resets servo
int ticsToMove= totalTics - abs(nMotorEncoder[leftDrive]);//tics left after IR
displayCenteredTextLine(0,"TTM: %d", ticsToMove);
moveBackwardTics(90, ticsToMove); //move to end after IR
turn(g_PidTurn, -87,40); //turn to go towards ramp
moveForwardInches(90, 44, false, LEFTENCODER); //forwards to ramp
turn(g_PidTurn, 95, 40); //turn to face ramp
moveForwardInches(90, 45, false, LEFTENCODER);//onto ramp
motor[lift]= -50;//starts the lift down
wait1Msec(500);
motor[lift]= 0;//stops lift
while(true){}
}
void getbackontape(){
while(HTIRS2readACDir(frontRightIR) != 5 && HTIRS2readACDir(backRightIR) != 9){
motor[fr] = 17;
motor[fl] = -17;
motor[br] = 17;
motor[bl] = -17;
}
}
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;
}
}
}
void rotateToBeacon ( )
{
while (true){
if (HTIRS2readACDir(irSeeker)< 5)
drive (0,0, -speed);
else if (HTIRS2readACDir(irSeeker)> 5)
drive (0, 0, speed);
else
{
drive (0,0,0);
return;
}
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
/* while ( LSvalNorm(lineFollower) > 27 && LSvalNorm(lineFollower) < 35 ) {
drive(0,speed,0);
wait1Msec(20);
}
wait1Msec (800);
drive(0,0,0); */
//drive onto platform.
drive (0, speed, 0);
wait1Msec (300);
while (LSvalNorm (lineFollower) > lineBlack) {
drive (0, 0, 0);
rotateToBeacon ( );
drive (0, speed, 0);
wait1Msec (200);
}
drive (0,speed,0);
wait1Msec (200);
drive (0,0,0);
//find line
while ((LSvalNorm (lineFollower) < lineWhite) && (LSvalNorm (lineFollower) > lineGray))
{
if (HTIRS2readACDir(irSeeker)< 5)
drive (speed,0,0);
else if (HTIRS2readACDir(irSeeker)> 5)
drive (-speed, 0, 0);
wait1Msec (200);
}
drive (0,0,0);
setWrist (.6);
/*motor [linearSlides]=100;
wait1Msec (3300); //BOTTOM ROW
motor [linearSlides]= 0;*/
while (true)
{}
}
int getIRReading(tSensors ir_seeker)
{
wait1Msec(1); // Wait 1 ms
int ir = HTIRS2readACDir(ir_seeker); // IR receiver -> ir variable
wait1Msec(1); // Down time before recheck
return ir;
}
// 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()
{
waitForStart();
initializeRobot();
wait1Msec(12000);
//sets seeker value
tHTIRS2DSPMode _mode = DSP_1200;
//Starts the first basket movements
moveRobot(firstBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
int seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to second basket movements
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to third basket movements
moveRobot(thirdBasketInches, forwardSpeedBlock, forward);
wait1Msec(seekerReadWait);
seekerValue = HTIRS2readACDir(IRRight);
nxtDisplayTextLine(0,"Sensor Value %d ",seekerValue);
//Sensor found, proceed to scoring
if(seekerValue >= autoSensorLowValue && seekerValue <= autoSensorHighValue){
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches- 1, forwardSpeedMove, backwards);
}
//No sensor found so score in fourth basket
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
void irturn(){
while(HTIRS2readACDir(frontRightIR) > 1){
motor[fr] = -17;
motor[fl] = 17;
motor[br] = -17;
motor[bl] = 17;
}
}
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;
}
}
// 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");
}
}
//////////////////////////////////////////////////////////////////////////////////////
//
// IR Functions
//
//////////////////////////////////////////////////////////////////////////////////////
int MoveUntilIR()
{
//1. Reset the encoders
nMotorEncoder[motorLeft] = 0;
//2. Start the motors, different speeds as motor has a bias
motor[motorRight] = 40;
wait1Msec(50);
motor[motorLeft] = 40;
//3. Set the variables to track the numbe of revolutions, so robot knows its position on the field
int currentTicks = 0;
string sTemp;
// 4. Detect the IR beam
// Read direction from the IR in a while loop
// While loop exits, if either 5 is detected or if robot is past the 4th basket
int direction = HTIRS2readACDir(IRSensor);
while( direction != 5 ) {
// read the direction
direction = HTIRS2readACDir(IRSensor);
// read the number of ticks - that is how far the robot has moved since it started
currentTicks = nMotorEncoder[motorLeft];
// check to see if we are past the 4th basket
if (currentTicks >= BASKET_ZONE4_MAX )
{
//IR Seeker did not detect the IR beam, we are way past the 4th basket,
//need to stop moving
break;
}
//sprintf(sTemp, "%d", currentTicks);
//nxtDisplayString(2, sTemp);
}
//5. Stop moving
motor[motorRight] = 0;
motor[motorLeft] = 0;
//6. Return Ticks Moved
return currentTicks;
}
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()
{
waitForStart();
initializeRobot();
wait1Msec(12000);
//sets seeker value
tHTIRS2DSPMode _mode = DSP_1200;
//Starts the first basket movements
moveRobot(firstBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to second basket movements
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
//No sensor found so move to third basket movements
moveRobot(thirdBasketInches, forwardSpeedBlock, forward);
//wait1Msec(seekerReadWait);
//Sensor found, proceed to scoring
if(HTIRS2readACDir(IRLeft) == autoSensorValue){
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches- 1, forwardSpeedMove, backwards);
}
//No sensor found so score in fourth basket
moveRobot(secondBasketInches, forwardSpeedBlock, forward);
scoreRobot(firstBasketInches + secondBasketInches + thirdBasketInches + secondBasketInches - 1, forwardSpeedMove, backwards);
}
int initialPosition(){
int maximumIRL = -1;
int maximumIRR = -1;
int initialPos = -1;
for(int i=1; i<=50; i++){
int currentIRL = HTIRS2readACDir(IRL);
int currentIRR = HTIRS2readACDir(IRR);
maximumIRL = (currentIRL > maximumIRL) ? currentIRL : maximumIRL;
maximumIRR = (currentIRR > maximumIRR) ? currentIRR : maximumIRR;
}
if(maximumIRL == 5 && maximumIRR == 5){
initialPos = 3;
}
else if(maximumIRL == 5 && maximumIRR == 6){
initialPos = 2;
}
else if(maximumIRL == 6 && maximumIRR == 7){
initialPos = 1;
}
writeDebugStreamLine("initial position is %d", initialPos);
displayTextLine(0, "initial position is %d", initialPos);
return initialPos;
}
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);
}
}
void showSensorValues() {
eraseDisplay();
int currentIRL = HTIRS2readACDir(IRL);
int currentIRR = HTIRS2readACDir(IRR);
if (currentIRL != previousIRL || currentIRR != previousIRR){
displayTextLine(2, "IR is %d,%d", currentIRL, currentIRR);
previousIRL = currentIRL;
previousIRR = currentIRR;
}
int currentTouch = SensorValue(touch);
if (currentTouch != previousTouch){
displayTextLine(3, "Touch is %d", currentTouch);
previousTouch = currentTouch;
}
int currentSonar = SensorValue(sonar);
if (currentSonar != previousSonar){
displayTextLine(4, "Sonar is %d", currentSonar);
previousSonar = currentSonar;
}
}
// 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);
}
}
int findIRBeacon()
{
int dir;
int i;
for (i = 0; i < 4; i++) {
moveBackward(travelDistance[i], 50);
wait1Msec(500);
dir = HTIRS2readACDir(IRSeeker);
showIRSegment(dir);
nxtDisplayTextLine(2, "Reading %d", dir);
if (dir == 5) {
return i;
}
}
return -1;
}
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;
}
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() {
int IRDir;
while (true) {
IRDir = HTIRS2readACDir(HTIRS2);
int leftBound = 4;
int rightBound = 6;
if (IRDir == 5 || IRDir == leftBound || IRDir == rightBound) {
// Center peg
eraseDisplay();
nxtDisplayCenteredTextLine(4, "Center %d", IRDir);
}
else if (IRDir < leftBound) {
// Left peg
eraseDisplay();
nxtDisplayCenteredTextLine(4, "Left %d", IRDir);
}
else if (IRDir > rightBound) {
// Right peg
eraseDisplay();
nxtDisplayCenteredTextLine(4, "Right %d", IRDir);
}
}
}
