Exemplo n.º 1
0
	/**
	 * Initialization code for autonomous mode should go here.
	 * 
	 * Use this method for initialization code which will be called each time
	 * the robot enters autonomous mode.
	 */
	void RA14Robot::AutonomousInit() {
		Config::LoadFromFile("config.txt");
		auto_case = (int) Config::GetSetting("auto_case", 1);
		alreadyInitialized = true;
		auto_timer->Reset();
		auto_timer->Start();
		missionTimer->Start();
		myDrive->ResetOdometer();
		myCamera->Set(Relay::kForward);
		myCollection->ExtendArm();
		cout<<"Reseting Gyro"<<endl;
		gyro->Reset();
		//myOdometer->Reset();
		//myDrive->ShiftUp();
		myDrive->ShiftDown();
		//shift to high gear
		if (!fout.is_open()) {
			cout << "Opening logging.csv..." << endl;
			fout.open("logging.csv");
			logheaders();
		}
		auto_state = 0;
#ifndef DISABLE_SHOOTER
		myCam->Reset();
		myCam->Enable();
#endif //Ends DISABLE_SHOOTER
	}
Exemplo n.º 2
0
	void DisabledInit() { 
		//Config loading
		try {
			cameraLight->Set(Relay::kOff);
			if (!Config::LoadFromFile("config.txt")) {
				cout << "Something happened during the load." << endl;
			}
			Config::Dump();
			
			myDrive->DisablePID();
			myDrive->ResetPID();
			
			if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
				fout.close();
			myShooter->ResetShooterProcess();
			
			lc->holdState(false);
			}
		} catch (exception ex) {
			cout << "Disabled exception. Trying again." << endl;
			cout << "Exception: " << ex.what() << endl;
		}
		
		//ResetShooterMotors();
		/*
		SmartDashboard::PutNumber("Target Info S",1741);
		cout<<SmartDashboard::GetNumber("Target Info S");
		*/
	}
Exemplo n.º 3
0
	void OperatorControl()
	{
		tank.StartDrive();
		kick.StartKicker();
		while (IsOperatorControl())
		{
			comp.checkCompressor();
			tank.Drive(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT));
			kick.StateMachine(PrimaryController.GetRawButton(BUTTON_KICK));
			collect.runCollector(PrimaryController.GetRawButton(BUTTON_ROLLER_ON));
		}
	}
Exemplo n.º 4
0
   Robot() :
      // these must be initialized in the same order
	  // as they are declared above.
      //driveTrain(),
	  m_stick(Port::joystickChannel),
	  m_gamepad(Port::gamepadChannel),
      m_controller(&m_stick, &m_gamepad)
   {
      driveTrain.SetExpiration(0.1);
      // invert the left side motors
      driveTrain.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
      // you may need to change or remove this to match your robot
      driveTrain.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
   }
Exemplo n.º 5
0
 void TeleopInit() {
 	//Open logging
 	if(!fout.is_open()) {
 		fout.open("logging.csv");
 		logheaders();
 	}
 	cameraLight->Set(Relay::kOff);
 	myDrive->SetPID(KP,KI,KD);
 	myDrive->EnablePID();
 	manualAngle = 0;
 	compressor->Start();
 	myDrive->setGyroDrive(true);
 	lc->setMode(0);
 	freshStart = false;
 	
 }
Exemplo n.º 6
0
	/**
	 * Initialization code for teleop mode should go here.
	 * 
	 * Use this method for initialization code which will be called each time
	 * the robot enters teleop mode.
	 */
	void RA14Robot::TeleopInit() {
		Config::LoadFromFile("config.txt");
		myCompressor->Start();
		alreadyInitialized = true;
		missionTimer->Start();
		myDrive->ResetOdometer();
		myDrive->ShiftUp();
		//myOdometer->Reset();
		if (!fout.is_open()) {
			cout << "Opening logging.csv..." << endl;
			fout.open("logging.csv");
			logheaders();
		}

#ifndef DISABLE_SHOOTER
		myCam->Reset();
		myCam->Enable();
#endif //Ends DISABLE_SHOOTER
	}
Exemplo n.º 7
0
	void RA14Robot::logheaders() {
		fout << "MissionTimer,";
#ifndef DISABLE_SHOOTER
		myCam->logHeaders(fout);
#endif //Ends DISABLE_SHOOTER
		myDrive->logHeaders(fout);
		fout << "CAMLeftCurrent,CAMRightCurrent,DriveLeftCurrent,DriveRightCurrent,AutoCase,GyroHeading,DropSensor,BatteryVoltage,";
		fout << "TargetValid,TargetHot,TargetDistance,TargetX,TargetY,TargetIsLeft,TargetIsRight,MatchTime,AutoInternalState,";
		fout << endl;
	}
Exemplo n.º 8
0
	/*
	 * Checks if the string from the Auto Modes meets any of the constants above. If so, it runs that Auto Mode.
	 * Right now has testing modes, but autonomous->driveOverDefense(); will do the checking for us.
	 */
	void AutonomousPeriodic(){
		//autonomous->driveOverDefense();
		const int kModesEqual=0;
		//autonomous->test();

		if (autoSelected.compare(DoNothing)==kModesEqual){
			autonomous->doNothing();
			SmartDashboard::PutString("DN TEST", "Done");
		}
		else if (autoSelected.compare(autoNameOver)==kModesEqual){
			autonomous->driveOverDefense();

		}
		else if (autoSelected.compare(autoNameRW)==kModesEqual){

			autonomous->rockWallOrMoat();
		}
		else if (autoSelected.compare(autoNameLeft)==kModesEqual){


			autonomous->leftLowGoal();
		}
		else if (autoSelected.compare(autoNameRight)==kModesEqual){

			autonomous->rightLowGoal();

		}
		else if (autoSelected.compare(autoNameP1)==kModesEqual){

			autonomous->p1High();
		}

		else if (autoSelected.compare(autoNameP2)==kModesEqual){

			autonomous->p2High();
		}

		else if (autoSelected.compare(autoNameP3)==kModesEqual){

			autonomous->p3High();
		}
		else if (autoSelected.compare(autoNameP4)==kModesEqual){

			autonomous->p4High();
		}


		else if (autoSelected.compare(autoNameP5)==kModesEqual){
			autonomous->p5High();

		}

		Drive->stayAtTheTop();

	}
