Delta2018/Robot.cpp at master · Team5593/Delta2018 · 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
#include "Robot.h"

#include <Commands/Scheduler.h>
#include <SmartDashboard/SendableChooser.h>
#include <CameraServer.h>
#include <GyroBase.h>
#include <DriverStation.h>

#include "CommandGroups/AutoBasic.h"
#include "CommandGroups/AutoLobBox.h"
#include "CommandGroups/AutoTest.h"
#include "CommandGroups/AutoMiddle.h"
#include "Commands/MoveDistance.h"

#include "OI.h" // this is just here to fix eclipse semantic errors...

DriveTrain Robot::drivetrain;
Shooter Robot::shooter;
Grompers Robot::grompers;
Feeder Robot::feeder;
OI Robot::oi;

void Robot::RobotInit() {
	position_selector->AddDefault("Middle", Position::Middle);
	position_selector->AddObject("Left", Position::Left);
	position_selector->AddObject("Right", Position::Right);

	CameraServer::GetInstance()->StartAutomaticCapture("Front Camera" , 0);

	drivetrain.GetGyro().Calibrate();
}

void Robot::RobotPeriodic() {

}

void Robot::AutonomousInit() {
	Command* auto_command;
	Position position = (Position) position_selector->GetSelected();

	if (position == Position::Middle) {
		// Go around the switch to the right.
		auto_command = new AutoMiddle;
	}
	else if (position == GetSwitchSide()) {
		// Put powercube in the switch
		auto_command = new AutoLobBox;
	}
	else { // switch_side != robot_position
		// Robot is on the wrong side, just cross the base line.
		auto_command = new AutoBasic;
	}

	frc::Scheduler::GetInstance()->AddCommand(auto_command);
}

void Robot::AutonomousPeriodic() {
	frc::Scheduler::GetInstance()->Run();
}

void Robot::TeleopInit() {
	frc::Scheduler::GetInstance()->RemoveAll();
}

void Robot::TeleopPeriodic() {
	frc::Scheduler::GetInstance()->Run();
}

void Robot::TestInit() {}
void Robot::TestPeriodic() {}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}

Robot::Position* Robot::GetGameData() {
	static Position sides[3];

	std::string gameData = frc::DriverStation::GetInstance().GetGameSpecificMessage();

	for (int i = 0; i < 3; i++) {
		sides[i] = Position(gameData[i]);
	}

	return sides;
}

Robot::Position Robot::GetSwitchSide() {
	Position* sides = Robot::GetGameData();
	return sides[Robot::Plate::SwitchClose];
}

START_ROBOT_CLASS(Robot)