void RobotDemo::MecanumDrive(float x, float y, float rotation, float gyroAngle)
{
	const float EPSILONFL=1.f, EPSILONFR=1.f, EPSILONBL=1.f, EPSILONBR=1.f;
		
	double xIn = x;
	double yIn = y;
	// Negate y for the joystick.
	yIn = -yIn;
	// Compenstate for gyro angle.
	RotateVector(xIn, yIn, gyroAngle);

	double wheelSpeeds[kMaxNumberOfMotors];
	wheelSpeeds[0] = xIn + yIn + rotation;
	wheelSpeeds[1] = -xIn + yIn - rotation;
	wheelSpeeds[2] = -xIn + yIn + rotation;
	wheelSpeeds[3] = xIn + yIn - rotation;

	Normalize(wheelSpeeds);

	uint8_t syncGroup = 0x80;

	victorFL->Set(EPSILONFL*wheelSpeeds[0] * 1, syncGroup);
	victorFR->Set(EPSILONFR*wheelSpeeds[1] * -1, syncGroup);
	victorBL->Set(EPSILONBL*wheelSpeeds[2] * 1, syncGroup);
	victorBR->Set(EPSILONBR*wheelSpeeds[3] * -1, syncGroup);

	CANJaguar::UpdateSyncGroup(syncGroup);
}
void RobotDemo::dumb_climber_code()
{
	//printf("Top:%i   Bottom:%i  " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
	if (drive_stick_prim ->GetRawButton(climber_hold_up))
	{
		if (top_claw_limit_switch->Get() == 1)
		{
			climbing_motor->Set(0.0);
			//printf("STOPPED\n");
		}
		else
		{
			climbing_motor->Set(1);
			//printf("GOING UP\n");
		}
	} // not climber hold up

	else if (drive_stick_prim->GetRawButton(climber_hold_down))
	{
		if (bottom_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(0.0);
			//printf("STOPED\n");
		}
		else
		{
			climbing_motor->Set(-1);
			//printf("GOING DOWN\n");
		}
	} // hold down button
	else
	{ // no js buttons pushed
		climbing_motor->Set(0.0);
	}
}
Exemple #3
0
	void TeleopPeriodic() {
		if(m_driver->GetRawButton(BUTTON_LB)) {
			// PYRAMID
			m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawButton(BUTTON_RB)) {
			// FEEDER
			m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
			m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
			m_PIDController->Enable();
		} else {
			// MANUAL CONTROL
			m_PIDController->Disable();
			
			m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
			m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
		}
		
		// ----- PRINT -----
		SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
		SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
		
	} // TeleopPeriodic()
Exemple #4
0
	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		while (IsOperatorControl())
		{
			left.Set(cont.GetRawAxis(2));
			right.Set(cont.GetRawAxis(4));
		}
	}
Exemple #5
0
 void AutonomousPeriodic(void)
 {
 	// Basic motor test code 
 	leftFrontDrive->Set(1);
 	leftRearDrive->Set(1);
 	
 	rightRearDrive->Set(1);
 	rightFrontDrive->Set(1);
 }
