예제 #1
0
// TODO: use correct sensor port (change to irBack)
task main() {
	calibrateGyro();

	{ // Drive to the IR basket, dump the block, drive back & square up against the wall
		int timeForward;

		ClearTimer(T1);
		driveBackward(DRIVE_SPEED);
		while (true) {
			if (time1[T1] > SEARCH_TIME)
				break;
			else if (SensorValue[irFront] == 2)
				break;
		}

		timeForward = time1[T1];

		// Keep moving to adjust for the first 2 / last 2 baskets
		if (timeForward < MIDDLE_TIME) {
			// Back up a little
			driveStop();
			wait1Msec(500);
			driveForward(FIRST_HALF_DELAY, DRIVE_SPEED);
			timeForward -= FIRST_HALF_DELAY;
		} else {
			// Back up a little
			driveStop();
			wait1Msec(500);
			driveForward(SECOND_HALF_BACKUP_TIME, DRIVE_SPEED);
			timeForward -= SECOND_HALF_BACKUP_TIME;
		}

		driveStop();
		flipper_flip();
		driveForward(timeForward + 1500, DRIVE_SPEED);
		motor[rightDrive] = DRIVE_SPEED;
		wait1Msec(500);
		driveStop();
	}

	{ // Drive next to the ramp & turn on to it
		motor[leftDrive] = -DRIVE_SPEED;
		wait1Msec(1050);
		motor[rightDrive] = -DRIVE_SPEED;
		wait1Msec(1300);
		driveStop();
		turnRightEuler(TURN_90_EULER, DRIVE_SPEED);
		PlaySound(soundBeepBeep);
		driveBackward(1500, DRIVE_SPEED);
	}
}
예제 #2
0
파일: Tess.c 프로젝트: ZackAlfakir/Tess
void navigate()
{
    extern enum driveState tess_drive_state;
    tess_ping = ping_cm(ULTRASONIC_PIN);
    debug("Distance = %i\n", tess_ping);
    if (tess_ping == 0 && tess_drive_state != IDLE)
    {
        tess_drive_state = IDLE;
        debug("Ultrasonic Error!\n", 0);
        setServo(0,0,"Error: in void navigate()\n");
    }
    else
    {    
        if (tess_ping < OBJECT_DISTANCE && tess_drive_state != TURNING)
        {
            //TODO stop and turn
            stop();
            debug("Turning on LED!\n", 0);
            high(LED_1);
            debug("About to turn!\n", 0);
            turn(LEFT); //enum from drivetrain
        }
        else if(tess_ping >= OBJECT_DISTANCE && tess_drive_state != MOVING)
        {
            driveForward(-1);
            low(LED_1);
        }
    }
}
예제 #3
0
task main()
{
	while(true)
	{
		readSensors();

		if (touchL != 0 || touchR != 0)
		{
			avoidObstacle();
		}
		else if (lightDetected && facingLight)
		{
		  driveForward();
	  }
		else if (lightDetected && !facingLight)
		{
			turnTowardsLight();
		}
		else
		{
			//Default case
		  scanForLight();
	}
	}
}
예제 #4
0
void reverse15Red()
{
	// variables
			int armMin = 300;
			int wallHeight = 1000;
			int goalHeight = 1550;
			int pot = analogRead (8);


			int encoder1 = 1200; //drive under wall
			int encoder2 = 136; // rotate 90 degrees
			int encoder3 = 1300; // backwards to the opponets goal
			int encoder4 = 900; //turn to bridge
			int encoder5 = 200; // knock their wallball
			int encoder6 = 200; // turn to other ball
			int encoder7 = 400; // knock their center ball
			int encoder8 = 400; // positioning



			// begin routine

			intakeDead(); // unfold
			delay(300); // needs to clear the wall..
			driveBack (encoder1); // drive backwards to under the bridge
			stopIntake();
			turnRight (encoder2); // turn ass to opponets goal
			driveBack (encoder3); // drive to opponets goal
			driveBackDead(); // 90 deg align
			delay(400);
			stopDrive ();
			armUp(wallHeight);// arm up
			scrapeLeft (encoder4); // turn to wall ball
			driveForward (encoder5); // knock off wall ball
			scrapeLeftBack(encoder6); // turn to middle ball
			driveForward(encoder7); // knock off middle ball
			driveBack (encoder8); // positioning

			stopAll ();




}
예제 #5
0
void kakitRed ()
{

// variables
	int armMin = 300;
	int wallHeight = 1000;
	int goalHeight = 1550;
	int dead1 = 1000;
	int dead2 = 2000;
	int dead3 = 3000;
	int pot = analogRead (8);


	int encoder1 = 1000;
	int encoder2 = 150;
	int encoder3 = 2000;
	int encoder4 = 75;
	int encoder5 = 75;
	int encoder6 = 75;
	int encoder7 = 75;
	int encoder8 = 75;
	int encoder9 = 75;


	// begin routine
	driveForwardDead (); //ram big balls
	intakeDead ();
	delay (3000);
	stopIntake();
	driveBackDead (4000); // wall align to 90 deg
	driveForward (encoder1);
	turnRight (encoder2); // turn towards buckys
	intakeDead ();
	//line follow forward (encoder 4)

	driveBackSlowDead (); // allign the bump
	driveBackDead();  // over the bump
	driveForwardSlowDead(); // alighn to bump
	driveBackSlow(encoder4);
	turnRight (encoder5);
	driveBackSlow (encoder6) ; // go under the bridge
	armUpDead (); // break the bridge
	delay(500);
	armDown (armMin);
	driveBack (encoder7);
	turnRight (encoder8);
	armUp (goalHeight);
	//line follow (encoder9);
	outtake(8000); // score all three balls in the goal

	stopAll ();

}
예제 #6
0
// ----------------------------------------------------------------------------
//  drive using light ambient sensors for directional command
// ------------------------------------------------------------[ driveByLight ]
void WRTbotDrive::driveByLight( wrt_motor_t &motor )
{
	// -------------------------------------------------------------------
	if( ( 0 == (motor.left.control  & wrt_motorControl_power_bit) ) ||
		( 0 == (motor.right.control & wrt_motorControl_power_bit) ) )
	{
		WRTbotDrive::stop();

		return;
	}

	// --- read ambient light sensor
	// -------------------------------------------------------------------
	sensor.light.data = analogRead( wrt_pin_ambientLight_sensor );

	Serial.println(" ");
	Serial.print( sensor.light.data );	Serial.print( " , " );

    // --- convert to ambient light units
	// -------------------------------------------------------------------
	sensor.light.data *= sensor.light.factor;

	Serial.print( sensor.light.data );	Serial.println( " , " );


	useNominal = true;
	// -------------------------------------------------------------------
	if( (sensor.light.data > sensor.light.limitMin) && 			// minimum limit
		(sensor.light.data < sensor.light.limitLowerBound) )	// highest lower bound
	{

		Serial.print( "driveLeft" );	Serial.println( "   " );


		driveLeft( motor );
	}
	else if( (sensor.light.data > sensor.light.limitUpperBound) &&	// lowest  upper bound
			 (sensor.light.data < sensor.light.limitMax) )			// maximum limit
	{

		Serial.print( "driveRight" );	Serial.println( "   " );

		driveRight( motor );
	}
	else
	{
		Serial.print( "driveForward" );	Serial.println( "  now " );

		driveForward( motor );
	}

}	// done driveByLight()
////////////////////////////////////////////////////////////////////////////////////
// Autonomous Routine supports "controlled" running, to allow steps to be replayed,
// Speeds adjusted, and more.
////////////////////////////////////////////////////////////////////////////////////
void our_auto(bool autoRun){
	writeDebugStreamLine("****** Starting Autonomous Function **********************");

	int stepCounter = 0;

	if(!autoRun){
		writeDebugStreamLine("--- AutoRun == false ---- ");
	}

	// Increment this when there's more steps!
	while(stepCounter < 20){
		writeDebugStreamLine("--Step number: %d", stepCounter);
		getStepSelection(stepCounter, autoRun);
  // .....................................................................................
  // This is the OFFICIAL Autonomous Routine.  Experiments go in a "SimpleTestHarness"
  // .....................................................................................


			switch(stepCounter){
				case 0:
					driveForward(1005, 100);
					break;
				case 1:
					spinLeft(45, 100);
					break;
				case 2:
					driveForward(1000, 100);
					break;
				case 3:
					spinLeft(90, 100);
					break;
				default:
					writeDebugStreamLine("--Step number %d not defined", stepCounter);
			}

			stepCounter++;
	} // End While loop
	writeDebugStreamLine("****** Completed Autonomous Function **********************");
}
예제 #8
0
void Motor_run()
{
  if(MPI_check_available(MOTOR_DRIVER_PID) == true)
    {
      MPI_get_message(MOTOR_DRIVER_PID, &incomingPID, &cmdSize, incomingMsg);

      if(cmdSize == sizeof(int))
	{
	  OS_memcpy(&incomingCmd, incomingMsg, cmdSize);

	  switch(incomingCmd)
	    {
	    case C_DRIVE_FORWARD:
	      UART_send("drive forward\r\n", CONSOLE_BASE);
	      driveForward();

	      break;

	    case C_DRIVE_REVERSE:
	      UART_send("drive reverse\r\n", CONSOLE_BASE);
	      driveReverse();

	      break;

	    case C_DRIVE_LEFT:
	      UART_send("drive left\r\n", CONSOLE_BASE);
	      driveLeft();
	      
	      break;

	    case C_DRIVE_RIGHT:
	      UART_send("drive right\r\n", CONSOLE_BASE);
	      driveRight();

	      break;

	    case C_DRIVE_HALT:
	      UART_send("drive halt\r\n", CONSOLE_BASE);
	      driveHalt();

	      break;

	    }

	}

    }

}
예제 #9
0
void avoidObstacle()
{
	driveBackward();
	wait1Msec(1000);

	if (touchL != 0)
	{
		lastHitDir = -1;
		rightDeg(45);
	}
	else if (touchR != 0)
	{
		lastHitDir = 1;
		leftDeg(45);
	}

	driveForward();
	wait1Msec(2000);
}
int main(){
while(1){
switch(state)
{
case 1:
	if(digitalRead(FRONT)==1)
	{	goUntilLROn();
		turnDirection(right);
	}
	if(digitalRead(RIGHT)==1)
	{
		driveForward();
	}
	else 
		turnRight(10);
default : break;
}
}
}
예제 #11
0
void classic15Blue()
{
	int armMin = 290;
	int wallHeight = 1000;
	int goalHeight = 1700;
	int dead1 = 1200;
	int dead2 = 2000;
	int dead3 = 3000;
	int pot = analogRead(8);

	//encoder values
	int encoder1 = 900; //drive to goal
	int encoder2 = 325; //keep going towards goal
	int encoder3 = 0; //drive slow, adjust to 90 degrees
	int encoder4 = 325; //back up
	int encoder5 = 1350; //back up across the barrier again
	int encoder6 = 80; // turn towards center large ball
	int encoder7 = 600; // hit 1st ball off the barrier
	int encoder8 = 350; // drive back
	int encoder9 = 200; // turn towards other large ball
	int encoder10 = 400; // hit 2nd ball off the barrier
	int encoder11 = 300; // drive back to square
	int encoder12 = 290; // turn to barf
	int encoder13 = 800; // drive to barf + pickup
	int encoder14 = 400;
	int encoder15 = 700;

	// begin routine

	intakeDead();
	delay(1000);
	stopIntake();

	// driveforward (some curve)////////////////////////////////////////////////////
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < encoder1)
	{
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
		motorSet(MOTOR_ARM_LEFT_BACK, 127);
		motorSet(MOTOR_ARM_LEFT_FRONT, 127);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);