Exemplo n.º 9
0
   /**
    * Runs the motors with Mecanum drive.
    */
   void OperatorControl()
   {
      driveTrain.SetSafetyEnabled(false);
      while (IsOperatorControl() && IsEnabled())
      {
         // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
         // This sample does not use field-oriented drive, so the gyro input is set to zero.
         // TODO change stick to m_controller
    	  //driveTrain.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());

         // wait 5ms to avoid hogging CPU cycles
         Wait(0.005);
      }
   }
Exemplo n.º 10
0
	void AutonomousInit(){
		SmartDashboard::PutString("AutonomousInit", "Autonomous Init Reached");
		autonomous->setSelected(*((std::string*)chooser->GetSelected()));
		//autoSelected = SmartDashboard::GetString("Auto Selector", *((std::string*)chooser->GetSelected()));
		autoSelected = *((std::string*)chooser->GetSelected());
		SmartDashboard::PutString("Auto Selected", autoSelected);
		//testSelected= SmartDashboard::GetString("Auto Selector", *((std::string*)chooser2->GetSelected()));
		testSelected = *((std::string*)chooser2->GetSelected());

		Drive->AutonomousInit();
		autonomous->startTimer();
		autonomous->reset();


	}
Exemplo n.º 11
0
 //logging functions
 void logheaders()
 {
 	fout << "Time,Mode,Battery,Gyro,";
 	autoLogHeaders();
 	myDrive->logHeaders(fout);
 	myShooter->logHeaders(fout);
 	//myClimber->LogHeaders(fout);
 	log_gamepad_headers(fout, "Driver");
 	log_gamepad_headers(fout, "Gamepad");
 	//autoLogHeaders();
 	fout << "FancyLights,";
 	fout << "Light1,";
 	fout << "Light2,";
 	fout<<endl;
 }
Exemplo n.º 12
0
 void logdata()
 {
 	//log data
 	fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ',';
 	fout << magicBox->getGyroAngle() << ',';
 	autoLog();
 	myDrive->log(fout);
 	myShooter->log(fout);
 	//myClimber->Log(fout);
 	log_gamepad(fout, driverGamepad);
 	log_gamepad(fout, operatorGamepad);
 	fout<<LEDTimer<<",";
 	fout<<LEDPercent(LEDTimer)<<",";
 	fout<<LEDPercent(LEDTimer + 30)<<",";
 	fout<<endl;
 }
Exemplo n.º 13
0
	void RA14Robot::logging() {
		if (fout.is_open()) {
		fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
		myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
		myDrive->log(fout);
		CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
				driveLeftSlot, driveRightSlot };
		
		DriverStation * ds = DriverStation::GetInstance();

		for (int i = 0; i < 4; ++i) {
			fout << slots[i]->Get() << ",";
		}

		fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
		fout << target->IsValid() << "," << target->IsHot() << "," <<  target->GetDistance() << "," << target->GetX() << ",";
		fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
		fout << ds->GetMatchTime() << "," << auto_state << ",";
		fout << endl;
		}
	}
Exemplo n.º 14
0
	/*Initializes objects necessary for teleop
	 * Adds options to sendable chooser on SmartDashboard
	 */
	void RobotInit(){
		/*if (fork()==0){
			system("/home/lvuser/start_vision &");
		}*/

		//creates DriveTrain and sendable choosers
		Drive= new DriveTrain();
		chooser = new SendableChooser();
		chooser2 = new SendableChooser();


		//adds options to both
		chooser->AddDefault(DoNothing, (void*)&DoNothing);
		chooser->AddObject(autoNameLeft, (void*)&autoNameLeft);
		chooser->AddObject(autoNameRight, (void*)&autoNameRight);
		chooser->AddObject(autoNameOver, (void*)&autoNameOver);
		chooser->AddObject(autoNameP1, (void*)&autoNameP1);
		chooser->AddObject(autoNameP2, (void*)&autoNameP2);
		chooser->AddObject(autoNameP3, (void*)&autoNameP3);
		chooser->AddObject(autoNameP4, (void*)&autoNameP4);
		chooser->AddObject(autoNameP5, (void*)&autoNameP5);
		chooser->AddObject(autoNameRW, (void*)&autoNameRW);
		SmartDashboard::PutData("Auto Modes", chooser);


		chooser2->AddDefault(autoNameTest1, (void*)&autoNameTest1);
		chooser2->AddObject(autoNameTest2, (void*)&autoNameTest2);
		chooser2->AddObject(autoNameTest3, (void*)&autoNameTest3);
		chooser2->AddObject(autoNameTest4, (void*)&autoNameTest4);
		SmartDashboard::PutData("Auto Modes 2", chooser2);
		Drive->AutonomousInit();
		autonomous = new Autonomous(Drive, *((std::string*)chooser->GetSelected()));

		//passes driveTrain and sendable chooser option to Autonomous class and creates it!

	}
Exemplo n.º 15
0
	//completes all necessary functions for teleop driving/operating
	void TeleopPeriodic(){
		Drive->Drive();
		Drive->stayAtTheTop();

	}
