예제 #1
0
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);
  }
}
예제 #4
0
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;
}
예제 #5
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);
  }
}
예제 #7
0
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
		}
	}
}
예제 #9
0
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);
	}
}
예제 #11
0
//=============================================================================================================================================
//---------------------------------------------------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
예제 #12
0
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);}
}