/
RobotBase.cpp
229 lines (203 loc) · 6.63 KB
/
RobotBase.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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "RobotBase.h"
#include "DriverStation.h"
#include "NetworkCommunication/FRCComm.h"
#include "NetworkCommunication/symModuleLink.h"
#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include <moduleLib.h>
#include <taskLib.h>
#include <unldLib.h>
#include <cstring>
RobotBase* RobotBase::m_instance = NULL;
const char *FILE_NAME = "/c/FRC_Lib_Version.ini";
const char *VERSION_STRING = "C++ 2014 Update 0";
void RobotBase::setInstance(RobotBase* robot)
{
wpi_assert(m_instance == NULL);
m_instance = robot;
}
RobotBase &RobotBase::getInstance()
{
return *m_instance;
}
/**
* Constructor for a generic robot program.
* User code should be placed in the constuctor that runs before the Autonomous or Operator
* Control period starts. The constructor will run to completion before Autonomous is entered.
*
* This must be used to ensure that the communications code starts. In the future it would be
* nice to put this code into it's own task that loads on boot so ensure that it runs.
*/
RobotBase::RobotBase()
: m_task (NULL)
, m_ds (NULL)
{
m_ds = DriverStation::GetInstance();
}
/**
* Free the resources for a RobotBase class.
* This includes deleting all classes that might have been allocated as Singletons to they
* would never be deleted except here.
*/
RobotBase::~RobotBase()
{
SensorBase::DeleteSingletons();
delete m_task;
m_task = NULL;
m_instance = NULL;
}
/**
* Check on the overall status of the system.
*
* @return Is the system active (i.e. PWM motor outputs, etc. enabled)?
*/
bool RobotBase::IsSystemActive()
{
return m_watchdog.IsSystemActive();
}
/**
* Return the instance of the Watchdog timer.
* Get the watchdog timer so the user program can either disable it or feed it when
* necessary.
*/
Watchdog &RobotBase::GetWatchdog()
{
return m_watchdog;
}
/**
* Determine if the Robot is currently enabled.
* @return True if the Robot is currently enabled by the field controls.
*/
bool RobotBase::IsEnabled()
{
return m_ds->IsEnabled();
}
/**
* Determine if the Robot is currently disabled.
* @return True if the Robot is currently disabled by the field controls.
*/
bool RobotBase::IsDisabled()
{
return m_ds->IsDisabled();
}
/**
* Determine if the robot is currently in Autnomous mode.
* @return True if the robot is currently operating Autonomously as determined by the field controls.
*/
bool RobotBase::IsAutonomous()
{
return m_ds->IsAutonomous();
}
/**
* Determine if the robot is currently in Operator Control mode.
* @return True if the robot is currently operating in Tele-Op mode as determined by the field controls.
*/
bool RobotBase::IsOperatorControl()
{
return m_ds->IsOperatorControl();
}
/**
* Determine if the robot is currently in Test mode.
* @return True if the robot is currently running tests as determined by the field controls.
*/
bool RobotBase::IsTest()
{
return m_ds->IsTest();
}
/**
* Indicates if new data is available from the driver station.
* @return Has new data arrived over the network since the last time this function was called?
*/
bool RobotBase::IsNewDataAvailable()
{
return m_ds->IsNewControlData();
}
/**
* Static interface that will start the competition in the new task.
*/
void RobotBase::robotTask(FUNCPTR factory, Task *task)
{
RobotBase::setInstance((RobotBase*)factory());
RobotBase::getInstance().m_task = task;
RobotBase::getInstance().StartCompetition();
}
void RobotBase::WriteVersionString() {
FILE *file = fopen(FILE_NAME, "w");
if (file != NULL) {
fputs(VERSION_STRING, file);
fclose(file);
}
}
/**
*
* Start the robot code.
* This function starts the robot code running by spawning a task. Currently tasks seemed to be
* started by LVRT without setting the VX_FP_TASK flag so floating point context is not saved on
* interrupts. Therefore the program experiences hard to debug and unpredictable results. So the
* LVRT code starts this function, and it, in turn, starts the actual user program.
*/
void RobotBase::startRobotTask(FUNCPTR factory)
{
#ifdef SVN_REV
if (strlen(SVN_REV))
{
printf("WPILib was compiled from SVN revision %s\n", SVN_REV);
}
else
{
printf("WPILib was compiled from a location that is not source controlled.\n");
}
#else
printf("WPILib was compiled without -D'SVN_REV=nnnn'\n");
#endif
// Check for startup code already running
int32_t oldId = taskNameToId(const_cast<char*>("FRC_RobotTask"));
if (oldId != ERROR)
{
// Find the startup code module.
char moduleName[256];
moduleNameFindBySymbolName("FRC_UserProgram_StartupLibraryInit", moduleName);
MODULE_ID startupModId = moduleFindByName(moduleName);
if (startupModId != NULL)
{
// Remove the startup code.
unldByModuleId(startupModId, 0);
printf("!!! Error: Default code was still running... It was unloaded for you... Please try again.\n");
return;
}
// This case should no longer get hit.
printf("!!! Error: Other robot code is still running... Unload it and then try again.\n");
return;
}
// Let the framework know that we are starting a new user program so the Driver Station can disable.
FRC_NetworkCommunication_observeUserProgramStarting();
// Let the Usage Reporting framework know that there is a C++ program running
nUsageReporting::report(nUsageReporting::kResourceType_Language, nUsageReporting::kLanguage_CPlusPlus);
RobotBase::WriteVersionString();
// Start robot task
// This is done to ensure that the C++ robot task is spawned with the floating point
// context save parameter.
Task *task = new Task("RobotTask", (FUNCPTR)RobotBase::robotTask, Task::kDefaultPriority, 64000);
task->Start((int32_t)factory, (int32_t)task);
}
/**
* This class exists for the sole purpose of getting its destructor called when the module unloads.
* Before the module is done unloading, we need to delete the RobotBase derived singleton. This should delete
* the other remaining singletons that were registered. This should also stop all tasks that are using
* the Task class.
*/
class RobotDeleter
{
public:
RobotDeleter() {}
~RobotDeleter()
{
delete &RobotBase::getInstance();
}
};
static RobotDeleter g_robotDeleter;