void RobotDemo::dumb_drive_code()
{
#if DUMB_DRIVE_CODE
	left_drive_motor_A->Set(-drive_stick_sec ->GetY());
	left_drive_motor_B->Set(-drive_stick_sec->GetY());
	right_drive_motor_A->Set(drive_stick_prim->GetY());
	right_drive_motor_B->Set(drive_stick_prim->GetY());
#endif
}
Exemple #7
0
	/**
	 * This method is called every 20ms to update the robots components during autonomous.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void AutonomousPeriodic()
	{
		//In the first two seconds of auto, drive forwardat half power
		if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
		{
			rd->SetLeftRightMotorOutputs(.5, .5);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
		{
			//2 to 4 seconds, stop drivetrain and spin shooter wheels
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(1);
			shoot2->Set(1);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
		{
			//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
			shoot1->Set(1);
			shoot2->Set(1);
			kicker->Set(-1);
		}
		else
		{
			//After all that, stop everything
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(0);
			shoot2->Set(0);
			kicker->Set(0);
		}
	}
 void Reset() {
   m_talonCounter->Reset();
   m_victorCounter->Reset();
   m_jaguarCounter->Reset();
   m_talon->Set(0.0f);
   m_victor->Set(0.0f);
   m_jaguar->Set(0.0f);
 }
Exemple #9
0
	/**
	 * This method is called every 20ms to update the robots components during teleop.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void TeleopPeriodic()
	{
		//Make the robot move like an arcade stick controlled device
		rd->ArcadeDrive(j1);

		//If button 1 on the joystick is pressed, spin the shooter wheels up,
		//otherwise stop them
		if(j1->GetRawButton(1))
		{
			shoot1->Set(1);
			shoot2->Set(2);
		}
		else
		{
			shoot1->Set(0);
			shoot2->Set(0);
		}

		//If button 2 on the joystick is pressed, spin the kicker, otherwise stop the kicker
		if(j1->GetRawButton(2))
		{
			kicker->Set(-1);
		}
		else
		{
			kicker->Set(0);
		}
	}
Exemple #10
0
	void ShootModeSet()
	{
		if (controller.GetRawButton(BTN_MODE_RELOAD) == true && ShootMode == eShoot)
		{
			ShootMode = eReload;
			spinspeed = 0;
			shootertime.Stop();
			shootertime.Reset();
			shottime.Stop();
			shottime.Reset();
		}
		else if (controller.GetRawButton(BTN_MODE_SHOOT) == true && ShootMode == eReload)
		{
			ShootMode = eShoot;
			spinspeed = 1; // CHANGE BACK TO 1.  40% IS BUTTERZONE FOR CHILDREN
			shootertime.Start();
			shottime.Start();
		}
		spin1.Set(-spinspeed);
	}
Exemple #11
0
	void TeleopPeriodic()
	{
		char myString[64];
		float   y;


// get the joystick position and filter the deadband
		y = -joystick->GetY();

		if (y > -0.1 && y < 0.1)
			y = 0;

		sprintf(myString, "joystick setting: %f\n", y);
		SmartDashboard::PutString("DB/String 1", myString);
		motor->Set(y, 0); // set the speed of the motor
		sprintf(myString, "gear count: %d\n", gearToothCounter->Get());
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "gear stopped: %d\n", gearToothCounter->GetStopped());
		SmartDashboard::PutString("DB/String 3", myString);

//		sprintf(myString, "In val: %d\n", toothInput->GetValue());
//		SmartDashboard::PutString("DB/String 2", myString);
	}
	void Disabled(void)
	{
		bool stopped = false;
		while(!IsOperatorControl() && !IsAutonomous())
		{
			if(!stopped)
			{
				frontLeftMotor->Set(0.0);
				rearLeftMotor->Set(0.0);
				frontRightMotor->Set(0.0);
				rearRightMotor->Set(0.0);
				liftMotor1->Set(0.0);
				liftMotor2->Set(0.0);
				gripMotor1->Set(0.0);
				//gripMotor2->Set(0.0);
				PIDLift->Reset();
				PIDDriveLeft->Reset();
				PIDDriveRight->Reset();
				PIDGrip->Reset();
				stopped = true;
			}
		}
	}
	void OperatorControl(void)
	{
		// Teleoperated Code.
		/*double WaitDash = 0.0;
		double FireDash = 0.0;
		double intPause = 0.0;
		
		SmartDashboard::PutNumber("P", 3.0);
		SmartDashboard::PutNumber("W", 0.0);
		SmartDashboard::PutNumber("A", 0.0);
		
		WaitDash = SmartDashboard::GetNumber("P");
		FireDash = SmartDashboard::GetNumber("W");
		intPause = SmartDashboard::GetNumber("A");
		
		SmartDashboard::PutNumber("P", WaitDash);
		SmartDashboard::PutNumber("W", FireDash);
		SmartDashboard::PutNumber("A", WaitDash);
		*/	
		// Enable and start the compressor.
		//compressor->Enabled();
		compressor->Start();

		// Enable drive motor safety timeout.
		myRobot.SetSafetyEnabled(true);

		// Enable watchdog and initial feed.
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();

		// Set robot in low gear by default. Not active.
		//s[0]->Set(false);
		GetWatchdog().Feed();

		//bool blnShoot = false;
		bool blnLowHang = false;
		bool blnShift = false;

		GetWatchdog().Feed();

		bool blnShooterSpd = false;
		bool blnReverse = false;

		float fltShoot;
		float fltSpeed = 1;

		int intFail = 0;
		
		timerLowHang.Reset();
		timerShift.Reset();
		timerFire.Reset();
		timerShooter.Reset();
		timerDriveCtrl.Reset();
		timerCamera.Reset();
		timerReverse.Reset();

		timerLowHang.Start();
		timerShift.Start();
		timerFire.Start();
		timerShooter.Start();
		timerDriveCtrl.Start();
		timerCamera.Start();
		timerReverse.Start();
		
		GetWatchdog().Feed();

		//sd->sendIOPortData();

		// Local variables.
		//float fltStick1X, fltStick1Y;

		while (IsOperatorControl())
		{
			if(timerReverse.Get() > 0.5)
			{
				if(stick1->GetRawButton(8) && blnReverse == false )
				{
					blnReverse = true;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
				else if(stick1->GetRawButton(9) && blnReverse == true)
				{
					blnReverse = false;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
			}
			if(blnReverse == false)
			{
				fltStick1Y = stick1->GetY();
				fltStick1X = stick1->GetX();
				SmartDashboard::PutBoolean("Reverse",false);
				GetWatchdog().Feed();
			}
			else if(blnReverse == true)
			{
				fltStick1Y = ((stick1->GetY())*(-1));
				fltStick1X = ((stick1->GetX())*(1));
				SmartDashboard::PutBoolean("Reverse",true);
				GetWatchdog().Feed();
			}
			myRobot.ArcadeDrive(fltStick1Y,fltStick1X);
			//myRobot.ArcadeDrive(stick1);
			GetWatchdog().Feed(); // Feed hungary demonic Watchdog.


			SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get());
			SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100));
			SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100));

			GetWatchdog().Feed();
			//End Stick1 arcade drive code.

			GetWatchdog().Feed();

			fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2);

			GetWatchdog().Feed();

			SmartDashboard::PutNumber("Shooter Power (%)", fltShoot);
			SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100));



			//float fltPressureSwitch = m_pressureSwitch;
			//float fltRelay = m_relay;
			//SmartDashboard::PutNumber("Demo",3);

			GetWatchdog().Feed();
			if(timerShift.Get() > 0.2)
			{
				if(stick1->GetRawButton(7) || stick1->GetTrigger())
				{
					if(blnShift == false)
					{
						GetWatchdog().Feed();
						s[0]->Set(false);
						s[1]->Set(true);
						SmartDashboard::PutString("Gear","Low");
						blnShift = true;
						timerShift.Stop();
						timerShift.Reset();
						timerShift.Start();
						GetWatchdog().Feed();
					}	
					else if (blnShift == true)
					{
						GetWatchdog().Feed();
						s[0]->Set(true);
						s[1]->Set(false);
						SmartDashboard::PutString("Gear","High");
						blnShift = false;
						timerShift.Stop();
						timerShift.Reset();
						timerShift.Start();
						GetWatchdog().Feed();
					}
				}		
			}
			if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5)
			{
				blnLowTime = true;
				blnLowHang = true;
				timerLowHang.Stop();
				timerLowHang.Reset();
				timerLowHang.Start();
				GetWatchdog().Feed();
			}
			else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5)
			{
				blnLowTime = false;
				blnLowHang = false;
				timerLowHang.Stop();
				timerLowHang.Reset();
				timerLowHang.Start();
				GetWatchdog().Feed();
			}
			if(blnLowTime == true)
			{
				s[3]->Set(true);
				GetWatchdog().Feed();
			}
			else if (blnLowTime == false)
			{
				s[3]->Set(false);
				GetWatchdog().Feed();
			}

			if(stick1->GetRawButton(3))
			{

				mtdCameraCode();
				GetWatchdog().Feed();

			}

			if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8)
			{
				s[2]->Set(true);
				SmartDashboard::PutString("Shooter Piston","In");
				intFail = 1;
				GetWatchdog().Feed();
				timerFire.Stop();
				timerFire.Reset();
				timerFire.Start();
				GetWatchdog().Feed();
			}
			else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8)
			{
				s[2]->Set(false);
				intFail = 0;
				SmartDashboard::PutString("Shooter Piston","Out");
				GetWatchdog().Feed();
				timerFire.Stop();
				timerFire.Reset();
				timerFire.Start();
				GetWatchdog().Feed();
			}

			if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5)
			{
				GetWatchdog().Feed();
				myShooter1.Set(-fltSpeed);
				myShooter2.Set(-fltSpeed);
				SmartDashboard::PutString("Shooter","On");
				SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100));
				blnShooterSpd = true;
				GetWatchdog().Feed();
				timerShooter.Stop();
				timerShooter.Reset();
				timerShooter.Start();
				GetWatchdog().Feed();
			}
			else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5)
			{
				GetWatchdog().Feed();
				myShooter1.Set(0);
				myShooter2.Set(0);
				SmartDashboard::PutString("Shooter","Off");
				SmartDashboard::PutNumber("Shooter Speed (%)",0);
				blnShooterSpd = false;
				GetWatchdog().Feed();
				//Wait(0.2);
				timerShooter.Stop();
				timerShooter.Reset();
				timerShooter.Start();
				GetWatchdog().Feed();

			}

			if(stick2->GetRawButton(10))
			{
				fltSpeed = 0.6;
				GetWatchdog().Feed();
			}
			if(stick2->GetRawButton(9))
			{
				fltSpeed = 0.7;
				GetWatchdog().Feed();
			}
			if(stick2->GetRawButton(8))
			{
				fltSpeed = 0.8;
				GetWatchdog().Feed();
			}
			if(stick2->GetRawButton(7))
			{
				fltSpeed = 0.9;
				GetWatchdog().Feed();
			}
			if(stick2->GetRawButton(6))
			{
				fltSpeed = 1;
				GetWatchdog().Feed();
			}
			if(stick2->GetRawButton(11))
			{
				fltSpeed = fltShoot;
				GetWatchdog().Feed();
			}
			GetWatchdog().Feed();
		}
	}
