// Drive forward at maxSpeed for a specific amount of time
void Robot::DriveFor(double seconds, double speed)
{
RobotTimer.Reset();
RobotTimer.Start();
while(!RobotTimer.HasPeriodPassed(seconds))
{
ArcadeDrive(speed, 0);
}
ArcadeDrive(0, 0);
RobotTimer.Stop();
}
void Robot::AutonomousInit()
{
ArcadeDrive(0, 0);
GearSlide.Set(0);
ClimbMotor.Set(0);
Gyro.Reset();
if(SmartDashboard::GetBoolean("DB/Button 0", false))
{
SmartDashboard::PutString("Auto Status", "Scoring on the left peg");
DriveFor(1.0);
TurnRobot(60);
DriveFor(0.4);
}
//Test Code
else if(SmartDashboard::GetBoolean("DB/Button 4", false))
{
TurnRobot(-60);
}
// Score in center peg
else if(SmartDashboard::GetBoolean("DB/Button 1", false))
{
SmartDashboard::PutString("Auto Status", "Scoring on the center peg");
DriveFor(0.75);
}
// Score in right peg
else if(SmartDashboard::GetBoolean("DB/Button 2", false))
{
SmartDashboard::PutString("Auto Status", "Scoring on the right peg");
DriveFor(1.9);
TurnRobot(-60);
DriveFor(1.5);
}
// Cross baseline
else if(SmartDashboard::GetBoolean("DB/Button 3", false))
{
SmartDashboard::PutString("Auto Status", "Crossing the Baseline");
DriveFor(1.9, 0.4);
}
else
{
ArcadeDrive(0, 0);
GearSlide.Set(0);
ClimbMotor.Set(0);
}
ArcadeDrive(0, 0);
GearSlide.Set(0);
ClimbMotor.Set(0);
SmartDashboard::PutString("Auto Status", "Autonomous completed");
}
void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis,
GenericHID &rotateStick, uint32_t rotateAxis,
bool squaredInputs) {
float moveValue = moveStick.GetRawAxis(moveAxis);
float rotateValue = rotateStick.GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving.
* Given two joystick instances and two axis, compute the values to send to either two
* or four motors.
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
* @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
*/
void RobotDrive::ArcadeDrive(GenericHID* moveStick, UINT32 moveAxis,
GenericHID* rotateStick, UINT32 rotateAxis,
bool squaredInputs)
{
float moveValue = moveStick->GetRawAxis(moveAxis);
float rotateValue = rotateStick->GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
void OperatorControl(void)
{
SetWatchdogEnabled(true);
while (IsOperatorControl())
{
WatchdogFeed();
ArcadeDrive(JOYSTICK_PORT);
Wait(0.005);
}
}
void Robot::DisabledInit()
{
//Init motors/gear
ArcadeDrive(0, 0);
GearSlide.Set(0);
ClimbMotor.Set(0);
SmartDashboard::PutString("DB/String 0", "Left Peg");
SmartDashboard::PutString("DB/String 4", "Test");
SmartDashboard::PutString("DB/String 1", "Center Peg");
SmartDashboard::PutString("DB/String 2", "Right Peg");
SmartDashboard::PutString("DB/String 3", "Cross Baseline");
}
/**
* Arcade drive implements single stick driving.
* Given a single Joystick, the class assumes the Y axis for the move value and the X axis
* for the rotate value.
* (Should add more information here regarding the way that arcade drive works.)
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be increased for small values
*/
void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs)
{
// simply call the full-featured ArcadeDrive with the appropriate values
ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
}
void Robot::AutonomousPeriodic()
{
ArcadeDrive(0, 0);
GearSlide.Set(0);
ClimbMotor.Set(0);
}
/**
* \todo IMPLEMENT
*/
void PidSimpleDrive::ArcadeDrive(float moveValue, float rotateValue) {
TryToggling(Rate);
ArcadeDrive(moveValue, rotateValue, false);
}
