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)
{}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
motor [linearSlides]=100;
wait1Msec (3300); //BOTTOM ROW
motor [linearSlides]= 0;
/* while ( LSvalNorm(lineFollower) > 27 && LSvalNorm(lineFollower) < 35 ) {
drive(0,50,0);
wait1Msec(20);
}
wait1Msec (800);
drive(0,0,0); */
drive (50, 0, 0);
while (LSvalNorm (lineFollower) < 35);
drive (0, 0, 0);
setWrist (.6);
while (true)
{}
}
task main() {
int raw = 0;
int nrm = 0;
int tempRaw = 0;
int tempNrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "LIGHT");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S2 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 2");
wait1Msec(2000);
nxtDisplayClearTextLine(7);
nxtDisplayTextLine(5, "Press [enter]");
nxtDisplayTextLine(6, "to toggle light");
wait1Msec(2000);
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
if (raw != tempRaw){
nxtDisplayClearTextLine(5);
nxtDisplayTextLine(5, "Raw: %4d", raw);
tempRaw = raw;
}
if (nrm != tempNrm){
nxtDisplayClearTextLine(6);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
tempNrm = nrm;
}
wait1Msec(50);
}
}
void init(){
fs1 = fs2 = 0; //Force Sensor Values
servoPos = 0; //Initiate Claw Servo Pos
servo[servo1] = servoPos; //Set servo to servoPos
nMotorEncoder[armR] = 0; //Initiate Encoder Pos
servo[servo3]=35; //Initiate Servvo Ramp Lock
power=50; //Motor Power Level
LSsetActive(LEGOLS); //Turn on color sensor
lsVal = LSvalNorm(LEGOLS);
////Column positions left to right////
col1=col2=col3=0;
}
task main () {
int raw = 0;
int nrm = 0;
// Get control over the buttons
nNxtButtonTask = -2;
LSsetActive(LEGOLS);
eraseDisplay();
nxtDisplayTextLine(0, "Light Sensor Cal.");
nxtDisplayTextLine(2, "Left: set black");
nxtDisplayTextLine(3, "Right: set white");
nxtDisplayTextLine(7, "Grey: exit");
while (true) {
switch(nNxtButtonPressed) {
// if the left button is pressed calibrate the black value for the sensor
case kLeftButton:
LScalLow(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
// if the left button is pressed calibrate the white value for the sensor
case kRightButton:
LScalHigh(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Read the raw value of the sensor
raw = LSvalRaw(LEGOLS);
// Read the normalised value of the sensor
nrm = LSvalNorm(LEGOLS);
// Display the raw and normalised values
nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);
// Display the values for black and white
nxtDisplayTextLine(6, "B: %4d W: %4d", lslow, lshigh);
wait1Msec(50);
}
}
task main() {
short raw = 0;
short nrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
displayCenteredTextLine(0, "Lego");
displayCenteredBigTextLine(1, "LIGHT");
displayCenteredTextLine(3, "SMUX Test");
displayCenteredTextLine(5, "Connect SMUX to");
displayCenteredTextLine(6, "S1 and sensor to");
displayCenteredTextLine(7, "SMUX Port 1");
sleep(2000);
displayClearTextLine(7);
displayTextLine(5, "Press [enter]");
displayTextLine(6, "to toggle light");
sleep(2000);
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (getXbuttonValue(xButtonEnter))
{
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
sleep(500);
}
displayClearTextLine(5);
displayClearTextLine(6);
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
displayTextLine(5, "Raw: %4d", raw);
displayTextLine(6, "Norm: %4d", nrm);
sleep(50);
}
}
task main() {
int raw = 0;
int nrm = 0;
bool active = true;
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "Light Sensor");
nxtDisplayTextLine(2, "Press orange");
nxtDisplayTextLine(3, "button to switch");
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
// Turn the light off
LSsetInactive(LEGOLS);
else
// Turn the light on
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Get the raw value from the sensor
raw = LSvalRaw(LEGOLS);
// Get the normalised value from the sensor
nrm = LSvalNorm(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", raw);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
wait1Msec(50);
}
}
//===================================================
// 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 main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[handJoint] = 240;
wait1Msec(100);
black_threshold = LSvalNorm(middle_light);
white_threshold = black_threshold + 5;
writeDebugStreamLine("black_threshold: %d", black_threshold);
writeDebugStreamLine("white_threshold: %d", white_threshold);
//
// Move to other side of field
//
moveStraight(50,2000);
move(-50,50,450);
moveStraight(50,500);
//
// Find the line
//
// Continue forward until the middle sensor detects the white line
//
writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));
while (LSvalNorm(middle_light) < white_threshold){
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
setAllMotorVals(10);
}
allStop();
wait1Msec(100);
//
// Turn 90 degrees
//
// Move forward a little more and then turn until the middle sensor detects the white line
//
bool isLeft = false; // If this boolean is true, the 90 degree turn is toward the left
moveStraight(30,350);
//moveStraight(15,1000); // with long arm
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter1 = 0;
while (LSvalNorm(middle_light) < white_threshold){
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("left light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
if (isLeft){
if (counter1 > 500){
setDriveMotorVals(40,-40);
}
else{
setDriveMotorVals(20,-20);
}
}
else{
if (counter1 > 100){
setDriveMotorVals(-40,40);
}
else{
setDriveMotorVals(-20,20); // with long arm
}
}
counter1 += 1;
}
//
// Follow the line until touch sensor is triggered
//
// 1. Left on white, middle on white (or black), right on black: turn toward the left
// 2. Left on black, middle on white (or black), right on white: turn toward the right
// 3. Left on black, middle on white, right on black: move straight
// 4. Left on black, middle on black, right on black: lost...
//
// Set up touch sensor variables
int nButtonsMask;
bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
// If one has, this changes to true.
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter2 = 0;
// Initial check of the touch sensor values. If a robot is pushed up against the spear,
// do not deploy it.
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
if (isTouch == false){
//deploy the spear
deploySpear(true);
}
while (isTouch == false){
writeDebugStreamLine("-----------------counter2 val: %d", counter2);
// Read light sensor values
left_nrm = LSvalNorm(left_light);
middle_nrm = LSvalNorm(middle_light);
right_nrm = LSvalNorm(right_light);
//!--Code for debugging
//if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", left_nrm);
//temp_left_nrm = left_nrm;
//}
//if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
//temp_middle_nrm = middle_nrm;
//}
//if (right_nrm != temp_right_nrm) {
writeDebugStreamLine("middle light sensor value: %d", right_nrm);
//temp_right_nrm = right_nrm;
//}
//--!
if (left_nrm < black_threshold){
// left sensor is black
if (middle_nrm < black_threshold){
// middle sensor is black
if (right_nrm > black_threshold){
// right sensor is white or gray
// turn right
writeDebugStreamLine("Case 1: turned right");
if (counter2 > 600){
setDriveMotorVals(0,40);
}
else{
setDriveMotorVals(0,30);
}
}
else{
// right sensor is black
// lost, so just turn right a lot
setDriveMotorVals(-30,30);
}
}
else{
// middle sensor is not black
if (middle_nrm > white_threshold){
// middle sensor is white
// assume right sensor is black
// move straight
writeDebugStreamLine("Case 2: went straight");
if (counter2 > 600){
setAllMotorVals(40);
}
else{
setAllMotorVals(30);
}
}
else{
// middle sensor is gray
// assume right sensor is gray
// turn right
writeDebugStreamLine("Case 3: turned right");
if (counter2 > 600){
setDriveMotorVals(0,40);
}
else{
setDriveMotorVals(0,30);
}
}
}
}
else{
// left sensor is not black
if (left_nrm > white_threshold){
// left sensor is white
// assume middle sensor is black
// assume the right sensor is black too
// turn left
writeDebugStreamLine("Case 4: turned left");
if (counter2 > 600){
setDriveMotorVals(40,0);
}
else{
setDriveMotorVals(30,0);
}
}
else{
// left sensor is gray
// middle sensor is gray
// assume right sensor is black
// turn left
writeDebugStreamLine("Case 5: turned left");
if (counter2 > 600){
setDriveMotorVals(40,0);
}
else{
setDriveMotorVals(30,0);
}
}
}
counter2 += 1;
// Read touch sensor values
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
}
allStop();
wait1Msec(1000);
//
// More later....scoring stuff
//
// 1. Move backward a little
// 2. Unfold arm
// 3. Move forward a little
// 4. Move arm down to score
// 5. Back up
// 6. Reset arm
// 7. Move toward our rings?
//
// back up a little
moveStraight(-20,800);
// unfold arm
fold_arm(false);
// move forward a little
moveStraight(20,600);
// move arm down
motor[shoulderJoint] = -50;
wait1Msec(800);
motor[shoulderJoint] = 0;
// back up
moveStraight(-40,500);
// reset arm
fold_arm(true);
//retract spear
//deploySpear(false);
while (true){
return;
}
}
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);
}
}
//=============================================================================================================================================
//---------------------------------------------------BEGIN INITIALIZATION CODE-----------------------------------------------------------------
task main() {
//Initialize the display with the program choices
chooseProgram();
switch (PROGID) {
case 1:
FORWARD_SCORE_FORWARD_LINE_1 = true;
linesToFind = 1;
break;
case 2:
FORWARD_SCORE_FORWARD_LINE_2 = true;
linesToFind = 2;
break;
case 3:
FORWARD_SCORE_BACKWARD_LINE_1 = true;
linesToFind = 1;
break;
case 4:
FORWARD_SCORE_BACKWARD_LINE_2 = true;
linesToFind = 2;
break;
case 5:
useDummyAutonomous();
break;
case 6:
//useOriginalAutonomous();
PlaySoundFile("Woops.rso");
break;
}
//---------------------------------------------------------END INITIALIZATION CODE-------------------------------------------------------------
//=============================================================================================================================================
//if (PROGID == 1 || PROGID == 2 || PROGID == 3 || PROGID == 4) {
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
//PlaySound(soundBlip);
//wait1Msec((2 * PI) * 1000); //TAUUUU
//wait1Msec(10000);//wait 10 seconds for other teams who **might** have better autonomous code
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
servo[servoLift] = 123;
servo[servoSweep] = 199;
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
//float irval = acS3L;
//StringFormat(irvalres, "%3.0f", irval);
//WriteText(irFileHandle, IOResult, "Test");
//WriteString(irFileHandle, IOResult, irvalres);
//WriteByte(irFileHandle, IOResult, 13);
//WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
//if it doesnt find the beacon, dont bother returning to start if it has been set to
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
if (time1[T1] < 2000) {
wait1Msec(600);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
//irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
//Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
driveToEnd(-80, timeToEnd - roundTime);//drive to end of ramp
}
if (FORWARD_SCORE_BACKWARD_LINE_1 || FORWARD_SCORE_BACKWARD_LINE_2) {
driveToEnd(80, roundTime);
}
}
wait1Msec(300);
//=======================================================================================================================================
//------------------------BEGIN 90 DEGREE TURNS------------------------------------------------------------------------------------------
//HTGYROstartCal(HTGYRO);
ClearTimer(T3);
while (true) {
wait1Msec(20);
//if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degFirstTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
//---------------LINE DETECTOR--------------------------
LSsetActive(LEGOLS);
while (linesFound < linesToFind) {
motor[motorLeft] = -50;
motor[motorRight] = -50;
wait1Msec(10);
if (LSvalNorm(LEGOLS) >= WHITE_LINE_VALUE) {
linesFound++;
}
if (linesFound >= linesToFind) { //ever-present failsafe
break;
LSsetInactive(LEGOLS);
}
}
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degSecondTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
} else {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = -60;
motor[motorRight] = 60;
if (heading <= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
}
break;
}
}
//==================================================================================
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
float heading = 92;
HTGYROstartCal(HTGYRO);
while (koth) {
//wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
MissionImpossible();
/*
}//END MAIN IF PROGIDS THING
else if (PROGID == 5) {
useDummyAutonomous();
}
*/
}//task main
task main()
{
//Initiate Robot
init();
//Raise Arm
while(nMotorEncoder[armR]<5){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[armR]);
}
stop();
lsVal = LSvalNorm(LEGOLS);
while(lsVal>85){
lsVal = LSvalNorm(LEGOLS);
forward(10);
}
stop();
wait1Msec(500);
ClearTimer(T1);
while(time1[T1]<1000){
HTIRS2readAllDCStrength(HTIRS2,dcS1,dcS2,dcS3,dcS4,dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
nxtDisplayCenteredTextLine(1,"IR:");
nxtDisplayCenteredTextLine(2,"%i",acS1);
nxtDisplayCenteredTextLine(3,"%i",acS2);
nxtDisplayCenteredTextLine(4,"%i",acS3);
nxtDisplayCenteredTextLine(5,"%i",acS4);
nxtDisplayCenteredTextLine(6,"%i",acS5);
}
if(acS2<20&&acS4<20&&acS3>20)
goto column2;
else if(acS2>acS4)
goto column3;
else
goto column1;
////////First Column/////////////
column1:;
stop();
nxtDisplayCenteredTextLine(1,"Column One:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);
////////Second Column/////////////
column2:;
lsVal = LSvalNorm(LEGOLS);
while(time1[T1]<2000){
if(lsVal<50){
forward(15);
}
else{
left(20);
}
lsVal = LSvalNorm(LEGOLS);
}
stop();
//forward(20);
//wait1Msec(1000);
//stop();
//forward(30);
//wait1Msec(250);
//stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Two:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
////////Third Column//////////////
column3:;
stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Three:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
}