void RobotDemo::climber_code()
{
	if (drive_stick_prim ->GetRawButton(climber_off_button))
	{
		climbing_motor-> Set(0.0);
		claw_go_down = false;
		claw_ = false;

	}
	else
	{
		if (drive_stick_prim ->GetRawButton(climber_hold_up))
		{
			claw_go_down = false;
			claw_ = false;
			if (top_claw_limit_switch->Get())
			{
				climbing_motor->Set(0.0);
			}
			else
			{
				climbing_motor->Set(1);
			}

		}// hold up
		else
		{
			if (drive_stick_prim ->GetRawButton(climber_hold_down))
			{
				claw_go_down = false;
				claw_ = false;
				if (bottom_claw_limit_switch->Get() == false)
				{
					climbing_motor ->Set(0.0);
				}
				else
				{
					climbing_motor ->Set(-1);
				}
			} // hold down
			else if (drive_stick_sec ->GetRawButton(climber_send_top))
			{
				claw_ = true;
				claw_go_down = false;
			}
			else if (drive_stick_sec ->GetRawButton(climber_send_bottom))
			{
				claw_go_down = true;
				claw_ = false;
			}
			else
			{ // no holding or sending
				if ((claw_ == false) && (claw_go_down == false))
				{
					climbing_motor ->Set(0.0);
				}
			}

		} // no climber hold up button

	} // no climber off button

	if (claw_go_down)
	{
		if (bottom_claw_limit_switch->Get() == 1)
		{
			climbing_motor ->Set(-1);
		}
		else
		{
			climbing_motor ->Set(0.0);
		}

	}
	if (claw_)
	{
		if (top_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(1);
		}
		else
		{
			climbing_motor ->Set(0.0);
		}
	}
}
Exemple #15
0
	void OperatorControl(void)
	{
		float counter;
		timer.Start();
		float percent;
		deadband = .05;
		float pi = 3.141592653589793238462643;
		float bpotval, fpotval;
		float min = 600, max;
		float fchgval = .5;
		float bchgval = .5;
		
		while (IsOperatorControl())
		{
			// comp.checkCompressor();
			ShootModeSet();
			Shoot(true);
			fpotval = fpot.GetValue();
			bpotval = bpot.GetValue();
				counter = timer.Get();
				if (controller.GetRawButton(3))
				{
					frot.SetSpeed(0);
					brot.SetSpeed(0);
					flmot.SetSpeed(0);
					frmot.SetSpeed(0);
					blmot.SetSpeed(0);
					brmot.SetSpeed(0);
				}
				else if (controller.GetRawButton(BTN_L1) == false)
				{
					if (controller.GetRawButton(7))
					{
						if (fpotval < 860)
							frot.SetSpeed(-0.5);
						if (bpotval <  725)
							brot.SetSpeed(-0.5);
						if (fpotval > 860 and bpotval > 725)
						{
							frot.Set(0);
							brot.Set(0);
							flmot.Set(0.5);
							frmot.Set(0.5);
							blmot.Set(0.5);
							brmot.Set(0.5);
						}
					}
					else if (controller.GetRawButton(8))
					{
						if (fpotval < 860)
							frot.SetSpeed(-0.5);
						if (bpotval <  725)
							brot.SetSpeed(-0.5);
						if (fpotval > 860 and bpotval > 725)
						{
							frot.Set(0);
							brot.Set(0);
							flmot.Set(-0.5);
							frmot.Set(-0.5);
							blmot.Set(-0.5);
							brmot.Set(-0.5);
						}
					}
					else
					{
						if (controller.GetRawAxis(4) <= 0)
							percent = ((acos(controller.GetRawAxis(3)) / pi));
						else if (controller.GetRawAxis(4) > 0)
							percent = ((acos(-controller.GetRawAxis(3)) / pi));
						fpotval = percent * 550 + 330;
						percent = (fpotval - 330) / 550;
						bpotval = percent * 500 + 245;
						
						if (fpot.GetValue() < fpotval)
							fchgval = -.5;
						
						else if (fpot.GetValue() > fpotval)
							fchgval = .5;
						
						if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
							fchgval = 0;
			
						if (bpot.GetValue() > bpotval)
							bchgval = -.5;
									
						else if (bpot.GetValue() < bpotval)
							bchgval = .5;
									
						if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
							bchgval = 0;
						
						frot.Set(fchgval);
						brot.Set(bchgval);
						
						if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
						{
							if (controller.GetRawAxis(4) > 0)
							{
								flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
							}
							
							else if (controller.GetRawAxis(4) < 0)
							{
								flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
							}
						}
						else
						{
							flmot.Set(0);
							frmot.Set(0);
							blmot.Set(0);
							brmot.Set(0);
						}
						contaxis4 = 0;
					}
				}
				else
				{
					if (contaxis4 == 0)
						contaxis4 = controller.GetRawAxis(4);
					if (controller.GetRawButton(BTN_R1))
					{
						if (contaxis4 > 0)
						{
							flmot.Set(-0.25);
							frmot.Set(0.25);
							blmot.Set(-0.25);
							brmot.Set(0.25);
						}
						else if (contaxis4 < 0)
						{
							flmot.Set(0.25);
							frmot.Set(-0.25);
							blmot.Set(0.25);
							brmot.Set(-0.25);
						}
					}	
				}
				if (float (fpot.GetValue()) < min)
					min = float (fpot.GetValue());
				
				else if (float (fpot.GetValue()) > max)
					max = float (fpot.GetValue());
				
				if (counter >= .1)
				{
					lcda->Clear();
					lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
					lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
					lcda->UpdateLCD();
					timer.Reset();
				
				}
		}
	}
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();
		s[0]->Set(true);
		s[1]->Set(true);
		SmartDashboard::PutString("Gear","High");
		compressor->Start();
		myRobot.SetSafetyEnabled(true);


		int WaitDash = 0;
		int FireDash = 0;
		int intPause = 0;
		GetWatchdog().Feed();

		// Incase we need to recreate the controls for Autonomous.
		//SmartDashboard::PutNumber("fire_wait", 3);
		//SmartDashboard::PutNumber("fire_amount", 3);
		//SmartDashboard::PutNumber("fire_pause", 5);

		WaitDash = static_cast<int>(SmartDashboard::GetNumber("fire_wait"));
		FireDash = static_cast<int>(SmartDashboard::GetNumber("fire_amount"));
		intPause = static_cast<int>(SmartDashboard::GetNumber("fire_pause"));
		GetWatchdog().Feed();


		/* CAMERA THRESHOLD AND DECLARATIONS, UNCOMMENT IF CAMERA READDED TO AUTONOMOUS
		//Their threshold values suck DDDD, from the NIvision assistant will be below.
		//Threshold threshold(60, 100, 90, 255, 20, 255);	//HSV threshold criteria, ranges are in that order ie. Hue is 60-100
		//Threshold threshold(100, 255, 230, 255, 140, 255); //New threshold values
		Threshold threshold(0, 255, 0, 255, 221, 255);
		
		ParticleFilterCriteria2 criteria[] = {
				{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
		};												//Particle filter criteria, used to filter out small particles
		// AxisCamera &camera = AxisCamera::GetInstance();	//To use the Axis camera uncomment this line
				
		*/
		while (IsAutonomous() && IsEnabled())
		{

			myShooter1.Set(-1);
			myShooter2.Set(-1);
			GetWatchdog().Feed();


			GetWatchdog().Feed();

			// Wait before firing.
			for(intAutoCtrl = 0; intAutoCtrl < (1);intAutoCtrl++)
			{
				GetWatchdog().Feed();
				for(intDelay = 0; intDelay< (((intPause)*2)+1);intDelay++)
				{
					GetWatchdog().Feed();
					Wait(0.5);
				}
				GetWatchdog().Feed();


				// Fire a number of frisbees as set in the dashboard.
				//for(intCount = 0; intCount < (FireDash+1); intCount++)
			//	{
					GetWatchdog().Feed();
					s[2]->Set(true);

					// Delay between firing each frisbees.
					for(int wait = 0; wait<((WaitDash)*2);wait++)
					{
						GetWatchdog().Feed();
						Wait(0.5);
					}
					s[2]->Set(false);
					GetWatchdog().Feed();
					for(intSecondWait = 0;intSecondWait < 3;intSecondWait++)
					{
						GetWatchdog().Feed();
						Wait(0.5);
					}
					GetWatchdog().Feed();
				}
				GetWatchdog().Feed();
			}

			myShooter1.Set(0);
			myShooter2.Set(0);
			GetWatchdog().Feed();
		//}

	}