///////////////////////////////////////////////////////////////////////////////

	//driveForward(encoder1); // drive to goal
	armUp(goalHeight); //raise arm after cross barrier
	driveForwardSlow(encoder2);  //keep going towards goal
	driveForwardSlowDead(); //drive slow, adjust to 90 degrees
	delay(1000);
	outtake(1700); // outtake
	driveBack(encoder4); // back up
	delay(300);
	armDown(armMin); // lower arm
	driveBack(encoder5); //back up across the barrier again
	delay(300);
	turnLeft(encoder6); // turn towards center large ball
	armUp(wallHeight); // arm up to wall height
	driveForward(encoder7); // hit it off the barrier
	delay(500);
	driveBack(encoder8); // drive back
	delay(300);
	turnRight(encoder9); // turn towards other large ball
	driveForward(encoder10); // hit 2nd ball off the barrier
	delay(500);
	driveBack(encoder11); // drive back to square
	delay(600);
	turnLeft(encoder12); // turn to barf
	delay(300);
	armDown(armMin);
	intakeDead(); // pick up barf
	driveForward(encoder13); // drive towards barf + intake it
	delay(500);

	//end of routine

	stopAll();
	delay(60000); ///////////////////////////////////////////////////////////////////////////////////

}
예제 #12
0
	/****** AUTO FUNCTIONS END *******/
	void Autonomous()
	{
		int counter=0;
		int autonomousEngagement = 0;
		DriverStationLCD *screen = DriverStationLCD::GetInstance();	
		compressor.Start(); //starts compressor class
		rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down
		leftArmSolenoid.Set(DoubleSolenoid::kReverse);
		/*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/
		if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1)
		{
			winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly
			dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear
			Wait(0.2); // Giving the pistons time to engage properly
			winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
			ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet
			Wait(0.2); // Giving the pistons time to engage properly
		}
		while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed
			{
			winchMotor.Set(1); //Now starts the winch motor to load the catapult
			}
		// If the Catapult Left &  Limit Switches are (0,0), (0,1), (1,0)
		{
			winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
			if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged
				{
					winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded.
					dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety
					Wait(0.1); // Giving the pistons time to engage properly
					winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
				}
			else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged
				{
					ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety
					Wait(0.1); // Giving the pistons time to engage properly
				}
		}
		/*** DONE LOADING THE CATAPULT ***/
		float pLower = 5; // min height of rectangle for comparison
		float pUpper = 15;	// max height of rectangle for comparison
		int criteriaCount = 1; // number of elements to include/exclude at a time
		int rejectMatches = 1;	// when set to true, particles that do not meet the criteria are discarded
		int connectivity = 1;	// declares connectivity value as 1; so corners are not ignored
		int filterFunction;	// removes small blobs
		int borderSetting;	// variable to store border settings, limit for rectangle
		int borderSize = 1;  // border for the camera frame (if you don't put this, DriverStation gets mad at you)
		ParticleFilterCriteria2 particleCriteria;	
		ParticleFilterOptions2 particleFilterOptions;
		int numParticles;
		particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use
		particleCriteria.lower = pLower; // The lower bound of the criteria range
		particleCriteria.upper = pUpper; // The upper bound of the criteria range
		particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
		particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
		particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
		particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
		particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
		while ((IsAutonomous()))
		{
			if (logitech.GetRawButton(4))
				{
					autonomousEngagement = 1;
				}
			if (autonomousEngagement == 0) // If real autonomous is not engaged start
			{
				if (logitech.GetRawButton(1))
					{
						driveForward();
					}
				if (logitech.GetRawButton(9))
				{
					dogSolenoid.Set(DoubleSolenoid::kForward);		// Brings the pneumatic piston backward to raise the retrieval arm
					winchMotor.Set(0.1);
					Wait(0.3);
					ratchetSolenoid.Set(DoubleSolenoid::kForward);	// Pushes the pneumatic piston forward to lower the retrieval arm
					while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
						{
							winchMotor.Set(1);
						}
				}
				if (logitech.GetRawButton(2))
				{
					autonomousCatapultRelease();
				}
				if (logitech.GetRawButton(3))
				{
					stopDriving();
				}
				if (logitech.GetRawButton(5))
				{
					turnLeft();
				}
				if (logitech.GetRawButton(7))
				{
					turnLeftMore();
				}
				if (logitech.GetRawButton(6))
				{
					turnRight();
				}
				if (logitech.GetRawButton(8))
				{
					turnRightMore();
				}
			}// If real autonomous is not engaged end
			HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
			imgpointer = camera.GetImage();	//tells camera to capture image
			ringLight.Set(Relay::kForward); //turns ringlight on
			BinaryImage* binIMG = NULL;	// declares a container to hold a binary image
			binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255);	// thresholds HSL image and places in the binary image container
			delete imgpointer;	// deletes the HSL image to free up memory on the cRIO
			Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image
			filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
			borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy
			delete binIMG; //Deletes the Binary image
			int functionCountParticles; // stores number of particles
			int particleAmount; // stores the number of particles for the measure particle function
			functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles
			int functionOne; // The first measuring particle function (specifically for particle #1)
			int functionTwo; // The second measuring particle function (specifically for particle #2)
			double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find
			double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds
			functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1
			functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2
			screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation
			screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation
			imaqDispose(modifiedImage); // Deletes the filtered image
			/**LEFT POSITION**/
			if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on,  switch set off
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward			
				if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10))
					
					// The target should be hot. Now it goes to the other goal.
					/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnRight();
					//driveForward();
					Wait(3); 
					stopDriving();
					//autonomousCatapultRelease();
				}
				else // The target isn't hot. So it starts going toward this not hot goal.
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnRight();
					driveForward();
					Wait(4);
					stopDriving();
					//autonomousCatapultRelease();
				}		
			}
			/**CENTER POSITION**/
			else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing 			
				if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
					/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnLeftMore();
					driveForward();
					Wait(3); 
					stopDriving();
					autonomousCatapultRelease();
				}
				else // The target isn't hot. So it starts going toward this not hot goal.
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					driveForward();
					Wait(3);
					stopDriving();
					autonomousCatapultRelease();
				}
			}
			/** RIGHT POSITION**/
			else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
				if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
					/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						turnLeftMore();
						driveForward();
						Wait(3); 
						stopDriving();
						autonomousCatapultRelease();
					}
					else // The target isn't hot. So it starts going toward this not hot goal.
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						driveForward();
						Wait(3);
						stopDriving();
						autonomousCatapultRelease();
					}
			}
			else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and  switch on
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); //  position and facing forward
				if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal.
					/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnLeft();
					driveForward();
					Wait(3);
					stopDriving();
					autonomousCatapultRelease();
				}
				else // The target isn't hot. So it starts going toward this not hot goal.
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					driveForward();
					Wait(3);
					stopDriving();
					autonomousCatapultRelease();
				}
			}
			counter++;
			screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get());
			screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter);
			screen->UpdateLCD();
		}
		compressor.Stop();
	}
