-
Notifications
You must be signed in to change notification settings - Fork 0
/
RhsRobot.cpp
113 lines (93 loc) · 2.83 KB
/
RhsRobot.cpp
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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
/** \file
* Main robot class.
*
* The RhsRobot class is the main robot class. It inherits from RhsRobotBase and MUST define the Init() function, the Run() function, and
* the OnStateChange() function. Messages from the DS are processed and commands sent to the subsystems
* that implement behaviors for each part for the robot.
*/
#include "RhsRobot.h"
#include "WPILib.h"
//Robot
#include "ComponentBase.h"
#include "RobotParams.h"
RhsRobot::RhsRobot() {
Controller_1 = NULL;
Monitor_1 = NULL;
drivetrain = NULL;
autonomous = NULL;
iLoop = 0;
}
RhsRobot::~RhsRobot() {
std::vector<ComponentBase *>::iterator nextComponent = ComponentSet.begin();
for(; nextComponent != ComponentSet.end(); ++nextComponent)
{
delete (*nextComponent);
}
delete Controller_1;
delete Monitor_1;
}
void RhsRobot::Init() {
/*
* Set all pointers to null and then allocate memory and construct objects
* EXAMPLE: drivetrain = NULL; (in constructor)
* drivetrain = new Drivetrain(); (in RhsRobot::Init())
*/
Controller_1 = new Joystick(0);
Monitor_1 = new JoystickMonitor(Controller_1);
Monitor_1->SetAxisTolerance(.05);
drivetrain = new Drivetrain();
autonomous = new Autonomous();
std::vector<ComponentBase *>::iterator nextComponent = ComponentSet.begin();
if(drivetrain)
{
nextComponent = ComponentSet.insert(nextComponent, drivetrain);
}
if(autonomous)
{
nextComponent = ComponentSet.insert(nextComponent, autonomous);
}
}
void RhsRobot::OnStateChange() {
std::vector<ComponentBase *>::iterator nextComponent;
for(nextComponent = ComponentSet.begin();
nextComponent != ComponentSet.end(); ++nextComponent)
{
(*nextComponent)->SendMessage(&robotMessage);
}
}
void RhsRobot::Run() {
/* Poll for control data and send messages to each subsystem. Surround blocks with if(component) so entire components can be disabled
* by commenting out their construction.
* EXAMPLE: if(drivetrain)
* {
* //Check joysticks and send messages
* }
*/
if(autonomous)
{
if(GetCurrentRobotState() == ROBOT_STATE_AUTONOMOUS)
{
// all messages to components will come from the autonomous task
return;
}
}
if (drivetrain)
{
//for keepalign tests: comment out everything to the sendMessage
//also comment out the if (ISAUTO) at the bottom of Drivetrain::Run()
//robotMessage.command = COMMAND_DRIVETRAIN_START_KEEPALIGN;
#if 1
robotMessage.command = COMMAND_DRIVETRAIN_DRIVE_TANK;
robotMessage.params.tankDrive.left = TANK_DRIVE_LEFT * fDriveMax;
robotMessage.params.tankDrive.right = TANK_DRIVE_RIGHT * fDriveMax;
#else
robotMessage.command = COMMAND_DRIVETRAIN_DRIVE_ARCADE;
robotMessage.params.arcadeDrive.x = ARCADE_DRIVE_X;
robotMessage.params.arcadeDrive.y = ARCADE_DRIVE_Y;
#endif
drivetrain->SendMessage(&robotMessage);
}
Monitor_1->FinalUpdate();
iLoop++;
}
START_ROBOT_CLASS(RhsRobot)