Exemple #17
0
 void TeleopPeriodic(void)
 {
         //Drive code
         leftFrontDrive->Set(-1 * leftStick->GetY());
         rightFrontDrive->Set(rightStick->GetY());
         leftRearDrive->Set(-1 * leftStick->GetY());
         rightRearDrive->Set(rightStick->GetY());
         
         //airCompressor->Start();
         // Simple Button Drive Forward
         if(rightStick->GetRawButton(3))
         {
         	leftFrontDrive->Set(-1);
         	rightFrontDrive->Set(1);
         	leftRearDrive->Set(-1);
         	rightRearDrive->Set(1);
         }
         
         // Simple Button Drive Backwards
         if(rightStick->GetRawButton(2))
         {
         	leftFrontDrive->Set(1);
         	rightFrontDrive->Set(-1);
         	leftRearDrive->Set(1);
         	rightRearDrive->Set(-1);
         }
         
         // Code to print to the user messages box in the Driver Station
         LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
         LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
         LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
         LCD->UpdateLCD();
         Wait(0.2); 
 }
	void TeleopPeriodic(void) {
		float x = gamepad->GetLeftX();
		float y = gamepad->GetLeftY();
		float rot = gamepad->GetRightX();
		
		//small gamepad values are ignored
		if (x < 0.1f && x > -0.1f)
			{
				x = 0;
			}
		if (y < 0.1f && y > -0.1f)
			{
				y = 0;
			}
		
		if (rot < 0.1f && rot > -0.1f)
			{
				rot = 0;
			}
		
		drive->MecanumDrive_Cartesian(SPEED_LIMIT * x, SPEED_LIMIT * y, SPEED_LIMIT * rot);
		
		//shoot smoke if button is pressed
		if (gamepad2->GetNumberedButton(FIRE_SMOKE_BUTTON)){
			//SHOOT SMOKE!
			//makingSmoke = !makingSmoke;
			smoke_cannon->Set(SMOKE_CANNON_SPEED);
			lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Shooting");
				
			firing_smoke_timer->Start(); //measure how long we've fired smoke, so we know if it's ok to make more
			
		}
		else
		{
			smoke_cannon->Set(0.0f);
			lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Not shooting");
			firing_smoke_timer->Stop(); //stop the timer, since we're not firing smoke.
										//don't reset, cuz we need to how much smoke we've fired.

		}
		//Eye Code
		
//		float eye_pos = gamepad2->GetLeftX();
//		
//		right_eye_x->Set((eye_pos * 60) + default_eye_position);
//		left_eye_x->Set((eye_pos * 60) + default_eye_position - LEFT_EYE_OFFSET);
//		
//		//button lock code
//		if(gamepad2->GetNumberedButtonPressed(EYE_LOCK_BUTTON)){
//			default_eye_position = eye_pos;
//		}
//		
		
		
		//left eye control
		//If A isn't pressed the value should stay the same as before
		if (!gamepad2->GetNumberedButton(1)){
			float left_joystick_x = gamepad2->GetLeftX();
			float left_eye_x_axis = (1 - left_joystick_x)*60;
			left_eye_val = left_eye_x_axis + 50;
			
			float right_joystick_x = gamepad2->GetRawAxis(4);//right x axis
			float right_eye_x_axis = (1-right_joystick_x)*60;
			right_eye_val = right_eye_x_axis+20;
		}
		left_eye_x->SetAngle(left_eye_val);		
		right_eye_x->SetAngle(right_eye_val);
		
		//float right_joystick_y = gamepad2->GetRawAxis(4);
		//float right_eye_y_axis = (right_joystick_y+1)*60;
		//right_eye_y->SetAngle(right_eye_y_axis);

		/*
		bool rbutton = gamepad2->GetNumberedButton(HEAD_UP_BUTTON);
		bool lbutton = gamepad2->Ge tNumberedButton(HEAD_DOWN_BUTTON);
		if (rbutton){
			lcd->PrintfLine(DriverStationLCD::kUser_Line6, "rb pressed");
			jaw_motor->Set(0.2f);
		}else if(lbutton){
			lcd->PrintfLine(DriverStationLCD::kUser_Line6, "lb pressed");
			jaw_motor->Set(-0.15f);
		}else{
			lcd->PrintfLine(DriverStationLCD::kUser_Line6, "no buttons");
			jaw_motor->Set(0.0f);
		}
		*/

		//REAL head & jaw code
		//move head down
		if(gamepad2->GetRightX()<=-0.5f && can_move_head_down() && can_move_jaw_down()){
			head_motor->Set(-0.3f);
			jaw_motor->Set(0.3f);
		}
		//move head up
		else if(gamepad2->GetNumberedButton(HEAD_UP_BUTTON) && can_move_head_up()){
			head_motor->Set(0.3f);
			jaw_motor->Set(-0.3f);
		}
		//move jaw down
		else if(gamepad2->GetRightX()>=0.5f && can_move_jaw_down()){
			jaw_motor->Set(0.3f);
		}
		//move jaw up
		else if(gamepad2->GetNumberedButton(JAW_UP_BUTTON) && can_move_jaw_up()){
			jaw_motor->Set(-0.3f);
		}
		//sets to zero if no buttons pressed
		else {
			jaw_motor->Set(0.0f);
			head_motor->Set(0.0f);
		}
		
		
		lcd->PrintfLine(DriverStationLCD::kUser_Line6, "b:%d t:%d c:%d", bottomjaw_limit->Get(), tophead_limit->Get(), crash_limit->Get());
		
		

		//Smoke code
		if (gamepad2->GetNumberedButton(MAKE_SMOKE_BUTTON)){
			//MAKE SMOKE!!!!!!!!!!!
			//only if we don't have too much excess smoke
			if (making_smoke_timer->Get() - firing_smoke_timer->Get() < MAX_EXCESS_SMOKE_TIME){
				lcd->PrintfLine(DriverStationLCD::kUser_Line4, "smoke");
				smoke_machine->Set(true);
			} else {
				lcd->PrintfLine(DriverStationLCD::kUser_Line4, "too much smoke");
				smoke_machine->Set(false);
			}
			making_smoke_timer->Start(); //measure how long we've been making smoke, so we don't overflow the machine
										//doesn't do anything if we've already started the timer
		}
		else
		{
			lcd->PrintfLine(DriverStationLCD::kUser_Line4, "not smoke");
			smoke_machine->Set(false);
			making_smoke_timer->Stop(); 	//stop the timer, since we're not making smoke
											//don't reset it, cuz we need to know how much smoke we've made
		}
		
		//if both timers are the same, we can set them both to zero to ensure we don't overflow them or something
		if (making_smoke_timer->Get() == firing_smoke_timer->Get()){
			making_smoke_timer->Reset();
			firing_smoke_timer->Reset();
		}
		
		lcd->PrintfLine(DriverStationLCD::kUser_Line1, "x:%f", x);
		lcd->PrintfLine(DriverStationLCD::kUser_Line2, "y:%f", y);
		lcd->PrintfLine(DriverStationLCD::kUser_Line3, "r:%f", rot);
		
		
		lcd->UpdateLCD();

		
		
	}