Exemplo n.º 16
0
    void TeleopPeriodic() {
    	//Target * target = GetLatestTarget();
    	//Begin DRIVER input section
    	if(driverGamepad->GetDPad()==Gamepad::kUp)
    		lc->setMode(1);
    	else if(driverGamepad->GetDPad()==Gamepad::kDown)
    		lc->setMode(6);
    	else
    		lc->setMode(0);
    	if(driverGamepad->GetDPad() == Gamepad::kRight)
    	{
    		cameraLight->Set(Relay::kOff);
    	}
    	else if(driverGamepad->GetDPad() == Gamepad::kLeft)
    	{
    		cameraLight->Set(Relay::kOn);
    	}
    	
    	if(driverGamepad->GetB())
    	{
    		cout << "Resetting the gyro! :D" << endl;
    		//driveGyro->Reset();
    		magicBox->resetGyro();
   		}
    	
    	bool modifySpeed = driverGamepad->GetRightBumper();
    	bool halfSpeed = driverGamepad->GetLeftBumper();
    	
    	/*
    	try {
    		std::string temp = SmartDashboard::GetString("TargetInfo");
    		cout << "temp: " << endl;
    	} catch (exception ex) {
    		cout << "bah! " << ex.what() << endl;
    	}*/
    	
    	//Target t = GetLatestTarget();
    	
    	/*
    	if (t.IsValid()) {
    		cout << "Valid target (" << (t.IsCenter() ? "center" : "side") << "):" << t.Distance() << " ft, " << t.X() << "," << t.Y() << endl;
    	} else {
    		//cout << "No valid target." << endl;
    	} */
    		
    	if(driverGamepad->GetBack())
    	{
    		cout << "Now driving in relation to the robot" << endl;
    			myDrive->setGyroDrive(false);
    	}
    	if(driverGamepad->GetStart())
    	{
    		cout << "Now driving in relation to the field (gyro)" << endl;
    		myDrive->setGyroDrive(true);
    	}
    	
    	// If the driver is holding the X button 
    	/*
    	if (driverGamepad->GetX()) {
    		// Stop using the joysticks to drive the robot, and let this function do it
    	    PointAtTarget(t);
    	} else { */
    		// We aren't auto-targeting. Reset the sequence.
    		target_state = RA13Robot::READY;
    		// Drive based on joysticks.
    		myDrive->Drive(driverGamepad->GetLeftX(), driverGamepad->GetLeftY(), 
    		    			driverGamepad->GetRightX(), magicBox->getGyroAngle(), modifySpeed,halfSpeed);
    	//}
    	//End DRIVER input section
    	
    	Gamepad::DPadDirection dir = operatorGamepad->GetDPad();
    	
    	if (dir != lastdir) {
    		switch(dir) {
    		case Gamepad::kUp:
    			manualAngle += 1;
    			break;
    		case Gamepad::kDown:
    			manualAngle -= 1;
    			break;
    		case Gamepad::kRight:
    			manualAngle += 0.25;
    			break;
    		case Gamepad::kLeft:
    			manualAngle -= 0.25;
    			break;
    		default:
    			break;
    		}
    	}
    	
    	//cout << "Current target angle: " << manualAngle << endl;
    	
    	//Begin OPERATOR input section
    		
    	if(operatorGamepad->GetBack())
    	{
    		myShooter->ResetShooterProcess(); // Re-homes the shooter
    	}
    	
    	if(operatorGamepad->GetLeftTrigger())
		{
    		myShooter->SetSpeeds(SHOOTER_FRONTWHEEL_SPEED,SHOOTER_REARWHEEL_SPEED); // (RPM,RPM)
		}
    	else if (operatorGamepad->GetLeftBumper())
    	{
    		myShooter->SetSpeeds(Config::GetSetting("shooter_main_speed_pyramid", 1500),
    							 Config::GetSetting("shooter_secondary_speed_pyramid", 2000));
    	}
    	else
    	{
    		myShooter->SetSpeeds(0,0); // (RPM,RPM)
    	}
    	
    	if(operatorGamepad->GetRightTrigger())
    	{
    		// TODO: Make this selectable somehow. This is probably too powerful for
    		// distant shots.
    		myShooter->ChamberFrisbee(Config::GetSetting("loader_power",-1));
    	}
    	else
    	{
    		myShooter->StopLoader();
    	}
    	
    	if(operatorGamepad->GetY())
    	{
    		myShooter->SetAngle(manualAngle);
    	}
    	if(operatorGamepad->GetX())
    	{
    		myShooter->SetAngle(Config::GetSetting("shooter_loading_angle",0)); //Angle for reloading
    		manualAngle = Config::GetSetting("shooter_loading_angle",0);
    	}
    	if(operatorGamepad->GetA()) //Shooting from load zone
    	{
    		myShooter->SetAngle(Config::GetSetting("load_zone_shooting_angle",13.25));
    		manualAngle = Config::GetSetting("load_zone_shooting_angle",13.25);
    	}
    	if(operatorGamepad->GetB())
    	{
    		myShooter->SetAngle(Config::GetSetting("pyramid_shooting_angle",23.25));
    		manualAngle = Config::GetSetting("pyramid_shooting_angle",23.25);
    	}
    	if(operatorGamepad->GetRightBumper())
    	{
    		//myShooter->SetAngle(Config::GetSetting("funnel_release_angle",35));
    		myShooter->SetAngle(Config::GetSetting("pyramid_basket_angle", 45));
    		manualAngle = Config::GetSetting("pyramid_basket_angle", 45);
    	}
    	if (operatorGamepad->GetLeftPush()) {
    		myShooter->ResetShooterProcess();
    	}
    	
    	// Do something similar, but don't touch the AngleJag.
    	if (operatorGamepad->GetRightPush()) {
    		myShooter->ResetShooterMotors();
    	}
    	
    	//pneumatics section
    	if(!driverGamepad->GetLeftTrigger())
    	{
    		pneumatics->extendEndgame();
    	}
    	else
    	{
    		pneumatics->retractEndgame();
    		myShooter->SetAngle(3);
    	}
    	
    	if(driverGamepad->GetRightTrigger())
    	{
    		pneumatics->extendCollector();
    	}
    	else
    	{
    		pneumatics->retractCollector();
    	}
    	//end pneumatics section
    	
    	lastdir = dir;
    	
    	myShooter->Process();
    	
    	//cout << "Shooter rear wheel" << myShooter->getRearWheelSpeed() << endl;
    	//End OPERATOR input section
   
    	
    	/*Manual control of the angle motor position using PID
    	 * THIS ASSUMES THE SHOOTER HAS BEEN ZERO'ED TO THE HOME POSITION SOMEHOW
    	if(operatorGamepad->GetLeftTrigger())
    	{
    		myShooter->ManualAngleControl(16); // (theta)
    	}
    	else
    	{
    		myShooter->ManualAngleControl(0);
    	}
    	*/ 
    	/*if (target != NULL) {
    		cout << "Target located. (" << target->X() << ',' << target->Y() << ") range is " << target->Distance() << " feet" << endl;
    	}
    	*/
//    	switch(myShooter->getFiringState())
//    	{
//    	case 1:
//    		lc->setMode(4);
////    		cout << "Spinning up!!" << endl;
//    		break;
//    	case 2:
//    		lc->setMode(5);
////    		cout << "Firing!!" << endl;
//    		break;
//    	default:
//    		lc->setMode(1);
//    		break;
//    	}
    	lc->useLights();
    	BuildAndSendPacket();
    	logdata();
    }
