Rambot2012/CommandBasedRobot.cpp at master · MMRambotics/Rambot2012 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
#include "WPILib.h"
#include "Commands/Command.h"
#include "CommandBase.h"
#include "Commands/ArcadeDrive.h"
#include "Commands/TankDrive.h"
#include "Commands/KinectTankDrive.h"
#include "Commands/DeployRamp.h"
#include "Commands/UndeployRamp.h"

class CommandBasedRobot : public IterativeRobot {
private:
    Compressor *compressor;
    Command *driveCommand;
    SendableChooser *driveStyle;

    bool previousShiftTrigger;
    bool currentShiftTrigger;
    bool previousRampTrigger;
    bool currentRampTrigger;

public:
    CommandBasedRobot() {
        compressor = new Compressor(PRESSURE_SWITCH_PORT, COMPRESSOR_RELAY_PORT);
        compressor->Start();

        driveStyle = new SendableChooser();
        driveStyle->AddDefault("Arcade", new ArcadeDrive());
        driveStyle->AddObject("Tank", new TankDrive());

        CommandBase::init();
    }

    virtual void RobotInit() {
        NetworkTable::Initialize();
    }

    virtual void AutonomousInit() {

    }

    virtual void AutonomousPeriodic() {
    }

    virtual void TeleopInit() {
        CommandBase::turret->Reset();
        CommandBase::turret->Start();

        driveCommand = (Command*) driveStyle->GetSelected();
        driveCommand->Start();
    }

    virtual void TeleopPeriodic() {
        // Shift Gears
        currentShiftTrigger = CommandBase::oi->GetLeftStick()->GetRawButton(1);
        if (currentShiftTrigger !=  previousShiftTrigger && currentShiftTrigger == false) {
            CommandBase::drive->SwitchGear();
        }
        previousShiftTrigger = currentShiftTrigger;

        // Operate Conveyor
        if (CommandBase::oi->GetGamePad()->GetDPadY() > 0.0) {
            CommandBase::conveyor->ConveyorUp();
        } else if (CommandBase::oi->GetGamePad()->GetDPadY() < 0.0) {
            CommandBase::conveyor->ConveyorDown();
        } else {
            CommandBase::conveyor->ConveyorStop();
        }

        // Operate Roller
        if (CommandBase::oi->GetGamePad()->GetButton(F310::kAButton)) {
            CommandBase::roller->rollerForward();
        } else if (CommandBase::oi->GetGamePad()->GetButton(F310::kYButton)) {
            CommandBase::roller->rollerBackward();
        } else {
            CommandBase::roller->rollerStop();
        }

        // Control turret Pan
        CommandBase::turret->Pan(-CommandBase::oi->GetGamePad()->GetX(F310::kLeftStick));

        // Print gear state
        if (CommandBase::drive->GetState() == Drive::kHighGear) {
            SmartDashboard::GetInstance()->Log("High Gear", "Gear State");
        } else {
            SmartDashboard::GetInstance()->Log("Low Gear", "Gear State");
        }

        // Print ramp state
        if (CommandBase::giantFour->GetState() == GiantFour::kRampUp) {
            SmartDashboard::GetInstance()->Log("Ramp Up", "Ramp State");
        } else {
            SmartDashboard::GetInstance()->Log("Ramp Down", "Ramp State");
        }

        Scheduler::GetInstance()->Run();
    }
};

START_ROBOT_CLASS(CommandBasedRobot);