예제 #13
0
task main() {
	servoPrep();
	Joystick_WaitForStart();
	disableDiagnosticsDisplay();
	while(true) {
		encoderPrep();
		startTrackers();
		driveForward(310.0);
		turnRight(85.0);
		if(irDetected == true) {
			irPos2 = true;
			turnRight(40.0);
			irDetected = false;
			driveBackward(25.0);
			if(irDetected == true) {
				driveBackward(15.0);
				raiseLift(750.0);
				dropBallCenter();
				wait1Msec(300);
				servo[centerServo] = endPosCenter - 30;
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				driveForward(105.0);
				turnRight(85.0);
				driveBackward(300.0);
			}
			else	{
				irPos1 = true;
				driveBackward(110.0);
				turnRight(46.0);
				driveBackward(135.0);
				turnRight(3.0);
				raiseLift(750.0);
				dropBallCenter();
				wait1Msec(300);
				servo[centerServo] = endPosCenter - 30;
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				driveForward(130.0);
				turnRight(82.0);
				driveBackward(300.0);
			}
		}
		else	{
			driveForward(115.0);
			// ir loop
			if(irDetected == true) {
				irPos3 = true;
				driveBackward(15.0);
				raiseLift(750.0);
				dropBallCenter();
				wait1Msec(300);
				servo[centerServo] = endPosCenter - 30;
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				lowerLift(1000.0);
				driveForward(120.0);
				turnRight(85.0);
				driveBackward(300.0, 60);
				/*turnRight(43.0);
				driveBackward(250.0);
				motor[rightWheel] = -20;
				motor[leftWheel] = -20;
				wait1Msec(200);
				motor[rightWheel] = 0;
				motor[leftWheel] = 0;
				dropClamp();
				driveForward(10.0);
				raiseLift(700.0);
				dropBallGoal();
				resetDropGoal();*/
				break;
			}
			else	{
				driveBackward(220.0);
				turnRight(85.0);
				driveBackward(100.0);
			}
		}
		break;
	}
}
예제 #14
0
int main(void) {
	
	
	bt_init(BAUD_RATE);
	Sonar_init(CONTINUOUS);
	Servo_init(MANUAL, SCAN_AND_LOCK);
	motorDriverInit();
	
	while(1){ 
		bt_getStr( tab );								// Get string from buffer
		if(strlen( tab )){							// If isn't empty...
			//bt_sendStr( tab );						// ...send it back.
			if ( strcmp(tab, "w") == 0 ) {
				driveForward(speed);
			}
			else if ( strcmp(tab, "s") == 0) {
				driveReverse(speed);
			}
			else if ( strcmp(tab, " ") == 0) {
				stop();
			} 
			else if ( strcmp(tab, "a") == 0) {
				driveReverseLeftTrack(40); 
				driveForwardRightTrack(40);
			}
			else if ( strcmp(tab, "d") == 0) {
				driveForwardLeftTrack(40); 
				driveReverseRightTrack(40);
			}
			else if ( strcmp(tab, "ServoScanAndLock") == 0) {
				ServoChangeMode(SWEEP);
				ServoChangeSweepMode(SCAN_AND_LOCK);
			}
			else if ( strcmp(tab, "ServoScanAndGo") == 0) {
				ServoChangeMode(SWEEP);
				ServoChangeSweepMode(SCAN_AND_GO);
			}
			else if ( strcmp(tab, "ServoCenter") == 0) {
				ServoChangeMode(MANUAL);
				ServoMoveByDegree(0);
			}
			else if ( strncmp(tab, "SonarStartMeas",14) == 0) {
				int new_deg = ParseIntNumber(tab,14,3);
				SonarStartMeas(new_deg);
			}
			else if ( strncmp(tab, "SonarGetDistance",16) == 0) {
				char buffor[12];
				int new_deg = ParseIntNumber(tab,16,3);
				sprintf(buffor, "%04d,%04hu\n",new_deg,SonarGetDistance(new_deg));
				bt_sendStr(buffor);
			}
			else if ( strcmp(tab, "e") == 0) {
				if (speed<=90){
					speed+=10;
				}
			}
			else if ( strcmp(tab, "q") == 0) {
				if (speed>=10){
					speed-=10;
				}
			}
			else if (strncmp(tab, "speed",5) == 0){
				int new_speed = ParseIntNumber(tab,5,3);
				if (new_speed >= 0 && new_speed <= 100){
					speed = new_speed;
				}
			}
			else if ( strncmp(tab, "SonarLockRange",14) == 0) {
				int new_range = ParseIntNumber(tab,14,3);
				ServoChangeLockRange(new_range);
			}
		}
	} 
};
예제 #15
0
파일: 6-19-15.c 프로젝트: trsigg/VEX_NBN
task autonomous()/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
	int programmingSkills=0;
	int match=0;
	int red=0;
	int blue=0;
	int cube=0;
	int skyrise=0;
	SensorValue[leftLifttEncoder] = 0;
	SensorValue[rightLiftEncoder] = 0;

	if(SensorValue[autoPot2] > 1000) // competition or match
		{
			programmingSkills=1;
		}
		else

			match=1;

	if(SensorValue[autoPot] < 2500) //red or blue
		{
			red=1;
		}
		else

			blue=1;

	if(SensorValue[autoPot3] > 2000) // skyrise or cubes
		{
			skyrise=1;
		}
		else

			cube=1;


		//		DriveLoop(-1, 800, 0.11, 0.000000, 0.001);
		//  	DriveLoop( 1, 800, 0.11, 0.000000, 0.001);

			if(match==1 && skyrise==1 && red==1)//------------------RED SKYRISE-------
			{
				SensorValue[latch] = 1;
				armDown(0, 90);
				wait1Msec(300);
				armUp(130, 90);
				autoFeed(300, -127);
				autoFeed(1,0);
				wait1Msec(500);
				armDown(105, 90);
				autoGrab(1);
				armUp(300, 90);
				liftMotors(-18);
				DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(0, 90);
				autoGrab(0);			//drop one
				armUp(160, 90);
				liftMotors(-10);
				DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(105, 90);
				autoGrab(1);
				armUp(200, 90);
				DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(220, 90);
				autoGrab(0);			//drop two
				armUp(290, 90);
				liftMotors(-15);
				DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(105, 90);
				autoGrab(1);
				armUp(390, 90);
				DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(320, 90);
				autoGrab(0);			//drop three
				armUp(400, 90);
				Feed(-40);
				driveForward(127, -60, 160);
				Feed(-127);
				wait1Msec(1000);
				Feed(0);
				wait1Msec(5000000);
				}

	if(match==1 && skyrise==1 && blue==1)//---BLUE SKYRISE-----
			{
				SensorValue[latch] = 1;
				wait1Msec(300);
				armUp(160, 90);
				autoFeed(300, -127);
				autoFeed(1,0);
				wait1Msec(500);
				armDown(80, 90);
				autoGrab(1);
				armUp(185, 90);
				liftMotors(-20);
				DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(0, 90);
				autoGrab(0);			//drop one
				armUp(180, 90);
				liftMotors(-10);
				DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(80, 90);
				autoGrab(1);
				armUp(220, 90);
				DriveLoop(-1,-1, 925, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(180, 90);
				autoGrab(0);			//drop two
				armUp(250, 90);
				liftMotors(-15);
				DriveLoop(1,1, 925, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(80, 90);
				autoGrab(1);
				armUp(340, 90);
				DriveLoop(-1,-1, 920, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(320, 90);
				autoGrab(0);			//drop three
				armUp(400, 90);
				driveForward(-60, 120, 100);
				Feed(-127);
				driveForward(-60, 120, 170);
				wait1Msec(5000);
				Feed(0);
				wait1Msec(5000000);

			}
	if(programmingSkills==1) //--------------------------------SKILLS-------------
			{
			SensorValue[latch] = 1;
				armDown(0, 90);
				wait1Msec(300);
				armUp(130, 90);
				autoFeed(300, -127);
				autoFeed(1,0);
				wait1Msec(500);
				armDown(90, 90);
				autoGrab(1);
				armUp(300, 90);
				liftMotors(-15);
				DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(0, 90);
				autoGrab(0);			//drop one
				armUp(160, 90);
				liftMotors(-10);
				DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(200, 90);
				DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(220, 90);
				autoGrab(0);			//drop two
				armUp(290, 90);
				liftMotors(-10);
				DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(380, 90);
				DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
				armDown(320, 90);
				autoGrab(0);			//drop three
				armUp(370, 90);
				liftMotors(-15);
				DriveLoop(1,1, 905, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(560, 90);
				DriveLoop(-1,-1, 915, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(530, 90);
				autoGrab(0);			//drop four
				armUp(610, 90);
				liftMotors(-15);
				DriveLoop(1,1, 915, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(750, 90);
				DriveLoop(-1,-1, 925, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(700, 90);
				autoGrab(0);			//drop five
				armUp(750, 90);
				liftMotors(-15);
				DriveLoop(1,1, 950, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(950, 90);
				DriveLoop(-1,-1, 950, 0.11, 0.00000001, 0.1, 80, 30);
				armDown(900, 90);
				autoGrab(0);			//drop six
				armUp(950, 90);
				liftMotors(-15);
				DriveLoop(1,1, 950, 0.11, 0.00000001, 0.1, 80, 30);
				armDown(90, 90);
				autoGrab(1);
				armUp(1220, 90);
				DriveLoop(-1,-1, 900, 0.12, 0.00000001, 0.1, 80, 30);
				armDown(1110, 90);
				autoGrab(0);			//drop seven
				armUp(1220, 90);
				/*liftMotors(100);
				wait1Msec(500);
				liftMotors(15);*/
				driveForward(70, -70, 225);
				driveForward(70, 70, 100);
				autoFeed(750, -127);
				SensorValue[conveyer] = 1;
				driveForward(70, -70, 825);
				armDown(20, 127);
				liftMotors(-100);
				wait1Msec(500);
				liftMotors(-15);
				driveForward(70, 70, 100);
				autoFeed(1000, 127);
				driveForward(70, -70, 225);
				driveForward(70, 70, 100);
				autoFeed(1000, 127);
				driveForward(70, -70, 1000);
				armUp(1220, 127);
				driveForward(70, 70, 225);
				autoFeed(750, -127);
				wait1Msec(500);
				driveForward(70, 70, 50);
				autoFeed(750, -127);
				driveForward(-70, -70, 200);
				/*DriveLoop(1,-1, 250, 0.17, 0.00000001, 0.05, 120, 43);
				DriveLoop(1,1, 140, 0.14, 0.00000001, 0.05, 80, 35);
				autoFeed(1500, -127);
				DriveLoop(-1,-1, 140, 0.14, 0.00000001, 0.05, 80, 35);
				liftMotors(-50);
				DriveLoop(-1,1, 350, 0.17, 0.00000001, 0.05, 120, 43);
				liftMotors(-15);
				DriveLoop(1,1, 825, 0.12, 0.00000001, 0.05, 80, 35);
				DriveLoop(-1,1, 410, 0.12, 0.00000001, 0.05, 120, 43);
				liftMotors(-127);
				wait1Msec(1250);
				liftMotors(-15);
				DriveLoop(1,1, 175, 0.12, 0.00000001, 0.05, 80, 35);
				autoFeed(750, 127);
				wait1Msec(5000000);*/
			}
if(match==1 && cube==1 && red==1) //-----------RED CUBES Match
			{
				DriveLoop(-1,1, 400, 0.12, 0.00000001, 0.05, 120, 50);
		  	DriveLoop( 1,-1, 400, 0.12, 0.00000001, 0.05, 120, 50);
		  	wait1Msec(50000000000);


			/*SensorValue[latch] = 1;
				driveStop(300);
				armUp(1550, 90);
				liftMotors(-10);
				autoFeed(600, -127);
				autoFeed(1,0);
				wait1Msec(50);
				armDown(1200, 120);
				liftMotors(-127);
				wait1Msec(250);
				liftMotors(-15);
				driveForward(100, 100, 425);
				autoFeed(900, 127); //feed in red1
				stopRobot(10);
				liftMotors(60);
				driveForward(100, 100, 350);
				armUp(2300, 90);
				driveForward(90, -60, 250);
				autoFeed(800, -127); //feed out red1
				autoFeed(1,0);
				driveForward(-90, 60, 420);
				armDown(1200, 110);
				driveForward(100, 100, 100);
				wait1Msec(50);
				driveForward(70, -70, 125);
				liftMotors(-127);
				wait1Msec(450);
				liftMotors(-15);
				driveForward(100, 100, 650);
				autoFeed(700, 127); //pick up blue
				autoFeed(1, 0);
				driveForward(-70, 70, 15);
				autoFeed(375, 127); //move blue
				autoFeed(1, 0);
				driveForward(100, 100, 530);
				autoFeed(600, 127); //pick up red2
				driveForward(-70, 70, 10);
				autoFeed(1, 0);
				armUp(1800, 90);
				driveForward(90, -60, 250);
				wait1Msec(75);
				autoFeed(550, -127); //drop red2
				autoFeed(1,0);
				driveBack(-90, -90, -100);
				driveForward(-70, 70, 420);
				armDown(1390, 90);
				liftMotors(-10);
				driveForward(90, 90, 400);*/

		}
		if(match==1 && cube==1 && blue==1) //-----------BLUE CUBES-------------
			{
				SensorValue[latch] = 1;
				wait1Msec(300);
				armUp(1550, 90);
				liftMotors(-10);
				autoFeed(600, -127);
				autoFeed(1,0);
				wait1Msec(50);
				armDown(1425, 120);
				liftMotors(-127);
				wait1Msec(100);
				liftMotors(-15);
				SensorValue[conveyer] = 1;
				driveForward(100, 100, 425);
				autoFeed(925, 127); //feed in blue1
				stopRobot(10);
				liftMotors(40);
				driveForward(100, 100, 415);
				armUp(2300, 90);
				driveForward(-90, 60, 265);
				//	driveForward(90, 90, 20);
				autoFeed(800, -127); //feed out blue1
				autoFeed(1,0);
				//	driveBack(-90, -90, -20);
				driveForward(90, -60, 430);
				liftMotors(-40);
				driveForward(100, 100, 200);
				driveForward(-70, 70, 90);
				armDown(1425, 120);
				liftMotors(-127);
				wait1Msec(150);
				liftMotors(-15);
				driveForward(100, 100, 470);
				autoFeed(850, 127); //pick up red
				autoFeed(1, 0);
				//		driveForward(-70, 70, 15);
				autoFeed(250, 127); //move red
				autoFeed(1, 0);
				driveForward(100, 100, 590);
				autoFeed(650, 127); //pick up blue2
				//		driveForward(-70, 70, 10);
				autoFeed(1, 0);
				armUp(1800, 90);
				driveForward(-90, 60, 290);
				driveForward(90, 90, 40);
				autoFeed(550, -127); //drop blue2
				autoFeed(1,0);
				driveBack(-90, -90, -100);
				driveForward(-70, 70, 420);
				armDown(1390, 90);
				liftMotors(-10);
				driveForward(90, 90, 400);
				SensorValue[latch] = 1;
				wait1Msec(300);
				armUp(1550, 90);
				liftMotors(-10);
				autoFeed(600, -127);
				autoFeed(1,0);
				wait1Msec(50);
				armDown(1200, 120);
				liftMotors(-127);
				wait1Msec(250);
				liftMotors(-15);
				driveForward(100, 100, 425);
				autoFeed(900, 127); //feed in red1
				stopRobot(10);
				liftMotors(60);
				driveForward(100, 100, 350);
				armUp(2300, 90);
				driveForward(-90, 60, 250);
				autoFeed(800, -127); //feed out red1
				autoFeed(1,0);
				driveForward(90, -60, 420);
				armDown(1450, 110);
				driveForward(100, 100, 100);
				wait1Msec(50);
				driveForward(-70, 70, 85);
				armDown(1200, 110);
				liftMotors(-127);
				wait1Msec(450);
				liftMotors(-15);
				driveForward(100, 100, 650);
				autoFeed(700, 127); //pick up blue
				autoFeed(1, 0);
				//		driveForward(-70, 70, 15);
				autoFeed(375, 127); //move blue
				autoFeed(1, 0);
				driveForward(100, 100, 530);
				autoFeed(600, 127); //pick up red2
				//		driveForward(-70, 70, 10);
				autoFeed(1, 0);
				armUp(1800, 90);
				driveForward(-90, 60, 260);
				wait1Msec(75);
				autoFeed(550, -127); //drop red2
				autoFeed(1,0);
				driveBack(-90, -90, -100);
					driveForward(-70, 70, 420);
				armDown(1390, 90);
				liftMotors(-10);
				driveForward(90, 90, 400);
			}
}
예제 #16
0
void play() {
	HANDLE waitHandles[5] = { stopSignal, osStatusDongle.hEvent, calibratingSignal, newImageAnalyzed };
	int currentx = 320, currenty = 0, FPS2Count = 0;
	int timeOut = 30;
	float movingTime; //timeout after sending each command
	objectCollection goals, goals2;
	float nearestBallFloorX = 0, nearestBallFloorY = 0, initialBallDistance = 0;
	ignoreX = 0, ignoreY = 0;
	Timer ignoreTimer, ballSearchRotationSpeedTimer, chargingTimer, wanderingTimer, frameTimer;
	float lastBallFoundTime = 0, lastGoalFoundTime = 0, wanderingTime = 0; 
	bool rotateFast = true;
	FPS2Timer.start();
	FPS2Count = 0;
	frameTimer.start();

	bool turnFast = true;
	state = lookForBall;
	SetWindowText(goalGuessState, L"Goal on right");
	isBallInDribbler = false;
	prints(L"Started, driving to ball\n");
	SetWindowText(stateStatusGUI, L"Driving to ball");
	charged = false, ignoreBall = false;
	charge();
	chargingTimer.start();
	sendString(hCOMDongle, "9:bl\n");
	lastBallFoundTimer.start();
	lastGoalFoundTimer.start();
	movingTimer.start();

	while (true) {

		//display test stuff
		float floorX, floorY;
		int currentx, currenty;
		findNearestFloorBall(floorX, floorY, currentx, currenty);
		float angle = atanf(floorY / floorX) * 180.0 / PI;

		swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle);
		SetWindowText(infoGUI, buffer);
		//end display test stuff

		switch (WaitForMultipleObjects(4, waitHandles, FALSE, timeOut)) {
		//case WAIT_OBJECT_0: //start of a new command from the radio detected
		//						//ResetEvent(osStatus.hEvent);
		//	prints(L"radio event %X\n", dwCommEvent);
		//	//receiveCommand(); //receive and interpret the command
		//	//WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); //listen to new events, ie beginning character
		//	continue;
		case WAIT_OBJECT_0: //stop signal
			prints(L"Stop signal arrived.\n");
			SetWindowText(stateStatusGUI, L"stopped");
			currentDrivingState.angle = 0, currentDrivingState.speed = 0, currentDrivingState.vx = 0,
				currentDrivingState.vy = 0, currentDrivingState.angularVelocity = 0;
			setSpeedAngle(currentDrivingState);
			ResetEvent(startSignal);
			return;
		case WAIT_OBJECT_0 + 1: //info from the main board controller
								//prints(L"main board COM event %X\n", dwCommEvent);
			handleMainBoardCommunication();
			WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character
			continue;
		case WAIT_OBJECT_0 + 2: //calibrating signal
			SetWindowText(stateStatusGUI, L"calibrating");
			rotateAroundCenter(0);
			//WaitForSingleObject(calibratingEndSignal, INFINITE);
			continue;
		case WAIT_OBJECT_0 + 3: //new image analyzed
			//update FPS rate after every 5 images
			++FPS2Count;
			if (FPS2Count % 5 == 0) {
				float FPS = 5.0 / FPS2Timer.time();
				swprintf_s(buffer, L"FPS2: %.2f", FPS);
				SetWindowText(statusFPS2, buffer);
				FPS2Timer.start();
			}
			if (FPS2Count % 10 == 0) {
				receiveCommand();
			}
			break;
		default:
			continue;
		}

		if (attackBlue) { //set the side
			goals = goalsBlueShare;
			goals2 = goalsYellowShare;
		}
		else {
			goals = goalsYellowShare;
			goals2 = goalsBlueShare;
		}

		if (ignoreBall) { //ignore the ball just kicked for 1s
			if (ignoreTimer.time() > 1.0) {
				ignoreBall = false;
				ignoreX = 0, ignoreY = 0;
			}
			else {
				float ignoreXNew = ignoreX, ignoreYNew = ignoreY;
				findNearestFloorObjectToOldObject(ignoreXNew, ignoreYNew, ballsShare);
				//if we cant see the ball behind the dribbler, don't start ignoring a ball too far away
				ignoreX = ignoreXNew;
				ignoreY = ignoreYNew;

				swprintf_s(buffer, L"IgX %.2f IgY %.2f\n NX %.2f NY %.2f", ignoreX, ignoreY, ignoreXNew, ignoreYNew);
				SetWindowText(infoGUI2, buffer);
			}
		}
		else {
			SetWindowText(infoGUI2, L"Ignore OFF");
		}

		//keep track of the likely direction to turn to to find the goal
		if ((goalsBlueShare.count > 0 || goalsYellowShare.count > 0) && !(goalsBlueShare.count == 1 && goalsYellowShare.count == 1)) {
			int goalX, goalY;
			State stateGoalNew;
			if (goals.count > 0) {
				findLargestObject(goalX, goalY, goals);
				stateGoalNew = goalX > 320 ? goalOnRight : goalOnLeft;
			}
			else if (goals2.count > 0) {
				findLargestObject(goalX, goalY, goals2);
				stateGoalNew = goalX > 320 ? goalOnLeft : goalOnRight;
			}
			if (stateGoalNew != stateGoal && state != lookForGoal) {
				stateGoal = stateGoalNew;
				if (stateGoal == goalOnRight) {
					SetWindowText(goalGuessState, L"Goal on the right");
				}
				else {
					SetWindowText(goalGuessState, L"Goal on the left");
				}
			}
		}

		if (ballsOnFieldInSight() > 0) { //keep track of the likely direction to turn to to find the ball
			for (int i = 0; i < ballsShare.count; ++i) {
				if (!ballsShare.data[i].isObjectAcrossLine) {
					if (ballsShare.data[i].x > 320) {
						stateBallSeen = ballOnRight;
					}
					else {
						stateBallSeen = ballOnLeft;
					}
				}
			}
		}
		float frameTime = frameTimer.time();
		frameTimer.start();

		if (ballsOnFieldInSight() == 0 && state == lookForBall && !isBallInDribbler) {
			lastBallFoundTime += frameTime;
		}
		else if (ballsOnFieldInSight > 0) {
			lastBallFoundTime = 0;
		}
		if (goals.count == 0 && state == lookForGoal) {
			lastGoalFoundTime += frameTime;
		}
		else if (goals.count > 0) {
			lastGoalFoundTime = 0;
		}
		if (lastBallFoundTime > 3.0 || lastGoalFoundTime > 3.0) {
			prints(L"Wandering for ball\n");
			state = wanderForBall;
			movingTimer.start();
			wanderingTimer.start();
			wanderingTime = 0;
			lastBallFoundTime = 0;
			lastGoalFoundTime = 0;
		}

		if (fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD && state != rotate90 && state != wanderForBall) {
			state = rotate90;
			movingTimer.start();
		}

		if (state == lookForBall) {
			if (isBallInDribbler) {
				prints(L"Looking for goal\n");
				state = lookForGoal;
				movingTimer.start();
			}
			else {
				//prints(L"Looking for ball\n");
				SetWindowText(stateStatusGUI, L"Looking for ball");
				nearestBallFloorX = 0;
				int sign = stateBallSeen == ballOnRight ? -1 : 1;
				if (ballsOnFieldInSight() == 0) {
					if (fmodf(movingTimer.time(), 0.3) < 0.2) {
						rotateAroundCenter(sign * 200);
					}
					else {
						rotateAroundCenter(sign * 40);
					}
				}
				else {
					findNearestFloorBall(nearestBallFloorX, nearestBallFloorY, currentx, currenty);
					initialBallDistance = pow(nearestBallFloorX*nearestBallFloorX + nearestBallFloorY*nearestBallFloorY, 0.5);
					//prints(L"found nearest ball X: %.2f, Y: %.2f, currentx: %d, currenty: %d\n", nearestBallFloorX, nearestBallFloorY, currentx, currenty);
					prints(L"Driving to ball\n");
					state = driveToBall;
					rotateAroundCenter(0);
					movingTimer.start();
				}
			}
		}
		if (state == driveToBall) {
			if (isBallInDribbler) {
				rotateAroundCenter(0);
				Sleep(0);
				prints(L"Looking for goal\n");
				state = lookForGoal;
				movingTimer.start();
			}
			else {
				//prints(L"Driving to ball\n");
				SetWindowText(stateStatusGUI, L"driving to ball");
				float floorX, floorY;
				//findNearestObject(currentx, currenty, ballsShare);
				//prints(L"Floor coordinates x: %.2f, y: %.2f, currentx: %d, currenty: %d\n", floorX, floorY, currentx, currenty);
				if (findNearestFloorBall(floorX, floorY, currentx, currenty) == 0) {
					//prints(L"Drive state, but ballcount zero, starting to look for ball.\n");
					state = lookForBall;
					movingTimer.start();
					prints(L"Looking for ball\n");
					continue;
				}
				driveToFloorXYPID(floorX, floorY, initialBallDistance);
			}
		}
		if (state == lookForGoal) {
			//prints(L"Looking for goal\n");
			SetWindowText(stateStatusGUI, L"Looking for goal\n");
			if (isBallInDribbler) {
				int x = 0, y = 0;
				float floorX, floorY;
				findLargestObject(x, y, goals);
				convertToFloorCoordinates(x, y, floorX, floorY, GOALMIDHEIGHT);
				//prints(L"Goal x: %d\n", x);
				if (!(goalsBlueShare.count == 1 && goalsYellowShare.count == 1) &&
					(y > 0 && floorX > 0 && fabs(floorY) < 18.0 / 100.0 || abs(x-320) <= 35)) {
					state = kickBall;
					rotateAroundFront(0);
					prints(L"Kicking\n");
				}
				else {
					int sign;
					if (stateGoal == goalOnRight) {
						sign = -1;
					}
					else {
						sign = 1;
					}
					if (goals.count == 0) { //no goals in sight, turn faster
						rotateAroundFront(sign * 200);
					}
					else{
						//around 120 degs/s is max speed so that the goal doesn't go out, less than 30 degs/s is pointless
						float angle = atanf(tanf(angleOfView)*(x - 320.0) / 320.0); //roughly, in degrees
						float speedMultiplier = 1 - expf(-fabs(pow((x - 320.0) / 320.0, 4)));
						speedMultiplier = speedMultiplier > 1 ? 1 : speedMultiplier;
						float turningSpeed = 50 + (160 - 50)*speedMultiplier;

						rotateAroundFront(sign * turningSpeed);
					}
				}
			}
			else {
				state = lookForBall;
				movingTimer.start();
				prints(L"Looking for ball\n");
			}
		}
		if (state == kickBall) {
			SetWindowText(stateStatusGUI, L"Kicking");
			if (!isBallInDribbler) {
				state = lookForBall;
				movingTimer.start();
			}
			if (!charged) {//not charged, wait more
				if (chargingTimer.time() > 3.5) { //by this time we definitely should have gotten a charge command, charge again
					prints(L"No charge info received in time\n");
					chargingTimer.start();
					charge();
				}
				continue;
			}
			kick();
			Sleep(20);
			charged = false;

			//isBallInDribbler = false;
			//sendString(hCOMDongle, "9:bl\n");

			ignoreBall = true;
			ignoreX = 25.0 / 100.0, ignoreY = 0;
			ignoreTimer.start();

			state = lookForBall;
			movingTimer.start();
			prints(L"Looking for ball\n");

			//Sleep(80);
			charge();
			chargingTimer.start();
		}

		if (state == rotate90) {
			SetWindowText(stateStatusGUI, L"Rotating 90");
			if (movingTimer.time() < 0.5) {
				rotateAroundCenter(180);
				continue;
			}
			else {
				prints(L"Looking for ball\n");
				stateBallSeen = ballOnLeft;
				stateGoal = goalOnLeft;
				state = lookForBall;
				movingTimer.start();
			}
		}

		//wander around, try to drive towards a goal far enough away, if can't find one, drive to a direction where there are no goals, lines and there is enough green
		if (state == wanderForBall) {
			wanderingTime += frameTime;
			SetWindowText(stateStatusGUI, L"Wandering");
			if (wanderingTime > 4.0) { //if we didn't find a far away goal in 4 seconds
				int goalBlueX, goalBlueY;
				int goalYellowX, goalYellowY;
				findLargestObject(goalBlueX, goalBlueY, goalsBlueShare);
				findLargestObject(goalYellowX, goalYellowY, goalsYellowShare);
				if (goalsBlueShare.count != 0 && goalBlueX >= 150 && goalBlueX <= 500 
					|| goalsYellowShare.count != 0 && goalYellowX >= 150 && goalYellowX <= 500 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
					rotateAroundCenter(100);
					wanderingTimer.start();
				}
				else {
					if (wanderingTimer.time() > 1.0) {
						movingTimer.start();
						state = lookForBall;
						prints(L"Looking for ball\n");
						continue;
					}
					driveForward(0.5);
				}
			}
			else {
				if (goalsBlueShare.count == 0 && goalsYellowShare.count == 0 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
					rotateAroundCenter(120);
					wanderingTimer.start();
				}
				else {
					if (wanderingTimer.time() > 1.0) {
						movingTimer.start();
						state = lookForBall;
						prints(L"Looking for ball\n");
						continue;
					}
					int goalX = 0, goalY = 0;
					findLargestObject(goalX, goalY, goalsBlueShare);
					if (goalY > 450) {
						driveForward(0.5);
						continue;
					}
					else {
						goalX = 0, goalY = 0;
						findLargestObject(goalX, goalY, goalsYellowShare);
						if (goalY > 450) {
							driveForward(0.5);
						}
						else {
							rotateAroundCenter(100);
							wanderingTimer.start();
						}
					}
				}
			}
		}

	}
}
예제 #17
0
void rejectionBlue()
{

	// variables
	int armMin = 300;
	int wallHeight = 1000;
	int goalHeight = 1700;
	int pot = analogRead(8);
	int midLine = analogRead(2);

	int encoder1 = 1800; //drive under wall
	int encoder2 = 130; // rotate 90 degrees
	int encoder3 = 1400; // backwards to the opponets goal
	int encoder4 = 100; //turn to goal + enclose it
	int encoder5 = 450; //hit 1st blue ball off
	int encoder6 = 350; // drive back
	int encoder7 = 175; // turn to 2nd blue ball
	int encoder8 = 550; // hit 2nd ball off
	int encoder9 = 250; // drive back to position
	int encoder10 = 370; // rotate to line

	// begin routine

	intakeDead(); // unfold
	delay(300); // needs to clear the wall..
	driveBack(encoder1); // drive backwards to under the bridge
	stopIntake();
	turnLeft(encoder2); // turn ass to opponets goal
	driveBackDead(encoder3); // drive to opponets goal
	delay(500);

	midLine = 3000;
	while (midLine > 2000)
	{
		midLine = analogRead(2);
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
	motorSet(MOTOR_ARM_LEFT_BACK, 10);
	motorSet(MOTOR_ARM_LEFT_FRONT, 10);
	delay(85);

	armUp(wallHeight); // arm up
	turnRight(encoder4); // turn towards center blue ball
	driveForward(encoder5); // hit 1st ball off
	delay(700);
	driveBack(encoder6); // back
	delay(700);
	turnRight(encoder7); // turn towards 2nd blue ball
	delay(700);
	driveForward(encoder8); // hit off 2nd blue ball
	delay(700);
	intakeDead();
	driveBack(encoder9); // position
	delay(700);
	turnLeft(encoder10); // encase opponet goal
	delay(700);
	armUp(goalHeight);

	findLineRight();
	//followLine(300);

	driveForwardDead(); // drive to goal
	delay(700);
	stopDrive();
	outtake(3000);

	stopAll();
	delay(60000);

}
예제 #18
0
void classic15Red()
{
		int armMin = 300;
		int wallHeight = 1000;
		int goalHeight = 1550;
		int dead1 = 1000;
		int dead2 = 2000;
		int dead3 = 3000;
		int pot = analogRead (8);

	//encoder values
		int encoder1 = 1125; //drive to goal
		int encoder2 = 325; //keep going towards goal
		int encoder3 = 0; //drive slow, adjust to 90 degrees
		int encoder4 = 275; //back up
		int encoder5 = 800; //back up across the barrier again
		int encoder6 = 90; // turn towards center large ball
		int encoder7 = 220; // hit it off the barrier
		int encoder8 = 350; // drive back
		int encoder9 = 110; // turn towards other large ball
		int encoder10 = 681; // hit it off the barrier
		int encoder11 = 300; // drive back to square
		int encoder12 = 150; // ass to wall
		int encoder13 = 800;
		int encoder14 = 400;
		int encoder15 = 700;
		int encoder16 = 400;
		int encoder17 = 100;
		int encoder18 = 400;
		int encoder19 = 700;
		int encoder20 = 400;
		int encoder21 = 100;
		int encoder22 = 400;
		int encoder23 = 700;

	// begin routine

	intakeDead();
	delay(1000);
	stopIntake();
	driveForward(encoder1); // drive to goal
	armUp (goalHeight); //raise arm after cross barrier
	driveForwardSlow(encoder2);  //keep going towards goal
	driveForwardSlowDead (dead1); //drive slow, adjust to 90 degrees
	outtake (1500);
	driveBack(encoder4); // back up
	armDown(armMin); // lower arm
	driveBack(encoder5); //back up across the barrier again
	turnRight(encoder6); // turn towards center large ball
	armUp(wallHeight); // arm up to wall height
	driveForward(encoder7); // hit it off the barrier
	driveBack(encoder8); // drive back
	turnLeft(encoder9); // turn towards other large ball
	driveForward(encoder10); // hit it off the barrier
	driveBackSlowDead (dead2);  // drive back to square
	turnLeftArc (encoder12); // ass to wall
	driveBackSlowDead (dead2) ; // wall align 90degrees
	armDown (armMin); // arm down to floor
	intakeDead (); // start intaking
	driveForwardSlow (encoder13) ;// drive towards barf and intake whatever
	turnRightArc (encoder14); // rotate towards the barrier
	stopIntake();
	armUp(wallHeight); // arm up to wall height
	driveForwardSlow (encoder15); // drive to barrier
	stopDrive(); // stop at barrier
	outtake (1500); //drop the barf
	driveBackSlowDead(dead2); //hump da bump
	armDown (armMin); // arm down to floor
	intakeDead (); //start intaking
	turnLeftArc (encoder16); //face the barf
	driveForwardSlow (encoder17) ;// drive towards barf and intake whatever
	turnRightArc (encoder18) ; // rotate towards the barrier
	stopIntake();
	armUp(wallHeight); // arm up to wall height
	driveForwardSlow (encoder19); // drive to barrier
	stopDrive(); // stop at barrier
	outtake (1500); //drop the barf
	driveBackSlowDead(dead2); //drive to and align on bump
	armDown (armMin); // arm down to floor
	intakeDead (); //start intaking
	turnLeftArc (encoder20); //face the barf
	driveForwardSlow (encoder21) ;// drive towards barf and intake whatever
	turnRightArc (encoder22) ; // rotate towards the barrier
	stopIntake();
	armUp(wallHeight); // arm up to wall height
	driveForwardSlow (encoder23); // drive to barrier
	outtake (1500); //drop the barf



	//end of routine




	stopAll () ;
	delay(60000);///////////////////////////////////////////////////////////////////////////////////


}
예제 #19
0
//  drive the robot
// -------------------------------------------------------------------[ drive ]
void WRTbotDrive::drive( const uint16_t driveThisWay, wrt_motor_t &motor )
{
	// --- motor ON by default
	// -----------------------
	motor.left.control  |= wrt_motorControl_power_ON;
	motor.right.control |= wrt_motorControl_power_ON;

	// --- no drive by light
	// --------------------------
	this->driveByLightFactor = 0;

	switch( driveThisWay )
	{	default:
		// ------------------------------------------------------------[ stop ]
		case wrt_stop:
		{
			motor.left.control  &= wrt_motorControl_power_OFF;
			motor.right.control &= wrt_motorControl_power_OFF;
			// ---
			motor.left.speed   = 0;
			motor.right.speed  = 0;
			// ---
			drive( motor );

		}	break;
		// ---------------------------------------------------------[ forward ]
		case wrt_forward:
		{
			this->useNominal = true;
			// ---
			driveForward( motor );

		}	break;
		// ---------------------------------------------------------[ forward ]
		case wrt_forward_slow:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_min;
			motor.left.speed  = wrt_code_motor_speed_min;
			// ---
			driveForward( motor );

		}	break;
		// ----------------------------------------------------[ fast forward ]
		case wrt_forward_fast:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_max;
			motor.left.speed  = wrt_code_motor_speed_max;
			// ---
			driveForward( motor );

		}	break;
		// ---------------------------------------------------------[ reverse ]
		case wrt_reverse:
		{
			this->useNominal = true;
			// ---
			driveReverse( motor );

		}	break;
		// ----------------------------------------------------[ fast reverse ]
		case wrt_reverse_fast:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_max;
			motor.left.speed  = wrt_code_motor_speed_max;
			// ---
			driveReverse( motor );

		}	break;
		// -------------------------------------------------------[ left turn ]
		case wrt_drive_left:
		{
			driveLeft( motor );

		}	break;
		// --------------------------------------------------[ fast left turn ]
		case wrt_turn_left_fast:
		{
			turnLeftFast( motor );

		}	break;
		// --------------------------------------------------[ slow left turn ]
		case wrt_turn_left:
		{
			this->useNominal = true;

			turnLeft( motor );

		}	break;
		// ------------------------------------------------------[ right turn ]
		case wrt_drive_right:
		{
			driveRight( motor );

		}	break;
		// -------------------------------------------------[ fast right turn ]
		case wrt_turn_right_fast:
		{
			turnRightFast( motor );

		}	break;
		// -------------------------------------------------[ slow right turn ]
		case wrt_turn_right:
		{
			this->useNominal = true;

			turnRight( motor );

		}	break;
		// -----------------------------------------[ differential right turn ]
		case wrt_turnRightDiff:
		case wrt_turnRightDiff_fast:
		{
			turnRightDiferential( driveThisWay, motor );

		}	break;
		// -----------------------------------------[ differential left turn ]
		case wrt_turnLeftDiff:
		case wrt_turnLeftDiff_fast:
		{
			turnLeftDiferential( driveThisWay, motor );

		}	break;
	}


}	// done drive()
예제 #20
0
void kakitRed()
{

// variables
	int armMin = 300;
	int wallHeight = 1000;
	int goalHeight = 1650;

	int pot = analogRead(8);

	int encoder00 = 250; // back up abit to row
	int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
	int encoder1 = 162; // rotate towards 2 buckys
	int encoder2 = 150; // drive towards buckys
	int encoder3 = 400; // back up to bump
	int encoder4 = 200; // back up towards bridge
	int encoder5 = 360; // rotate 180 degrees to the large ball
	int encoder6 = 900; // go under bridge and knock out large ball
	int encoder7 = 90; // turn to goal
	int encoder8 = 200; // drive to goal
	int encoder9 = 75;

	// begin routine
	intake(300);
	armDownTrim();

	driveForwardDead(); //ram big balls
	delay(1500);
	stopDrive();
	delay(500);

	driveBack(encoder00); // back up to row abit
	intakeDead();

	turnLeft(encoder0); // turn 360 degrees

	driveBackDead(); // drive back + wall align
	delay(1300);
	stopDrive();
	delay(500);

	turnRight(encoder1); // turn to two buckys
	intakeDead();

	followLine(300); // make sure ur straight

	driveForwardSlowDead(); // drive towards buckys
	delay(500);
	stopDrive();
	delay(600);

	driveForwardDead(); //get the 2nd ball
	delay(200);
	stopDrive();
	delay(600);

	stopIntake();

	driveBack(encoder3); // back up to bump

	armUpDead(); // armup
	delay(50);
	stopArm();
	stopIntake();

	driveBackSlowDead(); // allign the bump
	delay(300);
	stopDrive();
	delay(500);

	driveBackDead();  // over the bump
	delay(1000);
	stopDrive();
	delay(500);

	driveForwardSlowDead(); // alighn to bump
	delay(800);
	stopDrive();
	delay(500);

	driveBackSlow(encoder4); // back up towards bridge
	turnLeft(encoder5); // rotate 180 degrees to the large ball
	armDown(armMin);
	armDownTrim();

	driveForward(encoder6); // go under the bridge + knock large ball
	armUp(goalHeight);

	turnRight(encoder7); // turn to goal

	findLineRight();
	followLine(300);

	driveForwardSlowDead(); // drive to goal
	delay(700);
	stopDrive();
	outtake(7000); // outtake 3

	stopAll();

}
예제 #21
0
	void Autonomous()
	{
		DriverStationLCD *screen = DriverStationLCD::GetInstance(); 
		while ((IsAutonomous()))
		{
			HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
			imgpointer = camera.GetImage();	//tells camera to capture image
			backpack.Set(Relay::kForward); //turns ringlight on
			BinaryImage* binIMG = NULL;	// declares a container to hold a binary image
			binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255);	// thresholds HSL image and places in the binary image container
			delete imgpointer;	// deletes the HSL image to free up memory on the cRIO
			Image* Kirby = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create 8 bit image
			Image* KirbyTwo = imaqCreateImage(IMAQ_IMAGE_U8, 0); // creates the second 8-bit image that we can use separately for counting particles. 
																 // (The first image gets eaten by the measureparticle function)
			float pLower = 175; // min height of rectangle for comparison
			float pUpper = 500;	// max height of rectangle for comparison
			int criteriaCount = 1; // number of elements to include/exclude at a time
			int rejectMatches = 1;	// when set to true, particles that do not meet the criteria are discarded
			int connectivity = 1;	// declares connectivity value as 1; so corners are not ignored
			int Polturgust3000;	// removes small blobs
			int borderSetting;	// variable to store border settings, limit for rectangle
			int cloningDevice; // we create another image because the ParticleMeasuring steals the image from particlecounter
			int borderSize = 1;  // border for the camera frame (if you don't put this, DriverStation gets mad at you)
			ParticleFilterCriteria2 particleCriteria;	
			ParticleFilterOptions2 particleFilterOptions;
			int numParticles;
			particleCriteria.parameter = IMAQ_MT_AREA; //The Morphological measurement we use
			particleCriteria.lower = pLower; // The lower bound of the criteria range
			particleCriteria.upper = pUpper; // The upper bound of the criteria range
			particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
			particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
			particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
			particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
			particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
			
			Polturgust3000 = imaqParticleFilter4(Kirby, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
			borderSetting = imaqSetBorderSize(Kirby, borderSize); // Sets a border size
			cloningDevice =  imaqDuplicate(KirbyTwo, Kirby); //Officially creating a duplicate of the first image to count the number of particles.
			delete binIMG; //Deletes the Binary image
			int ParticleCounter;	// stores number of particles
			int* countparticles; // stores the number of particles for the measure particle function

			ParticleCounter = imaqCountParticles(Kirby, TRUE, countparticles); // Counts the number of particles to be sent off later to the MeasureParticle function. Then it gets eaten by the measureparticle function
			int TinyRuler; // TRULY ARBITRARY name of the first measuring particle function (specifically for particle #1)
			int BabyYardstick; // TRULY ARBITRARY Name of the second measuring particle function (specifically for particle #2)
			double* unowidth; // TRULY ARBITRARY name of the first particle it find
			double* doswidth; // TRULY ARBITRARY name of the second particle it finds

			TinyRuler = imaqMeasureParticle(Kirby, 0, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, unowidth); // Function of measuring rectangle width is applied to particle 1 (unowidth)
			BabyYardstick = imaqMeasureParticle(Kirby, 1, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, doswidth); // Function of measuring width is applied to particle 2 (doswidth)

			
			screen->PrintfLine(DriverStationLCD::kUser_Line3,"W1: %f",*unowidth); // Prints the applied information to particle 1. (Rectangle width)
			screen->PrintfLine(DriverStationLCD::kUser_Line4,"W2: %f",*doswidth);
			imaqDispose(Kirby);
			imaqDispose(KirbyTwo);
			if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position");
				if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
					// Even this needs to be tested
				{	
					screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnRight();
					driveForward();
					Wait(3); 
					stopDriving();
					shootCatapult();
				}
				else // The target isn't hot. So it starts going toward this not hot goal.
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Not Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					driveForward();
					Wait(3);
					stopDriving();
					shootCatapult();
				}
			}
			//both on
			else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
				if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
					// These DEFINITELY need to be tested. All of them. Forreal.
					turnLeftMore();
					driveForward();
					Wait(3); 
					stopDriving();
					shootCatapult();
				}
				else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
					if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						turnRightMore();
						driveForward();
						Wait(3); 
						stopDriving();
						shootCatapult();
					}
					else // The target isn't hot. So it starts going toward this not hot goal.
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Not Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						driveForward();
						Wait(3);
						stopDriving();
						shootCatapult();
						driveForward();
						Wait(3);
						stopDriving();
					}
				}
				//Left button on && right off
				else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
				{
					screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position");
					if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						turnLeft();
						driveForward();
						Wait(3); 
						stopDriving();
						shootCatapult();
					}
					else // The target isn't hot. So it starts going toward this not hot goal.
					{
						screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Not Hot");
						// These DEFINITELY need to be tested. All of them. Forreal.
						driveForward();
						Wait(3);
						stopDriving();
						shootCatapult();
					}
				}
			Wait(0.005);
			screen -> UpdateLCD();
			}
		}
	}