Exemplo n.º 17
0
	/**
	 * Periodic code for autonomous mode should go here.
	 *
	 * Use this method for code which will be called periodically at a regular
	 * rate while the robot is in autonomous mode.
	 */
	void RA14Robot::AutonomousPeriodic() {
		StartOfCycleMaintenance();

		target->Parse(server->GetLatestPacket());
		float speed = Config::GetSetting("auto_speed", .5);
		cout<<"Auto Speed: "<<Config::GetSetting("auto_speed", 0)<<endl; // original .1
		float angle = gyro->GetAngle();
		float error = targetHeading - angle;
		float corrected = error * Config::GetSetting("auto_heading_p", .01);
		//float corrected = error * Config::GetSetting("auto_heading_p", .01);
		cout <<"Gyro angle: "<<angle<<endl;
		cout <<"Error: " << error << endl;
		//float lDrive = Config::GetSetting("auto_speed", -0.3) + (error * Config::GetSetting("auto_heading_p", .01));
		//float rDrive = Config::GetSetting("auto_speed", -0.3) - (error * Config::GetSetting("auto_heading_p", .01));
		// Reading p value from the config file does not appear to be working. When we get p from config, the math is not correct.
		float lDrive = Config::GetSetting("auto_speed", -0.3) + (error*0.01);
		float rDrive = Config::GetSetting("auto_speed", -0.3) - (error*0.01);
		cout << "Left: " << lDrive << endl;
		cout << "Right: " << rDrive << endl;
									
		
#ifndef DISABLE_AUTONOMOUS
		switch(auto_case)
		{
			case 0:
				// start master autonomous mode
				switch (auto_state) {
				case 0: // start
					auto_timer->Reset();
					auto_timer->Start();
					myCam->Process(false, false, false);
					break;
				case 1:
					myCam->Process(false, false, false);
					if (target->IsValid()) {
						auto_state = 2;
					} else if (auto_timer->Get() >= Config::GetSetting("auto_target_timeout", 1)) {
						auto_state = 10;
					}
					break;
				case 2:
					myCam->Process(false, false, false);
					if (target->IsHot()) {
						auto_state = 10;
					} else {
						if (auto_timer->Get() >= Config::GetSetting("auto_target_hot_timeout", 5)) {
							auto_state = 10;
						}
					}
					break;
				case 10:
					myCam->Process(true, false, false);
					if (myCam->IsReadyToRearm()) {
						auto_state = 11;
					}
					break;
				case 11:
					myCam->Process(false, false, false);
					myDrive->DriveArcade(corrected, speed);
					if (myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 100))
					{
						myDrive->Drive(0,0);
					}
					break;
				case 12:
					myDrive->Drive(0,0);
					break;
				default:
					cout << "Unknown state #" << auto_state << endl; 
					break;
				}
				// end master autonomous mode
				break;
			case 1:
				if( target->IsHot() && target->IsValid() )
				{
					cout << "Target is HOTTT taking the shot" << endl;
					//Drive forward and shoot right away
					//if( target->IsLeft() || target->IsRight() )
					//{
					if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
					{
						myDrive->Drive(corrected,speed);
					}
					else
					{
						myDrive->Drive(0,0);
						cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
						myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
					}
				/*}
				else
				{
					cout<<"Error"<<endl;
				}*/
				}
				else if(target->IsValid())
				{
					cout << "Target is valid, but cold. Driving and waiting" << endl;
					//Drive forward and wait to shoot 
					if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
					{
						myDrive->Drive(corrected, speed);
					}
					else
					{
						// now at the firing spot.
						myDrive->Drive(0,0);
						if( target->IsHot() )
						{
							cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
							myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
						}
					}
				}
				else
				{
					//Not valid
					cout << "Not valid target." << endl;
				}
				break;
			
			case 2:
#ifndef DISABLE_SHOOTER
			myCam->Process(false,false,false);
			if(auto_timer->Get() >= 4.0)
			{
				myCam->Process(true,false,false);
			}
#endif //Ends DISABLE_SHOOTER
			
			/*if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
			{
				cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
				myDrive->Drive(corrected, speed);
			}
			else
			{
				myDrive->Drive(0,0);
				cout << "FIRING" << endl;
			}*/
			if(auto_timer->Get() < 5.0)
			{
				cout<<"Waiting....."<<endl;
			}
			else
			{
				cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
				myDrive->Drive(-.5, -.5);
			}
				
			break;
			
			case 3:
#ifndef DISABLE_SHOOTER
			switch(auto_state) {
				case 0:
					// Home/rearm
					// fire, rearm, eject
					myCam->Process(false, false, false);
					if (myCam->IsReadyToFire()) {
						auto_state = 1;
					}
					break;
				case 1:
					// fire
					myCam->Process(true, false, false);
					if (myCam->IsReadyToRearm()) {
						auto_state = 2;
					}
					break;
				case 2:
					// rearm
					myCam->Process(false, true, false);
					if (myCam->IsReadyToFire()) {
						auto_state = 3;
						auto_timer->Reset();
						auto_timer->Start();
					}
					break;
				case 3:
					myCam->Process(false, false, false);
					myCollection->SpinMotor(Config::GetSetting("intake_roller_speed", .7));
					if (auto_timer->HasPeriodPassed( Config::GetSetting("auto_collection_delay", 1.0) )) {
						auto_state = 4;
					}
					break;
				case 4:
					// fire again!
					myCam->Process(true, false, false);
					auto_state = 5;
					break;
				case 5:
					myCam->Process(false, false, false);
					
					if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
					{
						myDrive->Drive(corrected, speed);
					} else {
						// now at the firing spot.
						auto_state = 6;
						myDrive->Drive(0,0);
					}
					break;
				case 6:
					myCollection->RetractArm();
					break;
				default:
					cout << "Error, unrecognized state " << auto_state << endl;
			}
#endif //Ends DISABLE_SHOOTER
			
			break;
			
			case 4:	/* One ball Autonomous - Drive forward specific distance, stop then shoot.*/
#ifndef DISABLE_SHOOTER
		
					switch(auto_state) {
		
						case 0:		//Reset odometer, lower the arm and set launcher to ready to fire
							myDrive->ShiftUp();
							myDrive->ResetOdometer();
							myCollection->ExtendArm();
							myCam->Process(false,true,false);
							auto_state = 1;
							break;
						case 1:		// Start driving forward
							// myDrive->Drive(-.5, -.5);
							//myDrive->Drive(lDrive, rDrive);
							myDrive->DriveArcade(corrected, speed);
							auto_state = 2;
							break;
						case 2:		// Continue driving until required distance
							if(myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 96)) {
								myDrive->Drive(0, 0);
								auto_state = 3;
							}
							break;
						case 3:		// Fire launcher
							myCam->Process(true,false,false);
							auto_state = 4;
							break;
						case 4:		// Idle state
							break;
					}
			