/******************************************************************************
 * Function:     UpdateActuatorCmnds
 *
 * Description:  Update the commands sent out to the RoboRIO.
 ******************************************************************************/
void UpdateActuatorCmnds(float ballRoller,
                         float desiredPos,
                         bool LgtB3,
                         bool LgtB5,
                         bool LgtB6,
                         bool LgtB7,
                         bool LgtB8,
                         bool LgtB9,
                         bool LgtB11,
                         float DrvLeft,
                         float DrvRight,
                         float fishTape,
                         float lowArm,
                         float wench)
  {
  float ArmError;
  float ballRotate;
  float ArmP = ballarm.GetDistance();

  /* Set the output for the angle of the ball arm: */
  if (desiredPos > 0)
    {
    /* If the desired position is above zero, the operator is commanding
     * the arm to a given position */
      ArmError = (desiredPos - ArmP) * (-0.01);
      ballRotate = ArmError;
    }
  else if ((LgtB11 == true) && (ArmP < 211))
    {
    ballRotate = -1;
    }
  else if (LgtB5 && ArmP < 211)
    {
    ballRotate = -0.4;
    }
  else if (LgtB3 && ArmP > -10)
    {
    ballRotate = 0.4;
    }
  else if (LgtB6 && ArmP > 20)
    {
    ballRotate = 0.8;
    }
  else
    {
    /* If the desired position is zero, the operator is not wanting the arm
     * to be controlled.  Set the output to the motor to zero which will
     * disable the motor and allow the arm to naturally fall. */
      ArmError = 0;
      ballRotate = ArmError;
    }



  /* Limit the allowed arm command if it could go past the allowed vertical position: */
  if ((ArmP > 230 && LgtB5 == true) ||
      (ArmP < -10 && LgtB3 == true))
    {
    ballRotate = 0;
    }

  /* This is limiting for when the arm goes past the vertical position: */
  if (ArmP > 211)
    {
    ballRotate = 0.5;
    }

  BallRoller.Set(ballRoller);
  BallRotate.Set(ballRotate);
  myDrive.TankDrive(DrvLeft, DrvRight);
  FishTape.Set(fishTape);
  LowArm.Set(lowArm);
  Wench.Set(wench);
  }
Exemple #20
0
	void OperatorControl()
	{
		timer.Start();
		while (IsOperatorControl())
		{
			x = Stick.GetRawAxis(1);
			y = -Stick.GetRawAxis(2);
			if (fabs(x) < 0.1)
			{
				x = 0;
			}
			if (fabs(y) < 0.1)
			{
				y = 0;
			}
			x2 = x*x;
			y2 = y*y;
			magnitude = pow((x2+y2),0.5);
			angle = atan2(y,x)*180/PI;
			magnitude = max(magnitude, -1);
			magnitude = min(magnitude, 1);
			if (angle < 0)
			{
				angle += 180;
				magnitude *= -1;
			}
			/*front = 730 - (angle * 2.7888);
			if ((front + 5) < float(fpot.GetValue()))
			{
				fturn.Set(0.5);
			}
			else if ((front - 5) > float(fpot.GetValue()))
			{
				fturn.Set(-0.5);
			}
			else
			{
				fturn.Set(0);
			}*/
			back = 235 + (angle * 2.8611);
			if ((back + 10) < float(bpot.GetValue()))
						{
							bturn.Set(0.5);
						}
						else if ((back - 10) > float(bpot.GetValue()))
						{
							bturn.Set(-0.5);
						}
						else
						{
							bturn.Set(0);
						}
					if (timer.Get() > 0.1)
					{
						lcd->Clear();
						lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front);
						lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back);
						lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue()));
						lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue()));
						lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle);
						lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude);
						lcd->UpdateLCD();
						timer.Reset();
						timer.Start();
					}
			
		}
	}
/**
 * Set the PWM value.
 *
 * The PWM value is set using a range of -1.0 to 1.0, appropriately
 * scaling the value for the FPGA.
 *
 * @param channel The PWM channel used for this Victor
 * @param speed The speed value between -1.0 and 1.0 to set.
 */
void SetVictorSpeed(UINT32 channel, float speed)
{
	Victor *victor = (Victor *) AllocatePWM(channel, CreateVictorStatic);
	if (victor) victor->Set(speed);
}
Exemple #22
0
	void AutonomousPeriodic()
	{

		drive->SetMaxOutput(1.0);
		printf("Distance: %f\n", rightEnc->GetDistance());

//		if (!launcherDown) {
//			if (timer->Get() > 1.0) {
//				winch->Set(0.5);
//				otherWinch->Set(0.5);
//			} else if (timer->Get() < 3.0) {
//				winch->Set(0.5);
//				otherWinch->Set(0.0);
//			} else {
//				winch->Set(0.0);
//				otherWinch->Set(0.0);
//				launcherDown = true;
//			}
//		}

		if(done) {
			winch->Set(0.0);
			otherWinch->Set(0.0);
			drive->ArcadeDrive(0.0,0.0);

			if (autoCounter > 10) {
				launchPiston->Set(0);
				launch1->Set(0.0);
				launch2->Set(0.0);
			} else {
				autoCounter++;
				if (shoot == "Yes") {
					launch1->Set(1.0);
					launch2->Set(1.0);
				}
			}
		} else {
			if (autoSelected == "Approach Only") {
				done = Autonomous::approachOnly();

				launch1->Set(0.0);
				launch2->Set(0.0);
				winch->Set(0.0);
				otherWinch->Set(0.0);

			} else if (!defenseCrossed) {
					if(Autonomous::crossFunctions.find(autoSelected) != Autonomous::crossFunctions.end()) {
						bool (*crossFunction)() = Autonomous::crossFunctions.at(autoSelected);
						defenseCrossed = crossFunction();
						launch1->Set(0.0);
						launch2->Set(0.0);
						winch->Set(0.0);
						otherWinch->Set(0.0);
					} else {
						done = true;
						launch1->Set(0.0);
						launch2->Set(0.0);
						winch->Set(0.0);
						otherWinch->Set(0.0);
						drive->ArcadeDrive(0.0,0.0);
					}
					timer->Reset();
			} else {
				if (autoSelected == "Spy Bot") {
					rotation = 90;
				}
				//after we cross...
				float difference = gyro->GetAngle() - rotation;

				if (difference > 10) {
					launch1->Set(0.0);
					launch2->Set(0.0);
					winch->Set(0.0);
					otherWinch->Set(0.0);

					drive->ArcadeDrive(0.0,difference * 0.3);
					timer->Reset();
				} else {
					if (goal == "Yes") {
						if (!Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer)) {
							launchPiston->Set(0);
						} else {
							launch1->Set(0.0);
							launch2->Set(0.0);
							winch->Set(0.0);
							otherWinch->Set(0.0);
							drive->ArcadeDrive(0.0,0.0);

							launchPiston->Set(1);
							done = true;
						}
					} else {
						launch1->Set(0.0);
						launch2->Set(0.0);
						winch->Set(0.0);
						otherWinch->Set(0.0);

						done = true;
					}
				}
			}
		}
	}
