C++ Drive::TeleopDrive方法代码示例

/**
 * 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 RobotDemo::TeleopPeriodic()
{
	drive->TeleopDrive(driverPad->GetLeftY() *DRIVER_LEFT_DIRECTION, driverPad->GetRightY() *DRIVER_RIGHT_DIRECTION);
	shooter->handle();
	shooter->debug();
	
	//shooter->setShooterPower(operatorPad->GetRightY());
	
	if((intakeMode == IN_INTAKE_MANUAL_MODE) && operatorPad->GetNumberedButton(INTAKE_AUTO_MODE_BUTTON))
	{
		intakeMode = IN_INTAKE_AUTO_MODE;
		printf ("set into auto roller mode");
		intake->setExtenderMode(IN_INTAKE_AUTO_MODE);
	}
	//sets intake mode to auto if previously in manual and operator presses auto button
	else if((intakeMode == IN_INTAKE_AUTO_MODE) && operatorPad->GetNumberedButton(INTAKE_MANUAL_MODE_BUTTON))
	{
		intakeMode = IN_INTAKE_MANUAL_MODE;
		printf ("set into manual roller mode");
		intake->setExtenderMode(IN_INTAKE_MANUAL_MODE);
	}
	
	intake->handle();
	
	if(intakeMode == IN_INTAKE_MANUAL_MODE)
	{
		intake->move(operatorPad->GetLeftY()*OPERATOR_LEFT_DIRECTION);
	}
	else
	{
		//Extender DPad control
		curPadDir = operatorPad->GetDPad();
		
		if(lastPadDir != curPadDir)
		{
			switch(curPadDir)
			{
			case Gamepad::kUp:
				intake->setExtenderTarget(FULL_EXTEND_DISTANCE);
				break;
			case Gamepad::kDown:
				intake->setExtenderTarget(FULL_RETRACT_DISTANCE);
				break;
			default:
				break;
			}
		}
		
		lastPadDir = curPadDir;
	}
	
	//intake->spinRoller(operatorPad->GetRightY()*OPERATOR_RIGHT_DIRECTION);
	intake->getCurrentPosition();
	
	
	//if operator taps in button roller continually spins inward
	if(operatorPad->GetNumberedButton(ROLLER_IN_BUTTON))
	{
		intakeCont = INTAKE_ROLLER_IN_STATE;
	}
	//if operator taps stop button roller stops continually
	else if(operatorPad->GetNumberedButton(ROLLER_OFF_BUTTON))
	{
		intakeCont = INTAKE_ROLLER_STOP_STATE;
	}
	
	//if operator holds spit button roller spins backwards no matter what
	if(operatorPad->GetNumberedButton(ROLLER_OUT_BUTTON))
	{
		intake->spinRoller(INTAKE_ROLLER_SPIT_POWER);
	}
	//not holding spit button
	else
	{
		//roller either spins or is stopped depending on previous code
		if(intakeCont)
		{
			intake->spinRoller(INTAKE_ROLLER_IN_POWER);
		}
		else
		{
			intake->spinRoller(INTAKE_ROLLER_STOP_POWER);
		}
	}
		

	
	if(operatorPad->GetNumberedButton(RIGHT_TRIGGER_BUTTON)) // low shot = Right Trigger
	{
		shooter->initShot(SHOT_LOW_ANGLE, SHOT_LOW_SPEED);
		printf("test button 1 is pressed");
	}
	else if(operatorPad->GetNumberedButton(LEFT_TRIGGER_BUTTON)) // pass = Left Trigger
	{
//.........这里部分代码省略.........

/**
 * 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 RobotDemo::AutonomousPeriodic() 
{
	shooter->handle();
	intake->handle();
	
	switch(curAutoState)
	{
	case AUTO_EXTEND:
		if(curAutoState != preAutoState)
		{			
			preAutoState = curAutoState;
			
			intake->move(INTAKE_EXTEND_POWER);
			printf("extending, pre %d, cur %d, \n", preAutoState, curAutoState);
		}
		
		if(intake->getCurrentPosition() >= EXTENDER_MAX_DISTANCE)
		{
			intake->move(STOP);
			curAutoState = AUTO_DRIVE;
			printf("pre %d, cur %d, stopped extender \n", preAutoState, curAutoState);
		}
		break;
	
	case AUTO_DRIVE:
		if(curAutoState != preAutoState)
		{
			preAutoState = curAutoState;
			
			drive->InitAutoDrive(AUTO_DRIVE_DISTANCE_RIGHT, 
					AUTO_DRIVE_DISTANCE_LEFT, 
					AUTO_DRIVE_SPEED);
			printf("We're moving! pre %d, cur %d, \n", preAutoState, curAutoState);
		}
/*		if(HotAutoSensor->Get())
		{
			HotAutoStarts = true;
		}
*/		
		if(drive->autoDrive() == true)
		{
			curAutoState = AUTO_STOP_DRIVING;
		}
		break;
		
	case AUTO_STOP_DRIVING:
		if(curAutoState != preAutoState)
		{
			preAutoState = curAutoState;
			printf("now stopping pre %d, cur %d, \n", preAutoState, curAutoState);
			
			drive->TeleopDrive(AUTO_STOPPING_SPEED_LEFT, AUTO_STOPPING_SPEED_RIGHT);
		}
		
		if(drive->getSpeed() > 0.01)
		{
			drive->TeleopDrive(AUTO_STOPPED_SPEED_LEFT, AUTO_STOPPED_SPEED_RIGHT);
//			if(HotAutoStarts || HotAutoTimer->Get() > 5.0)
//			{
			curAutoState = AUTO_SHOOT;
//			}
		}
		break;
		
	case AUTO_SHOOT:
		if(curAutoState != preAutoState)
		{
			preAutoState = curAutoState;
			
			printf("now shooting pre %d, cur %d, \n", preAutoState, curAutoState);
			shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED);
		}
		
		if(!shooter->isShooting())
		{
			curAutoState = AUTO_STOP;
		}
		break;
/*		
	case AUTO_MOBILITY:
		if(curAutoState != preAutoState)
		{
			drive->InitAutoDrive(AUTO_SETUP_SHOT_DISTANCE_RIGHT, 
					AUTO_SETUP_SHOT_DISTANCE_LEFT, 
					AUTO_SETUP_SHOT_SPEED);
		}
		
		if(drive->autoDrive())
		{
			curAutoState = AUTO_STOP;
		}
		break;
*/		
	case AUTO_STOP:
//.........这里部分代码省略.........