#endif //Ends DISABLE_SHOOTER
			
			break;
			
			case 5:	// Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
#ifndef DISABLE_SHOOTER
				// By Hugh Meyer - April 1, 2014
				
					switch(auto_state) {
					cout << "Executing mr m's auton" << endl;
				
						case 0:		// Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
							//myDrive->ShiftUp();				// Shift to low gear
							myDrive->ShiftDown();
							myDrive->ResetOdometer();			// Reset odometer to zero
							myCollection->ExtendArm();			// Extend arm to pickup position
							myCam->Process(false,true,false);	// Set launcher to ready to fire position
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while pickup extends
							auto_state = 1;						// Go on  to next state
							break;
																			
						case 1:		// Wait for timer to expire - Let arm get extended and stabalized
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_extend_delay", 1.0) )) {
								auto_state = 2;
							}
							break;
						case 21: 	//reset gyro and reset timer
							gyro->Reset();
							auto_timer->Reset();
							auto_timer->Start();
							auto_state = 22;
							break;
						case 22:
							if(auto_timer->HasPeriodPassed(Config::GetSetting("auton5_gyro_reset_delay", 2) )){
								auto_state = 2;
							}	
							break;								
						case 2:		// Activate pickup roller motor to drag speed, start driving forward
							myCollection->SpinMotor(Config::GetSetting("auton5_drag_speed", 0.3));	// Start motor to drag ball 2
							//myDrive->DriveArcade(corrected, speed);		// Drive straight
							myDrive->DriveArcade(0.0, Config::GetSetting("auton5_drive_speed", -0.7)); //Start driving forwards
							auto_state = 3;
							break;
						case 3:		// Continue driving until required distance, stop driving, stop pickup roller motor
							if(myDrive->GetOdometer() >= Config::GetSetting("auton5_drive_distance", 96.0))
							{
								myCollection->SpinMotor(0);		// Stop collector pickup motor
								myDrive->Drive(0, 0);			// Stop driving
								auto_state = 31;				// On to next state
							}
							break;
						case 31: // slightly un-eject herded ball to avoid contact with launch ball
							myCollection->SpinMotor(Config::GetSetting("auton5_eject_speed", 0.3));
							auto_timer->Reset();
							auto_timer->Start(); // setup timer to time un-eject
							auto_state = 32;
							break;
						case 32: // once timer has run out, stop ejection and fire
							if (auto_timer->HasPeriodPassed(Config::GetSetting("auton5_uneject_time", 0.25))) {
								myCollection->SpinMotor(0); // stop spinner
								auto_state = 4;
							}
							break;
						case 4:		// Fire launcher to shoot first ball, set wait timer
							myCam->Process(true,false,false);	// Fire ball # 1
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball launches
							auto_state = 5;						// On to next state
							break;
						case 5:		// Wait for timer to expire after ball one launches
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_1_launch_delay", 1.0) )) {
								auto_state = 6;					// On to next state
							}
							break;
						case 6:		// set launcher ready to fire for ball 2, set wait timer
							myCam->Process(false,true,false);	// Set launcher to ready to fire position
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball launches
							auto_state = 7;						// On to next state
							break;
						case 7:		// Wait for timer to expire
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_ready2fire_delay", 1.0) )) {
								auto_state = 8;					// Wait for launcher to get ready to accept ball 2
							}
							break;
						case 8:		// Activate pickup roller motor to load ball 2, set wait timer
							myCollection->SpinMotor(Config::GetSetting("auton5_intake_roller_speed", 0.7));
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball 2 loads
							auto_state = 9;						// On to next state
							break;
						case 9:		// Wait for timer to expire while ball 2 gets collected into launcher
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_settle_delay", 1.0) )) {
								auto_state = 10;				// Wait for ball 2 to be collected and settle
							}
							break;

						case 19:	// Drive backwards a little bit, set wait timer
							myDrive->DriveArcade(-1*corrected, -1*speed);		// Drive straight
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball launches
							auto_state = 20;				// Wait for ball 2 to be collected and settle
							break;
		
							
						case 20:		// Wait for timer to expire while ball 2 gets collected into launcher
							if (myDrive->GetOdometer() <= 
									  Config::GetSetting("auton5_drive_distance", 96) 
									- Config::GetSetting("auton5_backup_distance", 6))	{
								myDrive->Drive(0,0);
								auto_state = 10;
							}
							break;
						case 10:	// Fire launcher to shoot second ball and stop roller
							myCam->Process(true,false,false);	// Fire ball # 2
							myCollection->SpinMotor(0);			// Stop spinning the roller
							auto_state = 11;					// All done so go to idle state
							break;
						case 11:	// Idle state
							auto_state = 11;
							break;
