Quasics2016Code/SimpleRobot/src/Commands/SpyBoxAutonmous.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 SpyBoxAutonmous::Execute() {
#ifndef USE_GHETTO_CODE
    static int endstepTimer = 0;
    if (!isEndstep) {
        switch (autoStage) {
            case 0:
                Robot::driveSystem->ResetEncoders();
                Robot::driveSystem->ResetYaw();
                autoStage = 1;
                break;
            case 1:
                if (Robot::driveSystem->GetContinuousYaw() > 2.5) {
                    Robot::driveSystem->MoveLeft(50);
                    Robot::driveSystem->MoveRight(45);
                }
                else if (Robot::driveSystem->GetContinuousYaw() < -2.5) {
                    Robot::driveSystem->MoveLeft(45);
                    Robot::driveSystem->MoveRight(50);
                }
                else {
                    Robot::driveSystem->MoveLeft(50);
                    Robot::driveSystem->MoveRight(50);
                }
                if (Robot::driveSystem->GetEncoderDistance(DriveSystem::kLeft)
                        >= 1.45) {
                    Robot::driveSystem->ResetEncoders();
                    Robot::driveSystem->ResetYaw();
                    autoStage = 4;
                }
                break;
            case 2:
                Robot::driveSystem->MoveLeft(-50);
                Robot::driveSystem->MoveRight(50);
                if (Robot::driveSystem->GetContinuousYaw() <= 75) {
                    Robot::driveSystem->ResetEncoders();
                    Robot::driveSystem->ResetYaw();
                    autoStage = 5;
                }
                break;
            case 3:
                if (Robot::driveSystem->GetContinuousYaw() > 2.5) {
                    Robot::driveSystem->MoveLeft(50);
                    Robot::driveSystem->MoveRight(45);
                }
                else if (Robot::driveSystem->GetContinuousYaw() < -2.5) {
                    Robot::driveSystem->MoveLeft(45);
                    Robot::driveSystem->MoveRight(50);
                }
                else {
                    Robot::driveSystem->MoveLeft(50);
                    Robot::driveSystem->MoveRight(50);
                }
                if (Robot::driveSystem->GetEncoderDistance(DriveSystem::kLeft)
                        >= 1.80) {
                    Robot::driveSystem->ResetEncoders();
                    Robot::driveSystem->ResetYaw();
                    isEndstep = true;
                }
                break;
        }
    }
    else if (endstepTimer <= 50) {
        Robot::intakeArm->SetArmDirection(IntakeArm::kFalling);
        Robot::driveSystem->StopEverything();
        endstepTimer++;
    }
    else {
        Robot::intake->SetPower(Intake::kOutput);
        Robot::driveSystem->StopEverything();
    }
#else
    if (autoStage <= 200) {
        Robot::driveSystem->MoveLeft(.6);
        Robot::driveSystem->MoveRight(.6);
        autoStage++;
    }
    else {
        Robot::driveSystem->StopEverything();
    }
#endif
}
void SpyBoxAutonmous :: Execute ( ) { #ifndef USE_GHETTO_CODE static int endstepTimer = 0 ; if ( ! isEndstep ) { switch ( autoStage ) { case 0 : Robot :: driveSystem -> ResetEncoders ( ) ; Robot :: driveSystem -> ResetYaw ( ) ; autoStage = 1 ; break ; case 1 : if ( Robot :: driveSystem -> GetContinuousYaw ( ) > 2.5 ) { Robot :: driveSystem -> MoveLeft ( 50 ) ; Robot :: driveSystem -> MoveRight ( 45 ) ; } else if ( Robot :: driveSystem -> GetContinuousYaw ( ) < -2.5 ) { Robot :: driveSystem -> MoveLeft ( 45 ) ; Robot :: driveSystem -> MoveRight ( 50 ) ; } else { Robot :: driveSystem -> MoveLeft ( 50 ) ; Robot :: driveSystem -> MoveRight ( 50 ) ; } if ( Robot :: driveSystem -> GetEncoderDistance ( DriveSystem :: kLeft ) >= 1.45 ) { Robot :: driveSystem -> ResetEncoders ( ) ; Robot :: driveSystem -> ResetYaw ( ) ; autoStage = 4 ; } break ; case 2 : Robot :: driveSystem -> MoveLeft ( -50 ) ; Robot :: driveSystem -> MoveRight ( 50 ) ; if ( Robot :: driveSystem -> GetContinuousYaw ( ) <= 75 ) { Robot :: driveSystem -> ResetEncoders ( ) ; Robot :: driveSystem -> ResetYaw ( ) ; autoStage = 5 ; } break ; case 3 : if ( Robot :: driveSystem -> GetContinuousYaw ( ) > 2.5 ) { Robot :: driveSystem -> MoveLeft ( 50 ) ; Robot :: driveSystem -> MoveRight ( 45 ) ; } else if ( Robot :: driveSystem -> GetContinuousYaw ( ) < -2.5 ) { Robot :: driveSystem -> MoveLeft ( 45 ) ; Robot :: driveSystem -> MoveRight ( 50 ) ; } else { Robot :: driveSystem -> MoveLeft ( 50 ) ; Robot :: driveSystem -> MoveRight ( 50 ) ; } if ( Robot :: driveSystem -> GetEncoderDistance ( DriveSystem :: kLeft ) >= 1.80 ) { Robot :: driveSystem -> ResetEncoders ( ) ; Robot :: driveSystem -> ResetYaw ( ) ; isEndstep = true ; } break ; } } else if ( endstepTimer <= 50 ) { Robot :: intakeArm -> SetArmDirection ( IntakeArm :: kFalling ) ; Robot :: driveSystem -> StopEverything ( ) ; endstepTimer ++ ; } else { Robot :: intake -> SetPower ( Intake :: kOutput ) ; Robot :: driveSystem -> StopEverything ( ) ; } #else if ( autoStage <= 200 ) { Robot :: driveSystem -> MoveLeft ( .6 ) ; Robot :: driveSystem -> MoveRight ( .6 ) ; autoStage ++ ; } else { Robot :: driveSystem -> StopEverything ( ) ; } #endif }
Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
//Motor Stuff //reset Sensors //Motor Stuff //reset Sensors