void initializeRobot()
{
// Sensors are config'ed and setup by RobotC (need to stabalize).
// The IR sensor needs this command (it seems like RobotC doesn't
// initialize third-party sensors automatically.
HTIRS2setDSPMode(infrared, g_IRsensorMode);
Servo_SetSpeed(servo_IR, 10); // maximum speed!
Servo_SetSpeed(servo_claw, 10); // maximum speed!
Servo_SetSpeed(servo_ramp, 100); // slowly update so ramp doesn't release.
Servo_Rotate(servo_IR, g_IRServoExtended); // will fold back up in tele-op
Servo_Rotate(servo_claw, g_clawServoExtended); // will be folded in tele-op
Servo_Rotate(servo_ramp, g_rampServoDefault); // stops ramp from deploying
Motor_SetMaxSpeed(g_FullRegulatedPower);
Motor_ResetEncoder(motor_L);
Motor_ResetEncoder(motor_R);
Motor_ResetEncoder(motor_lift);
// Wait this long so the claw & IR servos get to update.
// The ramp-release servo shouldn't move; the long update time
// is to prevent sudden jerks that might release the ramp.
// We don't need to wait for the IR sensor to stabalize since
// the robot doesn't read from it until it's at the first column,
// which should be ample time for RobotC to setup the sensor.
Time_Wait(10);
return;
}
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
servo[gravityShelf] = SHELFDOWN;
servo[IRServo] = IRUP;
/*
* Assume lined up perpendicular to the pegs.
*/
HTMCsetTarget(HTMC);
nMotorPIDSpeedCtrl[driveLeft] = mtrSpeedReg;
nMotorPIDSpeedCtrl[driveRight] = mtrSpeedReg;
nMotorPIDSpeedCtrl[driveSide] = mtrSpeedReg;
/*
* Do not let the motors coast
*/
bFloatDuringInactiveMotorPWM = false;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode mode = DSP_1200;
// set the DSP to the new mode
HTIRS2setDSPMode(IRSeeker, mode);
return;
}
// 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("#");
}
}
}
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;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
//
// 1. Move motors and servos to a preset position.
// 2. Some sensor types take a short while to reach stable values during which time it is best that
// robot is not moving.
//
//
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void initializeRobot()
{
//Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
//IR Mode set
tHTIRS2DSPMode _mode = DSP_1200;
HTIRS2setDSPMode(IRSensor, _mode);
//scoop all the way up to hold the autonomous block
servo[servoScoop] = 0;
servo[servoIRDrop]= 175;
return;
}
void initializeRobot()
{
LSsetActive(lineFollower);
setWrist (.95);
while (!TSreadState (liftDownSensor))
motor [linearSlides]=-100;
motor [linearSlides]=0;
servo [rampLatch] = rampLatchClosed;
servo [seekerPivot]= seekerUp;
HTIRS2setDSPMode(irSeeker, DSP_1200);
wait1Msec (500);
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
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;
}
void initializeRobot()
{
// Place code here to initialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
// DSP mode is set to 1200 Hz
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to initialize IR sensor
if (HTIRS2setDSPMode(IRSeeker, _mode) == 0)
{
// if fail, display error and make noise
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "DSP INIT FAILED");
PlaySound(soundException);
wait10Msec(300);
return;
}
return;
}
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;
}
void initializeRobot()
{
tHTIRS2DSPMode mode = DSP_1200;
HTIRS2setDSPMode(IRSeeker, mode);
servoChangeRate[blockDump] = 5;
servo[blockDump] = BLOCK_SERVO_RETRACTED;
servo[middleElev] = SERVO_STOPPED;
lightStripInit(0x1F);
displayRestingPulse();
/*
* Work around the RobotC bug
*/
initSegment(dead_reckon_from_b3, 0, 0, -12, 100);
initSegment(dead_reckon_from_b3, 1, -90, -24, 100);
initSegment(dead_reckon_from_b3, 2, -90, 32, 100);
initSegment(dead_reckon_from_b3, 2, 0, 0, 0);
initSegment(dead_reckon_from_b2, 0, 0, -23, 100);
initSegment(dead_reckon_from_b2, 1, -90, -24, 100);
initSegment(dead_reckon_from_b2, 2, -90, -32, 100);
initSegment(dead_reckon_from_b2, 2, 0, 0, 0);
initSegment(dead_reckon_from_b1, 0, 0, -45, 100);
initSegment(dead_reckon_from_b1, 1, -90, -24, 100);
initSegment(dead_reckon_from_b1, 2, -90, -32, 100);
initSegment(dead_reckon_from_b1, 2, 0, 0, 0);
initSegment(dead_reckon_from_b0, 0, 0, -55, 100);
initSegment(dead_reckon_from_b0, 1, -90, -24, 100);
initSegment(dead_reckon_from_b0, 2, -90, -32, 100);
initSegment(dead_reckon_from_b0, 2, 0, 0, 0);
}
task main()
{
initializeRobot();
int motorLeft = 0;
int motorRight = 0;
waitForStart();
motor[driveLt]=100;
motor[driveRt]=100;
// IRseek loop
int _dirDC = 0;
int _dirAC = 0;
// DC and AC values from 5 internal detectors.
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
if (HTIRS2setDSPMode(HTIRS2, _mode) == 0)
{
// unsuccessful at setting the mode.
// display error message.
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "Init failed!");
nxtDisplayCenteredTextLine(3, "Connect sensor");
nxtDisplayCenteredTextLine(4, "to Port 1.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task. FTC-TD-0004-Rev-001-IRSeeker Page 12 of 16 9/14/2012 10:54:00 AM
wait10Msec(300);
return;
}
while((dcS3 < IRSEEK_CUTOFF) && (acS3 < IRSEEK_CUTOFF)){
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
}
wait10Msec(IRSEEK_POST_CUTOFF_WAIT);
servo[servo1] = 244;
wait10Msec(50);
servo[servo1] = 5;
wait10Msec(50);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=30;
motor[driveRt]=-30;
wait10Msec(50);
motor[driveLt]=80;
motor[driveRt]=80;
wait10Msec(100);
motor[driveLt]=0;
motor[driveRt]=0;
while (true)
{}
}
// 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 _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);
}
}
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 //
////////////////////////////
*/
}
// main task
task main ()
{
int last = 0;
int stage = 1;
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// show the user what to do
displayInstructions();
while(true)
{
// You can switch between the two different DSP modes by pressing the
// orange enter button
PlaySound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
nNumbCyles = 0;
++nInits;
while (true)
{
if ((nNumbCyles & 0x04) == 0)
nxtDisplayTextLine(0, "Initializing...");
else
nxtDisplayTextLine(0, "");
nxtDisplayCenteredBigTextLine(1, "IR Seekr");
// set the DSP to the new mode
if (HTIRS2setDSPMode(HTIRS2, _mode))
break; // Sensor initialized
++nNumbCyles;
PlaySound(soundShortBlip);
nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
nxtDisplayCenteredTextLine(6, "Connect Sensor");
nxtDisplayCenteredTextLine(7, "to Port S2");
wait1Msec(100);
}
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
if (_mode == DSP_1200)
nxtDisplayTextLine(0, " DC 1200");
else
nxtDisplayTextLine(0, " DC 600");
while (true)
{
++nNumbCyles;
if (nNxtButtonPressed == kEnterButton)
{
// "Enter" button has been pressed. Need to switch mode
_mode = (_mode == DSP_1200) ? DSP_600 : DSP_1200;
while(nNxtButtonPressed == kEnterButton)
{
// Wait for "Enter" button release
}
break;
}
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
//displayText(7, "Last", dcS5, acS5);
//nxtDisplayTextLine(7, "Last: "+last);
/*
if(acS3 >= 50){
int speed = 20;
motor[DFR] = -speed;
motor[DFL]= -speed;
motor[DBR] = -speed;
motor[DBL]= -speed;
}
else if(acS4+acS5 >= 10){
//Right
int speed = 20;
motor[DFR] = speed;
motor[DFL]= -speed;
motor[DBR] = speed;
motor[DBL]= -speed;
}
else if(acS1+acS2 >= 10){
//Left
int speed = 20;
motor[DFR] = -speed;
motor[DFL]= speed;
motor[DBR] = -speed;
motor[DBL]= speed;
}
else{
motor[DFR] = 0;
motor[DFL]= 0;
motor[DBR] = 0;
motor[DBL]= 0;
}
*/
while(stage == 1){
int acS1, acS2, acS3, acS4, acS5 = 0;
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
if(acS1 > acS2-100&&last>10){
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = -20;
last = -1;
stage = 2;
wait1Msec(1000);
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = 0;
}
else{
if(last<acS1)
last = acS1;
int speed = 90-3*(acS2 + acS3);
if (speed < 9) speed = 9;
motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = speed;
}
}
while(stage == 2){
//Right
//stage_done = 1;
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
int speed = 20-acS3;
if(speed < 9) speed = 9;
motor[DFR] = -speed;
motor[DFL]= speed;
motor[DBR] = -speed;
motor[DBL]= speed;
if(acS3>last)
last = acS3;
if(last-acS3 > 1&&last!=0){
motor[DFR] = 0;
motor[DFL]= 0;
motor[DBR] = 0;
motor[DBL]= 0;
stage = 3;
wait1Msec(1000);
}
// nxtDisplayTextLine(0, "Last: "+last);
// nxtDisplayTextLine(1, "acS3: "+dcS3);
wait1Msec(500);
}
}
}
}
// main task
task main ()
{
int _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 2.");
// 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 ) //while(maxSig<200)
{
// 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);
// first translate directional index so 0 is straight ahead.
val = _dirAC - 5;
// If the beacon is within a few centimeters of the detector, stop the robot
if(maxSig > 200)
{
motor[mLeft] = 0;
motor[mRight] = 0;
}
// Turn the robot to the left
else if (_dirAC > 5)
{
motor[mLeft]=30;
motor[mRight]=0;
}
// Turn the robot to the right
else if (_dirAC < 5)
{
motor[mLeft]=0;
motor[mRight]=30;
}
// If the beacon can't be found, spin in a circle until you find it again
else if(maxSig < 20)
{
motor[mRight] = 30;
motor[mLeft] = -30;
}
// Just drive straight
else
{
motor[mLeft]=50;
motor[mRight]=50;
}
nxtDisplayTextLine(6, "MainBatt: %2.2fv", externalBatteryAvg/1000.0);
nxtDisplayTextLine(7, "NxtBatt: %2.2fv", nAvgBatteryLevel/1000.0);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//////////////////////////////////BEGIN AUTONOMOUS CODE/////////////////////////////////////////////////////////
int dir;
int strength1, strength2, strength3, strength4, strength5;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode mode = DSP_1200;
// You can switch between the two different DSP modes by pressing the
// orange enter button
while (true)
{
// set the DSP to the new mode
if (HTIRS2setDSPMode(IRSeeker, mode))
break; // Sensor initialized
PlaySound(soundShortBlip);
nxtDisplayCenteredTextLine(6, "Connect Sensor");
nxtDisplayCenteredTextLine(7, "to Port S2");
wait1Msec(100);
}
eraseDisplay();
while (true)
{
// read the current modulated signal direction
dir = HTIRS2readACDir(IRSeeker);
if (dir < 0)
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(IRSeeker, strength1, strength2, strength3, strength4, strength5 ))
break; // I2C read error occurred
string tmpStr;
StringFormat(tmpStr, "Direction: %d", dir);
nxtDisplayTextLine(3, tmpStr);
StringFormat(tmpStr, "Strength: %d", strength3);
nxtDisplayTextLine(4, tmpStr);
if (dir < 5) {
// turn right
motor[driveRight] = 100;
motor[driveLeft] = -100;
} else if (dir > 5) {
// turn left
motor[driveRight] = -100;
motor[driveLeft] = 100;
} else if (strength3 < 110) {
motor[driveRight] = 100;
motor[driveLeft] = 100;
} else {
motor[driveRight] = 0;
motor[driveLeft] = 0;
}
}
while (true)
{}
}
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()
{
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);
}
}