/*						case 12:	// More states if we need them for changes.
							auto_state = 13;
							break;
						case 13:	// 
							auto_state = 14;
							break;
						case 14:	// 
							auto_state = 15;
							break;
						case 15:	// 
							auto_state = 15;
							break;
*/
					}
					
#endif //Ends DISABLE_SHOOTER
					
			break;
				
			
			case 6:	// Two Ball Autonomous - Drive forward specific distance, stop then shoot, backup get second ball, drive, shoot
#ifndef DISABLE_SHOOTER
				// By Hugh Meyer - April 1, 2014
					
					switch(auto_state) {
						
						case 0:		// Set low gear, reset odometer, extend pickup arm, set launcher ready to fire
							myDrive->ShiftDown();				// Shift to low gear
							myDrive->ResetOdometer();			// Reset odometer to zero
							myCollection->ExtendArm();			// Extend arm to pickup position
							myCam->Process(false,true,false);	// Set launcher to ready to fire position
							auto_state = 1;						// Go on to next state
							break;
						case 1:		// Drive forward
							myDrive->DriveArcade(corrected, speed);
							auto_state = 2;						// On to next state
							break;
						case 2:		// Continue driving forward until the specific distance is traveled
							if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
								myDrive->Drive(0, 0);
								auto_state = 3;
							}
							break;
						case 3:		// Fire launcher to launch first ball, set timer
							myCam->Process(true,false,false);	// Launch ball # 1
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball launches
							auto_state = 4;						// On to next state
							break;	
						case 4:		// Wait for timer while ball launcher fires
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_1_fire_delay", 1.0) )) {
								auto_state = 5;					// On to next state
							}
							break;
						case 5:		// Reset Odometer, Drive backwards, set launcher to ready to fire position, turn on pickup
							cout<<"Corrected Values: "<<corrected<<endl; 
							cout<<"Speed: "<< -1 * speed<<endl;
							myDrive->ResetOdometer();				// Reset odometer to zero
							myDrive->DriveArcade(-1* corrected, -1 * speed);	// Drive backwards
							myCam->Process(false,true,false);		// Set launcher to ready to fire position
							myCollection->SpinMotor(-1 * Config::GetSetting("auton6_intake_roller_speed", 0.7));	// Turn on pickup
							auto_state = 6;						// On to next state
							break;
						case 6:		// Continue driving backwards a specific distance
							if(myDrive->GetOdometer() <= -1 * Config::GetSetting("auton6_drive_forward_distance", -96.0)) {
								auto_state = 7;					// On to next state
							}
							break;
						case 7:		// Set timer
							auto_timer->Reset();				// Set timer to zero
							auto_timer->Start();				// Start the timer for a short delay while ball launches
							auto_state = 8;						// On to next state
							break;
						case 8:		// Wait for timer while ball loads and settles
							if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_2_load_delay", 1.0) )) {
								auto_state = 9;					// On to next state
								myDrive->ResetOdometer();
							}
							break;
						case 9:		// Drive forwards
							myDrive->DriveArcade(/*corrected*/ 0.01, speed);
							auto_state = 10;					// On to next state
							break;
						case 10:	// Continue driving forward until the specific distance is traveled
							cout << "I am driving forward: " << myDrive->GetOdometer() << endl;
							myDrive->DriveArcade(0.01, speed);
							if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
								myDrive->Drive(0, 0);
								auto_state = 11;				// On to next state
							}
							break;
						case 11:	// Fire launcher to launch second ball
							myCam->Process(true,false,false);	// Launch ball # 2
							auto_state = 15;					// On to next state
							break;
/*						case 12:	// 
							auto_state = 13;					// On to next state
							break;
						case 13:	// 
							auto_state = 14;					// On to next state
							break;
						case 14:	// 
							auto_state = 15;					// All done so go to idle state
							break;
*/
						case 15:	// Idle state
							auto_state = 15;
							break;

					}
					cout << myDrive->GetOdometer() << endl;
							