Exemple #23
0
	void TeleopPeriodic()
	{
		//camera->GetImage(frame);
		//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
		//CameraServer::GetInstance()->SetImage(frame);


		printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());

		drive->ArcadeDrive(driveStick);
		drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
		//printf("%f\n", (1-stick->GetThrottle())/2);
		//leftMotor->Set(0.1);
		//rightMotor->Set(0.1);

		if (shootStick->GetRawAxis(3) > 0.5) {
			launch1->Set(1.0);
			launch2->Set(1.0);
		} else if (shootStick->GetRawAxis(2) > 0.5) {
			printf("Power Counter: %i\n", powerCounter);
			if (powerCounter < POWER_MAX) {
				powerCounter++;
				launch1->Set(-0.8);
				launch2->Set(-0.8);
			} else {
				launch1->Set(-0.6);
				launch2->Set(-0.6);
			}
		} else {
			launch1->Set(0.0);
			launch2->Set(0.0);
			powerCounter = 0.0;
		}

		//use this button to spin only one winch, to lift up.
		 if (shootStick->GetRawButton(7)) {
		 	otherWinch->Set(0.5);
		 } else if (shootStick->GetRawButton(8)) {
			 otherWinch->Set(-0.5);
		 } else {
		 	otherWinch->Set(0.0);
		 }

		if (shootStick->GetRawButton(5)) {
			winch->Set(-0.7);

			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(-0.5);
			}
		} else if (shootStick->GetRawButton(6)) {
			winch->Set(0.7);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(0.5);
			}
		} else {
			winch->Set(0.0);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//,				otherWinch->Set(0.0);
			}
		}
		

		if (shootStick->GetRawButton(1)) {
			launchPiston->Set(1);
		} else {
			launchPiston->Set(0);
		}

		if (shootStick->GetRawButton(3)) {
			Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
		}

		if (shootStick->GetRawButton(3) && debounce == false) {
			debounce = true;
			if (defenseUp) {
				defensePiston->Set(DoubleSolenoid::Value::kReverse);
				defenseUp = false;
			} else {
				defenseUp =true;
				defensePiston->Set(DoubleSolenoid::Value::kForward);
			}
		} else if (!shootStick->GetRawButton(3)){
			debounce = false;
		}
	}
	void OperatorControl(void)
	{
		autonomous = false;
		//myRobot.SetSafetyEnabled(false);
		//myRobot.SetInvertedMotor(kFrontLeftMotor, true);
		//	myRobot.SetInvertedMotor(3, true);
		//variables for great pid
		double rightSpeed,leftSpeed;
		float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP;
		float stickY[2];
		float stickYAbs[2];
		bool lightOn = false;
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCameraParams::kResolution_160x120);
		camera.WriteMaxFPS(5);
		camera.WriteBrightness(50);
		camera.WriteRotation(AxisCameraParams::kRotation_0);
		rightEncoder->Start();
		leftEncoder->Start();
		liftEncoder->Start();
		rightEncoder->Reset();
		leftEncoder->Reset();
		liftEncoder->Reset();
		bool fastest = false; //transmission mode
		float reduction = 1; //gear reduction from 
		bool bDrivePID = false;
		//float maxSpeed = 500;
		float liftPower = 0;
		float liftPos = -10;
		bool disengageBrake = false;
		int count = 0;
		//int popCount = 0;
		double popStart = 0;
		double popTime = 0;
		int popStage = 0;
		int liftCount = 0;
		int liftCount2 = 0;
		const float LOG17 = log(17.61093344);
		float liftPowerAbs = 0;
		float gripError = 0;
		float gripErrorAbs = 0;
		float gripPropMod = 0;
		float gripIntMod = 0;
		bool shiftHigh = false;
		leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution,
		rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio
		DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO();
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);

		PIDDriveLeft->SetOutputRange(-1, 1);
		PIDDriveRight->SetOutputRange(-1, 1);
		//PIDDriveLeft->SetInputRange(-244,244);
		//PIDDriveRight->SetInputRange(-244,244);
		PIDDriveLeft->SetTolerance(5);
		PIDDriveRight->SetTolerance(5);
		PIDDriveLeft->SetContinuous(false);
		PIDDriveRight->SetContinuous(false);
		//PIDDriveLeft->Enable();
		//PIDDriveRight->Enable();
		PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		
		liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
		PIDLift->Enable();
		
		gripSP = 0;
		float goalGripSP = 0;
		bool useGoalSP = true;
		bool gripPIDOn = true;
		float gripP[10];
		float gripI[10];
		float gripD[10];
		PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up
		PIDGrip->SetTolerance(5);
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		miniDep->Set(miniDep->kForward);
		int i = 0;
		while(i < 10)
		{
			gripP[i] = GRIPPROPGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripI[i] = GRIPINTGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripD[i] = GRIPDERIVGAIN;
			i++;
		}

		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			count++;
#if !(SKELETON)
			sendVisionData();
#endif
			/*
			if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n");
			if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n");
			if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n");
			if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n");
			if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n");
			if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n");
			*/
			/*
			if(lsLeft->Get()) printf("LSLEFT\n");
			if(lsMiddle->Get()) printf("LSMIDDLE\n");
			if(lsRight->Get()) printf("LSRIGHT\n");
			*/
			stickY[0] = stickL.GetY();
			stickY[1] = stickR.GetY();
			stickYAbs[0] = fabs(stickY[0]);
			stickYAbs[1] = fabs(stickY[1]);
			if(bDrivePID)
			{
	#if 0
				frontLeftMotor->Set(stickY[0]);
				rearLeftMotor->Set(stickY[0]);
				frontRightMotor->Set(stickY[1]);
				rearRightMotor->Set(stickY[1]);
				
				if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(),
						rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get());
	#endif		
				if(stickYAbs[0] <= 0.05 )
				{
					leftSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveLeft->Reset();
						PIDDriveLeft->Enable();
					}
				}
				else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control
				if(stickYAbs[1] <= 0.05)
				{
					rightSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveRight->Reset();
						PIDDriveRight->Enable();
					}
				}
				else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
				
				if (BACKWARDBUTTON)
				{
					tempRightSP = rightSP;
					tempLeftSP = leftSP;
					rightSP = -tempLeftSP;
					leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
				}
				
				PIDDriveLeft->SetSetpoint(leftSP);
				PIDDriveRight->SetSetpoint(rightSP);
					
				
				leftSpeed = leftEncoder->GetRate();
				rightSpeed = rightEncoder->GetRate();
				if(!(count++ % 5))
				{
				printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", 
						leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
						PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
						frontRightMotor->Get());
						
				
				//printf("Throttle value: %f", stickR.GetThrottle());
				if(PIDDriveRight->OnTarget()) printf("Right on \n");
				if(PIDDriveLeft->OnTarget()) printf("Left on \n");
				}
					
				if(PIDRESETBUTTON)
				{
					//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); 
					//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
					PIDDriveLeft->Reset();
					PIDDriveRight->Reset();
					PIDDriveLeft->Enable();
					PIDDriveRight->Enable();
				}
			}
			else
			{
				if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
				if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
				if(DEMOSWITCH)
				{
					stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
					stickY[1] = stickY[0]*(1 - lift->getPosition());
				}
				if(stickYAbs[0] > 0.05)
				{
					frontLeftMotor->Set(stickY[0]);
					rearLeftMotor->Set(stickY[0]);
				}
				else
				{
					frontLeftMotor->Set(0);
					rearLeftMotor->Set(0);
				}
				if(stickYAbs[1] > 0.05)
				{
					frontRightMotor->Set(-stickY[1]);
					rearRightMotor->Set(-stickY[1]);
				}
				else
				{
					frontRightMotor->Set(0);
					rearRightMotor->Set(0);
				}
			}
			
			if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) &&
					stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID;
			
			if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH)
			{
				shifter->Set(shifter->kReverse);
				shiftHigh = false;
				maxSpeed = 12;
			}
			else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH)
			{
				shifter->Set(shifter->kForward);
				shiftHigh = true;
				maxSpeed = 25; //last value 35
			}

			sendIOPortData();
