added button edge logging + cleaned up various naming and comments
diff --git a/aos/atom_code/input/joystick_input.cc b/aos/atom_code/input/joystick_input.cc
index 01bcbc6..3c66606 100644
--- a/aos/atom_code/input/joystick_input.cc
+++ b/aos/atom_code/input/joystick_input.cc
@@ -46,7 +46,23 @@
}
data.Update(joysticks);
- // TODO(brians): posedge/negedge logging
+
+ {
+ using driver_station::JoystickFeature;
+ using driver_station::ButtonLocation;
+ for (int joystick = 0; joystick < JoystickFeature::kJoysticks;
+ ++joystick) {
+ for (int button = 0; button < ButtonLocation::kButtons; ++button) {
+ ButtonLocation location(joystick, button);
+ if (data.PosEdge(location)) {
+ LOG(INFO, "PosEdge(%d, %d)\n", joystick, button);
+ }
+ if (data.NegEdge(location)) {
+ LOG(INFO, "NegEdge(%d, %d)\n", joystick, button);
+ }
+ }
+ }
+ }
RunIteration(data);
}
diff --git a/aos/atom_code/input/joystick_input.h b/aos/atom_code/input/joystick_input.h
index d905eaa..de0ed9e 100644
--- a/aos/atom_code/input/joystick_input.h
+++ b/aos/atom_code/input/joystick_input.h
@@ -6,11 +6,17 @@
namespace aos {
namespace input {
+// A class for handling joystick packet values.
+// It will call RunIteration each time a new packet is received.
+//
+// This class automatically handles updating ::aos::robot_state and logging (at
+// INFO) button edges.
class JoystickInput {
public:
void Run();
private:
+ // Subclasses should do whatever they want with data here.
virtual void RunIteration(const driver_station::Data &data) = 0;
};
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
index b24204b..2a92437 100644
--- a/aos/atom_code/output/motor_output.cc
+++ b/aos/atom_code/output/motor_output.cc
@@ -63,6 +63,8 @@
void MotorOutput::SetSolenoid(uint8_t channel, bool set) {
if (set) {
values_.solenoid_values |= 1 << (channel - 1);
+ } else {
+ values_.solenoid_values &= ~(1 << (channel - 1));
}
}
@@ -80,6 +82,8 @@
values_.digital_output_enables |= 1 << shift_amount;
if (value) {
values_.digital_output_values |= 1 << shift_amount;
+ } else {
+ values_.digital_output_values &= ~(1 << shift_amount);
}
}
diff --git a/aos/atom_code/output/motor_output.h b/aos/atom_code/output/motor_output.h
index 73648ce..8828f14 100644
--- a/aos/atom_code/output/motor_output.h
+++ b/aos/atom_code/output/motor_output.h
@@ -52,7 +52,7 @@
void DisablePWMOutput(uint8_t channel);
void SetDigitalOutput(uint8_t channel, bool value);
- // The struct that's going to get sent over.
+ // The data that's going to get sent over.
// Gets reset (everything set so that it won't do anything) each time through.
NetworkRobotMotors values_;
diff --git a/aos/common/input/driver_station_data.h b/aos/common/input/driver_station_data.h
index 41609ed..dca33f9 100644
--- a/aos/common/input/driver_station_data.h
+++ b/aos/common/input/driver_station_data.h
@@ -4,6 +4,8 @@
// This file defines several types to support nicely looking at the data
// received from the driver's station.
+#include <assert.h>
+
#include <memory>
#include "aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h"
@@ -19,6 +21,10 @@
JoystickFeature(int joystick, int number)
: joystick_(joystick), number_(number) {}
+ // How many joysticks there are.
+ static const int kJoysticks = sizeof(NetworkRobotJoysticks::joysticks) /
+ sizeof(NetworkRobotJoysticks::joysticks[0]);
+
// Which joystick number this is (1-based).
int joystick() const { return joystick_; }
// Which feature on joystick() this is (1-based).
@@ -35,6 +41,9 @@
public:
ButtonLocation(int joystick, int number)
: JoystickFeature(joystick, number) {}
+
+ // How many buttons there are available on each joystick.
+ static const int kButtons = 12;
};
// Represents various bits of control information that the DS sends.
@@ -50,6 +59,10 @@
public:
JoystickAxis(int joystick, int number)
: JoystickFeature(joystick, number) {}
+
+ // How many axes there are available on each joystick.
+ static const int kAxes = sizeof(NetworkRobotJoysticks::Joystick::axes) /
+ sizeof(NetworkRobotJoysticks::Joystick::axes[0]);
};
class Data {
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
index 4944596..61575da 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
@@ -14,7 +14,6 @@
#include "WPILib/Utility.h"
#include "WPILib/WPIErrors.h"
#include "WPILib/SensorBase.h"
-#include "WPILib/DriverStation.h"
#include "WPILib/Timer.h"
const double NetworkRobot::kDisableTime = 0.15;
@@ -27,6 +26,7 @@
joystick_values_(),
send_task_("DS_Send", reinterpret_cast<FUNCPTR>(StaticSendLoop)),
last_received_timestamp_(0.0),
+ last_sent_state_valid_(false),
digital_modules_(), solenoid_bases_(),
allocated_digital_outputs_() {
}
@@ -40,10 +40,10 @@
close(send_socket_);
}
- for (size_t i = 0;
- i < sizeof(solenoid_bases_) / sizeof(solenoid_bases_[0]);
- ++i) {
- delete solenoid_bases_[i];
+ for (size_t module = 0;
+ module < sizeof(solenoid_bases_) / sizeof(solenoid_bases_[0]);
+ ++module) {
+ delete solenoid_bases_[module];
}
for (size_t module = 0;
module < sizeof(digital_modules_) / sizeof(digital_modules_[0]);
@@ -100,6 +100,7 @@
}
if (sender_address_ != NULL) {
+ // We only need to do it in a separate task if we're doing both parts.
if (receiver_address_ != NULL) {
send_task_.Start(reinterpret_cast<uintptr_t>(this));
}
@@ -145,7 +146,7 @@
// as dangerous as leaving them on, so just leave them alone.
// We took care of it, so we don't want the watchdog to permanently disable
- // us.
+ // everything.
m_watchdog.Feed();
}
@@ -179,36 +180,42 @@
timeout.tv_sec = 0;
timeout.tv_usec = kDisableTime *
1000.0 /*seconds to mseconds*/ *
- 1000.0 /*mseconds to useconds*/;
+ 1000.0 /*mseconds to useconds*/ + 0.5;
fd_set fds;
FD_ZERO(&fds);
FD_SET(receive_socket_, &fds);
int ret = select(receive_socket_ + 1,
- &fds, // read fds
- NULL, // write fds
- NULL, // exception fds (not supported)
+ &fds, // read fds
+ NULL, // write fds
+ NULL, // exception fds (not supported)
&timeout);
if (ret == 0) {
// timeout
return false;
} else if (ret == 1) {
return true;
+ } else if (ret != -1) {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "select returned %d", ret);
+ wpi_setErrorWithContext(-1, buf);
+ return false;
+ } else {
+ wpi_setErrnoErrorWithContext("waiting until the socket has data");
+ return false;
}
- wpi_setErrnoErrorWithContext("waiting until the socket has data");
- return false;
}
void NetworkRobot::ReceivePacket() {
if (!WaitForData()) return;
+ char buffer[sizeof(motors_) + buffers::kOverhead];
union {
sockaddr addr;
sockaddr_in in;
} sender_address;
int sender_address_length = sizeof(sender_address);
- char buffer[sizeof(values_) + buffers::kOverhead];
int received = recvfrom(receive_socket_,
buffer,
sizeof(buffer),
@@ -224,8 +231,8 @@
wpi_setErrnoErrorWithContext("recvfrom on motor value socket");
return;
}
- wpi_assert(static_cast<size_t>(sender_address_length) >=
- sizeof(sender_address.in));
+ assert(static_cast<size_t>(sender_address_length) >=
+ sizeof(sender_address.in));
if (sender_address.in.sin_addr.s_addr !=
expected_sender_address_.s_addr) {
char address[INET_ADDR_LEN];
@@ -236,7 +243,7 @@
return;
}
- if (values_.DeserializeFrom(buffer, sizeof(buffer))) {
+ if (motors_.DeserializeFrom(buffer, sizeof(buffer))) {
ProcessPacket();
} else {
char buf[64];
@@ -248,12 +255,13 @@
}
void NetworkRobot::ProcessPacket() {
- int8_t digital_number = values_.digital_module;
+ int8_t digital_number = motors_.digital_module;
if (digital_number != -1) {
if (digital_number == 0) {
digital_number = SensorBase::GetDefaultDigitalModule();
}
- if (digital_number < 1 || digital_number > 2) {
+ if (digital_number < 1 ||
+ digital_number > static_cast<int32_t>(SensorBase::kDigitalModules)) {
char buf[64];
snprintf(buf, sizeof(buf), "Got Digital Module %d",
static_cast<int>(digital_number));
@@ -270,7 +278,7 @@
}
for (int i = 0; i < 10; ++i) {
- digital_module->SetPWM(i + 1, values_.pwm_outputs[i]);
+ digital_module->SetPWM(i + 1, motors_.pwm_outputs[i]);
}
uint16_t old_allocated = allocated_digital_outputs_[digital_number - 1];
@@ -279,35 +287,36 @@
for (int i = 0; i < 16; ++i) {
// If we have it allocated and this packet says we shouldn't.
if ((old_allocated & (1 << i)) &&
- !(values_.digital_output_enables & (1 << i))) {
+ !(motors_.digital_output_enables & (1 << i))) {
digital_module->FreeDIO(15 - i);
allocated_digital_outputs_[digital_number - 1] &= ~(1 << i);
// If this packet says we should have it allocated and we don't.
- } else if ((values_.digital_output_enables & (1 << i)) &&
+ } else if ((motors_.digital_output_enables & (1 << i)) &&
!(old_allocated & (1 << i))) {
if (!digital_module->AllocateDIO(15 - i, false /*input*/)) return;
allocated_digital_outputs_[digital_number - 1] |= 1 << i;
}
}
wpi_assertEqual(allocated_digital_outputs_[digital_number - 1],
- values_.digital_output_enables);
- digital_module->SetDIOs(values_.digital_output_enables,
- values_.digital_output_values);
+ motors_.digital_output_enables);
+ digital_module->SetDIOs(motors_.digital_output_enables,
+ motors_.digital_output_values);
- if (values_.pressure_switch_channel != 0 &&
- values_.compressor_channel != 0) {
- digital_module->SetRelayForward(values_.compressor_channel,
+ if (motors_.pressure_switch_channel != 0 &&
+ motors_.compressor_channel != 0) {
+ digital_module->SetRelayForward(motors_.compressor_channel,
!digital_module->GetDIO(
- values_.pressure_switch_channel));
+ motors_.pressure_switch_channel));
}
}
- int8_t solenoid_number = values_.solenoid_module;
+ int8_t solenoid_number = motors_.solenoid_module;
if (solenoid_number != -1) {
if (solenoid_number == 0) {
solenoid_number = SensorBase::GetDefaultSolenoidModule();
}
- if (solenoid_number < 1 || solenoid_number > 2) {
+ if (solenoid_number < 1 ||
+ solenoid_number > static_cast<int32_t>(SensorBase::kSolenoidModules)) {
char buf[64];
snprintf(buf, sizeof(buf), "Got Solenoid Module %d",
static_cast<int>(solenoid_number));
@@ -320,17 +329,17 @@
new SolenoidBase(solenoid_number);
}
- solenoid_base->SetAll(values_.solenoid_values);
+ solenoid_base->SetAll(motors_.solenoid_values);
}
- // This code can only assume that whatever is sending it values knows what
- // state it should be in.
- DriverStation::FMSState state = m_ds->GetCurrentState();
- {
- // Don't have to synchronize around getting the current state too because
+ // This code can only assume that whatever is sending it values received the
+ // most recent state that it sent out.
+ if (last_sent_state_valid_) {
+ // Don't have to synchronize with writing the last sent state too because
// it's ok if we're 1 cycle off. It would just be bad if we reported not
// being in any state or in 2 states at once.
RWLock::Locker state_locker(m_ds->GetUserStateLock(), true);
+ const DriverStation::FMSState state = last_sent_state_;
m_ds->InDisabled(state == DriverStation::FMSState::kDisabled);
m_ds->InAutonomous(state == DriverStation::FMSState::kAutonomous);
m_ds->InOperatorControl(state == DriverStation::FMSState::kTeleop);
@@ -367,6 +376,9 @@
joystick_values_.control.set_fms_attached(data->fmsAttached);
joystick_values_.control.set_autonomous(data->autonomous);
joystick_values_.control.set_enabled(data->enabled);
+
+ last_sent_state_ = m_ds->GetCurrentState();
+ last_sent_state_valid_ = true;
}
++joystick_values_.index;
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
index c2b1998..47c0f49 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
@@ -16,6 +16,7 @@
#include "WPILib/ErrorBase.h"
#include "WPILib/SolenoidBase.h"
#include "WPILib/Task.h"
+#include "WPILib/DriverStation.h"
// A simple implementation of receiving motor values over UDP and sending
// joystick data back out.
@@ -25,7 +26,7 @@
// This class takes care of disabling outputs when no packets are received in
// kDisableTime and feeding the Watchdog correctly.
//
-// The receiving interface consists of receiving NetworkRobotValues structs on
+// The receiving interface consists of receiving NetworkRobotMotors structs on
// a given UDP port from a given sender address. Each time a set of motor values
// is received, this class will update all output values.
//
@@ -42,9 +43,7 @@
UINT16 send_port, const char *receiver_address);
virtual ~NetworkRobot();
- virtual void StartCompetition();
-
- // Called when a valid packet has been received into values_.
+ // Called when a valid packet has been received into motors_.
// Subclasses can override if they want to do more, however they still need to
// call this implementation.
virtual void ProcessPacket();
@@ -64,11 +63,13 @@
// Deals with calling inet_aton and passing any errors on to the logging
// system. A helper is necessary here because inet_aton is normally just kind
// of annoying to deal with, but under vxworks, you have to make a copy of the
- // string before doing anything with etc etc so it's really complicated.
+ // string before doing anything with it etc etc so it's really complicated.
// Returns whether or not it was successful. If it returns false, then an
// error will have already been recorded.
bool FillinInAddr(const char *const_ip, in_addr *inet_address);
+ virtual void StartCompetition();
+
// Cleans everything up after the main loop encounters an error so that it can
// try again.
void Cleanup();
@@ -77,7 +78,8 @@
// Returns whether or not there is readable data available.
bool WaitForData();
- // Receives a packet and calls ProcessPacket() if it's a good one.
+ // Attempts to receive a packet (with WaitForData()) and calls ProcessPacket()
+ // if it's a good one.
void ReceivePacket();
// Gets run in its own task to take DS data and send it out.
@@ -101,10 +103,13 @@
const char *const receiver_address_;
int receive_socket_;
- NetworkRobotMotors values_;
+ NetworkRobotMotors motors_;
int send_socket_;
NetworkRobotJoysticks joystick_values_;
+ // A task set up to run StaticSendLoop (aka SendLoop()).
+ // Only actually gets used if we're going to do both parts (sending and
+ // receiving).
Task send_task_;
// Helper function to copy all of the data for a single joystick into
@@ -115,6 +120,11 @@
// Using Timer::GetPPCTimestamp() values.
double last_received_timestamp_;
+ // What the last state we sent out was.
+ // Kept track of so that we can more accurately report it back.
+ DriverStation::FMSState last_sent_state_;
+ bool last_sent_state_valid_;
+
// Ownership of the pointers stored here stays in DigitalModule.
DigitalModule *digital_modules_[2];
// This object owns pointers stored here.