Quasics2016Code/Mettaton/src/Commands/TankDrive.cpp
// Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
void TankDrive::Execute() {
#define USE_SLIDER_FOR_TRIM
#ifdef USE_SLIDER_FOR_TRIM
    if (SmartDashboard::GetNumber("Left Trim", 0) > .5) {
        SmartDashboard::PutNumber("Left Trim", .5);
    }
    else if (SmartDashboard::GetNumber("Left Trim", 0) < -.5) {
        SmartDashboard::PutNumber("Left Trim", -.5);
    }
    if (SmartDashboard::GetNumber("Right Trim", 0) > .5) {
        SmartDashboard::PutNumber("Right Trim", .5);
    }
    else if (SmartDashboard::GetNumber("Right Trim", 0) < -.5) {
        SmartDashboard::PutNumber("Right Trim", -.5);
    }
    const double leftSliderValue = SmartDashboard::GetNumber("Left Trim", 0)
                                   + 1;
    const double rightSliderValue = SmartDashboard::GetNumber("Right Trim", 0)
                                    + 1;
    SpeedMode mode = eSlow;
    if ((Robot::oi->getPilotStick()->GetRawButton(5)
            || Robot::oi->getPilotStick()->GetRawButton(6))
            && !(Robot::oi->getPilotStick()->GetRawButton(7)
                 || Robot::oi->getPilotStick()->GetRawButton(8))) {
        mode = eSlow;
    }
    else if (!(Robot::oi->getPilotStick()->GetRawButton(5)
               || Robot::oi->getPilotStick()->GetRawButton(6))
             && (Robot::oi->getPilotStick()->GetRawButton(7)
                 || Robot::oi->getPilotStick()->GetRawButton(8))) {
        mode = eFast;
    }
    else {
        mode = eMedium;
    }
    const double leftFactor = ScalingFactors[int(mode)][0];
    const double rightFactor = ScalingFactors[int(mode)][1];
    float leftStick = 0;
    float rightStick = 0;
    if (fabs(Robot::oi->getPilotStick()->GetRawAxis(1)) >= .05) {
        leftStick = Robot::oi->getPilotStick()->GetRawAxis(1);
    }
    else {
        leftStick = 0;
    }
    if (fabs(Robot::oi->getPilotStick()->GetRawAxis(3)) >= .05) {
        rightStick = Robot::oi->getPilotStick()->GetRawAxis(3);
    }
    else {
        rightStick = 0;
    }
    Robot::driveBase->SetLeftPower(-leftStick * leftSliderValue
                                 * leftFactor);
    Robot::driveBase->SetRightPower(-rightStick * rightSliderValue
                                  * rightFactor);
#else
    SpeedMode mode = eSlow;
    if ((Robot::oi->getPilotStick()->GetRawButton(5)
            || Robot::oi->getPilotStick()->GetRawButton(6))
            && !(Robot::oi->getPilotStick()->GetRawButton(7)
                 || Robot::oi->getPilotStick()->GetRawButton(8))) {
        mode = eSlow;
    }
    else if (!(Robot::oi->getPilotStick()->GetRawButton(5)
               || Robot::oi->getPilotStick()->GetRawButton(6))
             && (Robot::oi->getPilotStick()->GetRawButton(7)
                 || Robot::oi->getPilotStick()->GetRawButton(8))) {
        mode = eFast;
    }
    else {
        mode = eMedium;
    }
    float leftStick = 0;
    float rightStick = 0;
    if (fabs(Robot::oi->getPilotStick()->GetRawAxis(1)) >= .05) {
        leftStick = Robot::oi->getPilotStick()->GetRawAxis(1);
    }
    else {
        leftStick = 0;
    }
    if (fabs(Robot::oi->getPilotStick()->GetRawAxis(3)) >= .05) {
        rightStick = Robot::oi->getPilotStick()->GetRawAxis(3);
    }
    else {
        rightStick = 0;
    }
    const double leftFactor = ScalingFactors[int(mode)][0];
    const double rightFactor = ScalingFactors[int(mode)][1];
    Robot::driveBase->SetLeftPower(
        -leftStick * leftFactor);
    Robot::driveBase->SetRightPower(
        -rightStick * rightFactor);
#endif
}
void TankDrive :: Execute ( ) { #define USE_SLIDER_FOR_TRIM #ifdef USE_SLIDER_FOR_TRIM if ( SmartDashboard :: GetNumber ( " " , 0 ) > .5 ) { SmartDashboard :: PutNumber ( " " , .5 ) ; } else if ( SmartDashboard :: GetNumber ( " " , 0 ) < -.5 ) { SmartDashboard :: PutNumber ( " " , -.5 ) ; } if ( SmartDashboard :: GetNumber ( " " , 0 ) > .5 ) { SmartDashboard :: PutNumber ( " " , .5 ) ; } else if ( SmartDashboard :: GetNumber ( " " , 0 ) < -.5 ) { SmartDashboard :: PutNumber ( " " , -.5 ) ; } const double leftSliderValue = SmartDashboard :: GetNumber ( " " , 0 ) + 1 ; const double rightSliderValue = SmartDashboard :: GetNumber ( " " , 0 ) + 1 ; SpeedMode mode = eSlow ; if ( ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 5 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 6 ) ) && ! ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 7 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 8 ) ) ) { mode = eSlow ; } else if ( ! ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 5 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 6 ) ) && ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 7 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 8 ) ) ) { mode = eFast ; } else { mode = eMedium ; } const double leftFactor = ScalingFactors [ int ( mode ) ] [ 0 ] ; const double rightFactor = ScalingFactors [ int ( mode ) ] [ 1 ] ; float leftStick = 0 ; float rightStick = 0 ; if ( fabs ( Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 1 ) ) >= .05 ) { leftStick = Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 1 ) ; } else { leftStick = 0 ; } if ( fabs ( Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 3 ) ) >= .05 ) { rightStick = Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 3 ) ; } else { rightStick = 0 ; } Robot :: driveBase -> SetLeftPower ( - leftStick * leftSliderValue * leftFactor ) ; Robot :: driveBase -> SetRightPower ( - rightStick * rightSliderValue * rightFactor ) ; #else SpeedMode mode = eSlow ; if ( ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 5 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 6 ) ) && ! ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 7 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 8 ) ) ) { mode = eSlow ; } else if ( ! ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 5 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 6 ) ) && ( Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 7 ) || Robot :: oi -> getPilotStick ( ) -> GetRawButton ( 8 ) ) ) { mode = eFast ; } else { mode = eMedium ; } float leftStick = 0 ; float rightStick = 0 ; if ( fabs ( Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 1 ) ) >= .05 ) { leftStick = Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 1 ) ; } else { leftStick = 0 ; } if ( fabs ( Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 3 ) ) >= .05 ) { rightStick = Robot :: oi -> getPilotStick ( ) -> GetRawAxis ( 3 ) ; } else { rightStick = 0 ; } const double leftFactor = ScalingFactors [ int ( mode ) ] [ 0 ] ; const double rightFactor = ScalingFactors [ int ( mode ) ] [ 1 ] ; Robot :: driveBase -> SetLeftPower ( - leftStick * leftFactor ) ; Robot :: driveBase -> SetRightPower ( - rightStick * rightFactor ) ; #endif }
Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
// Figure out what mode the driver wants // Set the drive speed, based on joystick position and speed mode.