#endif //Ends DISABLE_SHOOTER
							
			break;
			case 7:	// Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
			#ifndef DISABLE_SHOOTER
							// By Hugh Meyer - April 1, 2014
							
								switch(auto_state) {
								cout << "Executing mr m's auton" << endl;
							
									case 0:		// Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
										myDrive->ShiftUp();				// Shift to high gear
										//myDrive->ShiftDown();
										myDrive->ResetOdometer();			// Reset odometer to zero
										myCollection->ExtendArm();			// Extend arm to pickup position
										myCam->Process(false,true,false);	// Set launcher to ready to fire position
										auto_timer->Reset();				// Set timer to zero
										auto_timer->Start();				// Start the timer for a short delay while pickup extends
										auto_state = 1;						// Go on  to next state
										break;
																						
									case 1:		// Wait for timer to expire - Let arm get extended and stabalized
										if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_extend_delay", 1.0) )) {
											auto_state = 2;
										}
										break;
									case 21: 	//reset gyro and reset timer
										gyro->Reset();
										auto_timer->Reset();
										auto_timer->Start();
										auto_state = 22;
										break;
									case 22:
										if(auto_timer->HasPeriodPassed(Config::GetSetting("auton7_gyro_reset_delay", 2) )){
											auto_state = 2;
										}	
										break;								
									case 2:		// Activate pickup roller motor to drag speed, start driving forward
										myCollection->SpinMotor(Config::GetSetting("auton7_drag_speed", 0.3));	// Start motor to drag ball 2
										//myDrive->DriveArcade(corrected, speed);		// Drive straight
										myDrive->DriveArcade(0.05, speed);
										auto_state = 3;
										break;
									case 3:		// Continue driving until required distance, stop driving, stop pickup roller motor
										if(myDrive->GetOdometer() >= Config::GetSetting("auton7_drive_distance", 96.0))
										{
											myCollection->SpinMotor(0);		// Stop collector pickup motor
											myDrive->Drive(0, 0);			// Stop driving
											auto_state = 31;				// On to next state
										}
										break;
									case 31: // slightly un-eject herded ball to avoid contact with launch ball
										myCollection->SpinMotor(Config::GetSetting("auton7_eject_speed", 0.3));
										auto_timer->Reset();
										auto_timer->Start(); // setup timer to time un-eject
										auto_state = 32;
										break;
									case 32: // once timer has run out, stop ejection and fire
										if (auto_timer->HasPeriodPassed(Config::GetSetting("auton7_uneject_time", 0.25))) {
											myCollection->SpinMotor(0); // stop spinner
											auto_state = 4;
										}
										break;
									case 4:		// Fire launcher to shoot first ball, set wait timer
										myCam->Process(true,false,false);	// Fire ball # 1
										auto_timer->Reset();				// Set timer to zero
										auto_timer->Start();				// Start the timer for a short delay while ball launches
										auto_state = 5;						// On to next state
										break;
									case 5:		// Wait for timer to expire after ball one launches
										if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_1_launch_delay", 1.0) )) {
											auto_state = 6;					// On to next state
										}
										break;
									case 6:		// set launcher ready to fire for ball 2, set wait timer
										myCam->Process(false,true,false);	// Set launcher to ready to fire position
										auto_timer->Reset();				// Set timer to zero
										auto_timer->Start();				// Start the timer for a short delay while ball launches
										auto_state = 7;						// On to next state
										break;
									case 7:		// Wait for timer to expire
										if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_ready2fire_delay", 1.0) )) {
											auto_state = 8;					// Wait for launcher to get ready to accept ball 2
										}
										break;
									case 8:		// Activate pickup roller motor to load ball 2, set wait timer
										myCollection->SpinMotor(Config::GetSetting("auton7_intake_roller_speed", 0.7));
										auto_timer->Reset();				// Set timer to zero
										auto_timer->Start();				// Start the timer for a short delay while ball 2 loads
										auto_state = 9;						// On to next state
										break;
									case 9:		// Wait for timer to expire while ball 2 gets collected into launcher
										if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_settle_delay", 1.0) )) {
											auto_state = 10;				// Wait for ball 2 to be collected and settle
										}
										break;

									case 19:	// Drive backwards a little bit, set wait timer
										myDrive->DriveArcade(-1*corrected, -1*speed);		// Drive straight
										auto_timer->Reset();				// Set timer to zero
										auto_timer->Start();				// Start the timer for a short delay while ball launches
										auto_state = 20;				// Wait for ball 2 to be collected and settle
										break;
					
										
									case 20:		// Wait for timer to expire while ball 2 gets collected into launcher
										if (myDrive->GetOdometer() <= 
												  Config::GetSetting("auton7_drive_distance", 96) 
												- Config::GetSetting("auton7_backup_distance", 6))	{
											myDrive->Drive(0,0);
											auto_state = 10;
										}
										break;
									case 10:	// Fire launcher to shoot second ball and stop roller
										myCam->Process(true,false,false);	// Fire ball # 2
										myCollection->SpinMotor(0);			// Stop spinning the roller
										auto_state = 11;					// All done so go to idle state
										break;
									case 11:	// Idle state
										auto_state = 11;
										break;
			/*						case 12:	// More states if we need them for changes.
										auto_state = 13;
										break;
									case 13:	// 
										auto_state = 14;
										break;
									case 14:	// 
										auto_state = 15;
										break;
									case 15:	// 
										auto_state = 15;
										break;
			*/
								}
								
			#endif //Ends DISABLE_SHOOTER
								
						break;
/*			
			
			case 7:	// Extra structure for more changes
#ifndef DISABLE_SHOOTER
				// By Hugh Meyer - April 1, 2014
					
					switch(auto_state) {
						
						case 0:		// Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, drive foward
							myDrive->ShiftDown();				// Shift to low gear
							myDrive->ResetOdometer();			// Reset odometer to zero
							myCollection->ExtendArm();			// Extend arm to pickup position
							myCam->Process(false,true,false);	// Set launcher to ready to fire position
							myDrive->Drive(lDrive, rDrive);		// Drive straight
							auto_state = 1;						// Go on  to next state
							break;
						case 1:		// 
							auto_state = 2;						// On to next state
							break;
						case 2:		// 
							auto_state = 3;						// On to next state
							break;
						case 3:		// 
							auto_state = 4;						// On to next state
							break;	
						case 4:		// 
							auto_state = 5;						// On to next state
							break;
						case 5:		// 
							auto_state = 6;						// On to next state
							break;
						case 6:		// 
							auto_state = 7;						// On to next state
							break;
						case 7:		// 
							auto_state = 8;						// On to next state
							break;
						case 8:		// 
							auto_state = 9;						// On to next state
							break;
						case 9:		// 
							auto_state = 10;					// On to next state
							break;
						case 10:	// 
							auto_state = 11;					// On to next state
							break;
						case 11:	// 
							auto_state = 12;					// On to next state
							break;
						case 12:	// 
							auto_state = 13;					// On to next state
							break;
						case 13:	// 
							auto_state = 14;					// On to next state
							break;
						case 14:	// 
							auto_state = 15;					// All done so go to idle state
							break;
						case 15:	// Idle state
							auto_state = 15;
							break;
					}
								
#endif //Ends DISABLE_SHOOTER
									
			break;
*/								
			
			default:
			cout<<"Error in autonomous, unrecognized case: "<<auto_case<<endl;
		}