#if !(SKELETON)
			/*
			if(LIGHTBUTTON) lightOn = true;
			else lightOn = false;
			if(!lightOn) light->Set(light->kOff);
			if(lightOn) light->Set(light->kOn);
			*/
			if(!MODESWITCH)
			{
				lastLiftSP = liftSP;
				if(!PIDLift->IsEnabled()) PIDLift->Enable();
				if(LIFTLOW1BUTTON) liftSP = LIFTLOW1;
				if(LIFTLOW2BUTTON) liftSP = LIFTLOW2;
				if(LIFTMID1BUTTON) liftSP = LIFTMID1;
				if(LIFTMID2BUTTON) liftSP = LIFTMID2;
				if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1;
				if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2;
				
				if(!lift->isBraking() && !disengageBrake)
				{
					PIDLift->SetSetpoint(liftSP);
					if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
					{
						//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
						PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
					}
					else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
				}
				if(lift->isBraking() && lastLiftSP != liftSP)
				{
					PIDLift->SetSetpoint(lastLiftSP + 0.06);
					PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0);
					lift->brakeOff();
					disengageBrake = true;
					if(!liftCount) liftCount = count;
				}
				//set brake (because near)
				if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake)
				{
					if(liftCount == 0) liftCount = count;
					if(count - liftCount > 3)
					{
						PIDLift->Reset();
						liftMotor1->Set(LIFTNEUTRALPOWER);
						liftMotor2->Set(LIFTNEUTRALPOWER);
						Wait(0.02);
						lift->brakeOn();
						Wait(0.02);
						liftMotor1->Set(0.0);
						liftMotor2->Set(0.0);
						PIDLift->Enable();
						PIDLift->SetSetpoint(lift->getPosition());
						liftCount = 0;
					}
					//if(!(count%50)) printf("Braking/Not PID \n");
				}
				if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition());
				if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3)
				{
					disengageBrake = false;
					if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015);
					else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015);
					liftCount = 0;
				}
				
				//pid
				/*
				else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20)
				{
					PIDLift->Enable();
					liftPos = -10;
					printf("PID GO PID GO PID GO PID GO PID GO \n");
				}
				*/
				//when liftPos is positive, measures position
				//when liftPos = -10, is pidding
				//when liftPos = -20, has just released brake
			}
			else //(MODESWITCH)
			{
				if(PIDLift->IsEnabled()) PIDLift->Reset();
				liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)));
				liftPowerAbs = fabs(liftPower);
				if(liftPowerAbs > 1) liftPower /= liftPowerAbs;
				//if(!(count%5)) printf("Slider: %f", liftPower);
				
				if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff();
				else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn();
				if (liftPowerAbs > 0.05)
				{
					liftMotor1->Set(liftPower);
					liftMotor2->Set(liftPower);
				}
				else
				{
					liftMotor1->Set(0);
					liftMotor2->Set(0);
				}
			}
			if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset();
			/*
			if(!(count%5))
			{
				printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(),
						PIDLift->GetError(), PIDLift->GetSetpoint());
			}
			*/
			if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP)
			{	
				gripPIDOn = !gripPIDOn;
				printf("Switch'd\n");
			}
			if(gripPIDOn)	
			{
				if(!PIDGrip->IsEnabled()) PIDGrip->Enable();
				if(GRIPPERPOSUP && !GRIPPERPOSDOWN)
				{
					goalGripSP = 0;
				}
				else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5)
				{
					goalGripSP = 1;
				}
				/*
				else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP)
				{
					goalGripSP = 0.5;
				}
				*/
				
				gripError = PIDGrip->GetError();
				gripErrorAbs = fabs(gripError);
				PIDGrip->SetSetpoint(goalGripSP);
				
				if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up
				else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up
				if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2)
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet()));
				}
				else
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02);
				}
				if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0)
				{
					gripLatch1->Set(true);
					gripLatch2->Set(false);
				}
				else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || 
						gripPIDSource->PIDGet() < 0) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
					
				if(gripLatch1->Get() && !(count%20)) 
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				/*
				if(stickL.GetRawButton(1) && !(count%5)){
					gripIntMod -= 0.001;
				}
				
				if(stickR.GetRawButton(1) &&!(count%5))
				{
					gripIntMod += 0.001;
				}
				if(stickL.GetRawButton(2) && !(count%5))
				{
					gripPropMod -= 0.1;
				}
				if(stickL.GetRawButton(3) && !(count%5))
				{
					gripPropMod += 0.1;
				}
				*/
				/*
				if(LIFTBOTTOMBUTTON)
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				*/
				
				if(!(count%5))
				{
					printf("Gripper pos: %f Gripper error: %f Grip power: %f \n",
							gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get());
				}
				
			
			}

			//Calibration routine
			else
			{
				if(gripLatch1->Get() == true) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
				if(PIDGrip->IsEnabled()) PIDGrip->Reset();
				if(GRIPPERPOSUP)
				{
					gripMotor1->Set(-0.5);
					//gripMotor2->Set(0.5);
				}
				else if(GRIPPERPOSDOWN)
				{
					gripMotor1->Set(0.5);
					//gripMotor2->Set(-0.5);
					
				}
				else
				{
					gripMotor1->Set(0);
					//gripMotor2->Set(0);
				}
			}
			//if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage());
			//if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get());
			if(GRIPPEROPEN && !popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
			}
			else if(!popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#else
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#endif
			}
			if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1;
			if(popStage) popTime = GetClock();
			if(popStage == 1)
			{
				//popCount = count;
				popStart = GetClock();
				popStage++;
				//printf("POP START POP START POP START \n");
			}
			if(popStage == 2)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
				popStage++;
				//printf("GRIP OPEN GRIP OPEN GRIP OPEN \n");
			}
			if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15
			{
				gripPop1->Set(true);
				gripPop2->Set(false);
				popStage++;
				//printf("POP OUT POP OUT POP OUT \n");
			}
			if(popStage == 4 && popTime - popStart > .75) //time was 0.9
			{
				gripPop1->Set(false);
				gripPop2->Set(true);
				popStage++;
				//printf("POP IN POP IN POP IN \n");
			}
			if(popStage == 5 && popTime - popStart > 0.9)	popStage = 0; //time was 1.05
			
			if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse);
			else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn);
