Пример #1
0
	void Autonomous()
	{
		compressor.Start();//start compressor
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
		
		
		while (!shoot.Get()){//while shooter isnt cocked pull it back
			shooter.SetSpeed(1); //set motor
		}
		
		shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
		shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
		
		myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
		Wait(0.5);//wait for 0.5 seconds
		myRobot.Drive(0.0, 0.0);//stop robot
	}
	void MotorControlRight(float speed)
	{
		right_1->SetSpeed(speed);
		right_2->SetSpeed(speed);
	}
	/********************************* Teleop methods *****************************************/
	void MotorControlLeft(float speed) 
	{
		left_1->SetSpeed(speed);
		left_2->SetSpeed(speed);
	}
Пример #4
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();
				
				}
		}
	}
Пример #5
0
	void OperatorControl()
	{
		compressor.Start();
		myRobot.SetSafetyEnabled(true);
		myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
		myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
		myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
		myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
		
		DriverStation *ds;
		DriverStationLCD *dsLCD;
		ds = DriverStation::GetInstance();
		dsLCD = DriverStationLCD::GetInstance();
		
		dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
		dsLCD->UpdateLCD();
		
		while (true)
		{
			if (!compressor.GetPressureSwitchValue()){
				compressor.Start();
			}
			myRobot.ArcadeDrive(stick); 
			
			 /*PNEUMATIC CODE*/
			if (stick.GetRawButton(8)){
				shooterSole.Set(shooterSole.kForward);
			}
			if (stick.GetRawButton(1)){
				shooterSole.Set(shooterSole.kReverse);
			}
			 /*SHOOTER CODE*/
			if (stick.GetRawButton(2)){
				shooter.SetSpeed(1);
			}
			if (stick.GetRawButton(4)){
				shooter.SetSpeed(-1);
				dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
				dsLCD->UpdateLCD();
			}
			if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
				shooter.SetSpeed(0);
			}
			 /* FORKLIFT CODE*/
			if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
				forklift.SetSpeed(0);
			} 
			if (stick.GetRawButton(5)){
				forklift.SetSpeed(1);
			}
			if (stick.GetRawButton(6)){
				forklift.SetSpeed(-1);
				dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
				dsLCD->UpdateLCD();
			}
			if (!shoot.Get()){
				shooter.SetSpeed(0);
				shooterSole.Set(shooterSole.kForward);
			}
			
			
			if (stick.GetRawButton(11)){
				//myRobot.m_rearLeftMotor.SetSpeed(1);
				//myRobot.m_rearRightMotor.SetSpeed(1);
				//myRobot.m_frontLeftMotor.SetSpeed(1);
				//myRobot.m_frontRightMotor.SetSpeed(1);
			}
			//Wait(0.005);// wait for a motor update time
		}
	}