//#else
//		if (myDrive->GetOdometer() <= (4 * acos(-1) ) ) //216 is distance from robot to goal
//		{
//			float speed = Config::GetSetting("auto_speed", .3);
//			cout << myDrive->GetOdometer() << endl;
//			myDrive->Drive(speed, speed);
//		} else {
//			cout << "Finished driving";
//			myDrive->Drive(0, 0);
//		}
#endif //Ends DISABLE_AUTONOMOUS

		EndOfCycleMaintenance();
	}
Exemplo n.º 18
0
	/**
	 * Periodic code for teleop mode should go here.
	 *
	 * Use this method for code which will be called periodically at a regular
	 * rate while the robot is in teleop mode.
	 */
	void RA14Robot::TeleopPeriodic() {
		StartOfCycleMaintenance();

		//Input Acquisition
		DriverLeftY = DriverGamepad->GetLeftY();
		DriverRightY = DriverGamepad->GetRightY();
		DriverLeftBumper = DriverGamepad->GetLeftBumper(); // Reads state of left bumper 
		DriverRightBumper = DriverGamepad->GetRightBumper(); // Gets state of right bumper
		RingLightButton = OperatorGamepad->GetY();
		ShouldFireButton = DriverGamepad->GetRightTrigger();
		BallCollectPickupButton = DriverGamepad->GetBack();
		DriverDPad = DriverGamepad->GetDPad();
		OperatorDPad = OperatorGamepad->GetDPad();
		//End Input Acquisition


		//Current Sensing
		//myCurrentSensor->Toggle(DriverGamepad->GetStart());
		//End Current Sensing


		//Target Processing
		target->Parse(server->GetLatestPacket());

		/*
		 if (target->IsValid()) {
		 cout << "\tx = " << target->GetX() << ", y = " << target->GetY() << ", distance = " << target->GetDistance() << "ft";
		 cout << "\tside = " << (target->IsLeft() ? "LEFT" : "RIGHT") << ", hot = " << (target->IsHot() ? "HOT" : "NOT") << endl;
		 }
		 else {
		 cout << "No Target Received..." << endl;
		 }
		 */

		//End Target Processing


		//Ball Collection

		/*
		 if(BallCollectPickupButton)
		 {
		 myCollection->Collect();
		 }
		 else
		 {
		 myCollection->ResetPosition();
		 }
		 */

		float spinSpeed = Config::GetSetting("intake_roller_speed", 1);
		int armPosition = 0;
		int rollerPosition = 0;

		switch (OperatorDPad) {
		case Gamepad::kCenter:
			armPosition = 0;
			rollerPosition = 0;
			break;
		case Gamepad::kUp:
			armPosition = -1;
			break;
		case Gamepad::kUpLeft:
			armPosition = -1;
			rollerPosition = -1;
			break;
		case Gamepad::kUpRight:
			armPosition = -1;
			rollerPosition = 1;
			break;
		case Gamepad::kDown:
			armPosition = 1;
			break;
		case Gamepad::kDownLeft:
			armPosition = 1;
			rollerPosition = -1;
			break;
		case Gamepad::kDownRight:
			armPosition = 1;
			rollerPosition = 1;
			break;
		case Gamepad::kLeft:
			rollerPosition = -1;
			break;
		case Gamepad::kRight:
			rollerPosition = 1;
			break;
		}

		if (DriverGamepad->GetLeftTrigger()) {
			armPosition = 0;
			rollerPosition = -1;
		}
		
		if(DriverGamepad->GetB())
		{
			armPosition = -1;
			rollerPosition = 1;
		}

		spinSpeed *= rollerPosition;
		myCollection->SpinMotor(spinSpeed);

		if (armPosition > 0)
			myCollection->ExtendArm();
		else if (armPosition < 0)
			myCollection->RetractArm();
		//End Ball Collection

		//Fire Control

#ifndef DISABLE_SHOOTER
		myCam->Process(ShouldFireButton, (OperatorGamepad->GetX() || DriverGamepad->GetX() ), DriverGamepad->GetB());
		//myCam->Debug(cout);
#endif //Ends DISABLE_SHOOTER
		
		//End Fire Control


		//Ring Light
		if (RingLightButton) {
			RingLightButtonRecentlyPressed = true;
		}

		if (!RingLightButton && RingLightButtonRecentlyPressed) {
			if (myCamera->Get() == Relay::kOff)
				myCamera->Set(Relay::kForward);
			else
				myCamera->Set(Relay::kOff);

			RingLightButtonRecentlyPressed = false;
		}
		//End Ring Light


		//Drive Processing
		if(DriverDPad == Gamepad::kUp)
		{
			cout << "Forward" << endl;
			myDrive->reverseDirectionForward();
		}
		else if(DriverDPad == Gamepad::kDown)
		{
			cout << "Backward"<<endl;
			myDrive->reverseDirectionReverse();
		}
		
		if (DriverLeftBumper) {
			myDrive->ShiftUp();
		} else if (DriverRightBumper) {
			myDrive->ShiftDown();
		}

		//myDrive->Drive(DriverLeftY, DriverRightY);
		myDrive->DriveArcade(DriverGamepad->GetRightX(), DriverGamepad->GetRightY());
		myDrive->Debug(cout);
		//End Drive Processing


		EndOfCycleMaintenance();
	}
Exemplo n.º 19
0
	/*
	 * resets encoders, step, navX, and starts the spike relay
	 */
	void TeleopInit(){
		Drive->TeleopInit();
		autonomous->reset();
	}