#endif			
			if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kOff);
			/*
			if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kForward);
			*/
			Wait(0.02);				// wait for a motor update time
		}
	}
void RobotDemo::stop(){
	victorFL->Set(0.0);
	victorFR->Set(0.0);
	victorBL->Set(0.0);
	victorBR->Set(0.0);	
}
Exemple #26
0
		void OperatorControl (void) {
			GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
			
			DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
			
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
			
			SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
			SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
			SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
			
			while (IsOperatorControl() && IsEnabled()) {
				GetWatchdog().Feed(); // Feed the Watchdog.
				
				kinectMode = (bool) kinectModeSelector->GetSelected();
				demoMode = (bool) demoModeSelector->GetSelected();
				speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
				
				if (kinectMode) {
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
					
					if (!demoMode) {
						if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else {
							motorDriveLeft.Set(0);
							motorDriveRight.Set(0);
						}
					}
			
					if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER);
						motorFeed.Set(FEED_MOTOR_POWER);
						motorPickup.Set(PICKUP_MOTOR_POWER);
					} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
						motorFeed.Set(FEED_MOTOR_POWER * -1);
						motorPickup.Set(PICKUP_MOTOR_POWER * -1);
					} else {
						motorShooter.Set(0);
						motorFeed.Set(0);
						motorPickup.Set(0);
					}
				
					if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
						motorTurret.Set(TURRET_POWER);
					} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
						motorTurret.Set(TURRET_POWER * -1);
					} else {
						motorTurret.Set(0);
					}
				} else {
					if (joystickDriveLeft.GetThrottle() == 0) {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
						
						motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
							motorTurret.Set(TURRET_POWER);
						} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
							motorTurret.Set(TURRET_POWER * -1);
						} else {
							motorTurret.Set(0);
						}
					} else {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
						
						motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
					}
				}
				
				dsLCD->UpdateLCD();
			}

			GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
		}
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(false);
		AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11");
		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down.
		camera.WriteCompression(80);//Compresses the image(?)
		camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100
		DriverStationLCD *screen = DriverStationLCD::GetInstance();
		int count = 0;
		while(IsOperatorControl())
		{
			screen->UpdateLCD();
			count++;
			HSLImage* imgpointer;		//declares a new hue saturation lum image
			imgpointer = camera.GetImage();  //grabs an image to initialize that image
			//imaqCreateImage(IMAQ_IMAGE_U8,) 
			//imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1);
			BinaryImage* binImg = NULL;	//declares a new binary image
			
			
			//ThresholdHSL changes our regular image into a binary image.
			/*hueLow Low value for hue  
			hueHigh High value for hue  
			saturationLow Low value for saturation  
			saturationHigh High value for saturation  
			luminenceLow Low value for luminence  
			luminenceHigh High value for luminence   
			 */
			binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255);
			
			//Saves the image to a temp directory
			binImg->Write("/tmp/thresh.jpg");
			
			//Writes the binary image to disk
			
			imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL);
			
			delete imgpointer;
			Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0);
			int returnvalue;
			returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE);
			
	//		imaqWriteFile(Convex, "/tmp/convex.jpg", NULL); 
		
			delete binImg;
	//		screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue);
			screen->UpdateLCD();
			float lookuptable[256];
			lookuptable[0] = 0;
			lookuptable[1] = 65535;
			
			//Converts image to 16 bit, then back to 8 bit.
			Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0);
			imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0);
			imaqDispose(Convex);
			Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0);
			imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0);
			imaqDispose(cast);
	//		screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user
			imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL);
			
			//Image* SuperSize = im
			//imaqCreateImage(IMAQ_IMAGE_U8, 2240);
			//int returnvalue2;
			//returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL); 
			//screen->UpdateLCD();
			//imaqDispose (Convex);
//			ROI *roi;
//			Rect rectangle;
//			rectangle.top = 0;
//			rectangle.left = 0;
//			rectangle.width = 320;
//			rectangle.height = 120;
//			imaqAddRectContour(roi,rectangle);
			
			static RectangleDescriptor rectangleDescriptor = 
			{
				30, 	// minWidth (All values are estimated)
				200, 	// maxWidth
				20, 	// minHeight
				200		// maxHeight
			};
		
			static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the  
			{	
				IMAQ_NORMAL_IMAGE,	// extractionMode
				75, 				// threshold
				IMAQ_NORMAL, 		// filterSize
				25, 				// minLength
				15, 				// rowStepSize 
				15, 				// columnStepSize
				10, 				// maxEndPointGap
				FALSE,				// onlyClosed
				FALSE				// subpixelAccuracy
			};
			static ShapeDetectionOptions shapeOptions = 
			{		
				IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT,	// mode
				NULL,			// angle ranges
				0,				// num angle ranges
				{75, 125},		// scale range
				300				// minMatchScore
			};
			
			int matches = 0;
			//double highscore = 0;
			//int highestindex = -1;
			float difference = 0;
			float time = 0;
			float average = 0;
			double y = 0;
			
			
			//The big important line of code that does important stuff
			RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches);

//			DashboardDataSender *dds;
//			dds = new DashboardDataSender;

			//screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());
			//screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user
			screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user
			screen->UpdateLCD();
			
			/*for(int i = 0; i < matches; i++)
			{
				//screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score);
				if(recmatch[i].score > highscore)
				{
					highscore = recmatch[i].score;
					highestindex = i;
				}
				screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i);
				screen->UpdateLCD();
			}*/
			average = (recmatch->corner[1].x + recmatch->corner[3].x)/2;
			y = (472/(recmatch->height-4));
			screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average);
			screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height));
			//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y);
			screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x));
			screen->UpdateLCD();
			//screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x);
			//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width);
			if(average == 0)
			{
				screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found");
			}
			else if(average < 145)
			{
				difference = (160 - average);
				time = difference*0.0062;
				vic.Set(0.1);
				Wait(time);
				vic.Set(0.0);
				screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!");
				screen->UpdateLCD();
			}
			else if (average > 175)
			{
				difference = (average - 160);
				time = difference*0.0062;
				vic.Set(-0.1);
				Wait(time);
				vic.Set(0.0);
				screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!");
				screen->UpdateLCD();
			}
			else
			{
				vic.Set(0);
				screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!");
				screen->UpdateLCD();
			}
			screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time);
			screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference);

		//	screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex);
	/*		if(matches > 0)
			{
				int counter = 0;
				while (counter < 20)	
				{
					average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4;
					average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4;
				//	screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2);
					screen->UpdateLCD();
					if (average > 170)
					{
						vic.Set(-0.1);
						//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get());
						//screen->UpdateLCD();		
					}
					else if (average < 150)
					{
						vic.Set(0.1);
						//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get());
						//screen->UpdateLCD();
					}
					else
					{
						counter = 20;	
						vic.Set(0);
				//		screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
						screen->UpdateLCD();
					}
					counter++a;
				}
				*/
	//			imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL);
				
//			}
/*			else
			{
				vic.Set(0);
			//	screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
				screen->UpdateLCD();
			}*/
			imaqDispose(bitcast);
			imaqDispose(recmatch);
			//imaqDispose(roi);
			vic.Set(0);
		}

	}