update the information about enabled vs not
With the roboRIO, we get different information about whether motors are
enabled (and some about sensors too), so the system for propagating it
around to everything needs to change.
Change-Id: I7e5f9591eeac1fdfe57b271333c3827431fbef01
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index c88169a..395f8b9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -630,7 +630,7 @@
loop_->mutable_U(1, 0) = ::aos::Clip(
(R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-12.0, 12.0);
- loop_->mutable_U() *= 12.0 / position_.battery_voltage;
+ loop_->mutable_U() *= 12.0 / ::aos::robot_state->voltage_battery;
#endif
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index ea1324d..5f34e92 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -39,7 +39,6 @@
double right_encoder;
//double left_shifter_position;
//double right_shifter_position;
- double battery_voltage;
};
message Output {
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index e1a256b..f25434f 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -323,7 +323,7 @@
// Set the goal to here to make it so it doesn't move when disabled.
loop_->R = loop_->X_hat;
// Only progress if we are enabled.
- if (::aos::robot_state->enabled) {
+ if (::aos::joystick_state->enabled) {
if (AnySensorsActive(position)) {
state_ = MOVING_OFF;
} else {
@@ -336,7 +336,7 @@
LOG(DEBUG, "MOVING_OFF\n");
{
// Move off the hall effect sensor.
- if (!::aos::robot_state->enabled) {
+ if (!::aos::joystick_state->enabled) {
// Start over if disabled.
state_ = UNINITIALIZED;
} else if (position && !AnySensorsActive(position)) {
@@ -353,7 +353,7 @@
LOG(DEBUG, "ZEROING\n");
{
int active_sensor_index = ActiveSensorIndex(position);
- if (!::aos::robot_state->enabled) {
+ if (!::aos::joystick_state->enabled) {
// Start over if disabled.
state_ = UNINITIALIZED;
} else if (position && active_sensor_index != -1) {
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index a155b7e..0ea2236 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -116,8 +116,8 @@
have_zeroing_data = true;
}
- ::aos::robot_state.FetchLatest();
- if (::aos::robot_state.get() && ::aos::robot_state->enabled &&
+ ::aos::joystick_state.FetchLatest();
+ if (::aos::joystick_state.get() && ::aos::joystick_state->enabled &&
have_zeroing_data) {
zero_offset = 0;
for (size_t i = 0; i < sizeof(zeroing_data) / sizeof(zeroing_data[0]);
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 7115c38..de7fe88 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -20,7 +20,7 @@
while (run_) {
ds->WaitForData();
- auto new_state = ::aos::robot_state.MakeMessage();
+ auto new_state = ::aos::joystick_state.MakeMessage();
HALControlWord control_word;
HALGetControlWord(&control_word);
@@ -37,10 +37,10 @@
new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
}
}
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
+ LOG_STRUCT(DEBUG, "joystick_state", *new_state);
if (!new_state.Send()) {
- LOG(WARNING, "sending robot_state failed\n");
+ LOG(WARNING, "sending joystick_state failed\n");
}
}
}
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index b7b112e..cb5e4ec 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -21,16 +21,16 @@
::aos::SetCurrentThreadRealtimePriority(30);
while (run_) {
- no_robot_state_.Print();
- fake_robot_state_.Print();
+ no_joystick_state_.Print();
+ fake_joystick_state_.Print();
Read();
- ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get()) {
- LOG_INTERVAL(no_robot_state_);
+ ::aos::joystick_state.FetchLatest();
+ if (!::aos::joystick_state.get()) {
+ LOG_INTERVAL(no_joystick_state_);
continue;
}
- if (::aos::robot_state->fake) {
- LOG_INTERVAL(fake_robot_state_);
+ if (::aos::joystick_state->fake) {
+ LOG_INTERVAL(fake_joystick_state_);
continue;
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 1c8d4da..fe32763 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -75,12 +75,12 @@
::std::atomic<bool> run_{true};
- ::aos::util::SimpleLogInterval no_robot_state_ =
+ ::aos::util::SimpleLogInterval no_joystick_state_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), INFO,
- "no robot state -> not outputting");
- ::aos::util::SimpleLogInterval fake_robot_state_ =
+ "no joystick state -> not outputting");
+ ::aos::util::SimpleLogInterval fake_joystick_state_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), DEBUG,
- "fake robot state -> not outputting");
+ "fake joystick state -> not outputting");
};
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index b1dcd7e..dcd46c2 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -15,13 +15,12 @@
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
'<(AOS)/common/controls/controls.gyp:control_loop',
- '<(AOS)/common/controls/controls.gyp:sensor_generation',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/controls/controls.gyp:output_check',
'<(AOS)/common/messages/messages.gyp:robot_state',
'<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
'hall_effect',
'joystick_sender',
'loop_output_handler',
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 554993a..d950c02 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -7,17 +7,15 @@
#include <mutex>
#include <functional>
-#include "aos/common/controls/output_check.q.h"
-#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/common/messages/robot_state.q.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/common/util/phased_loop.h"
#include "aos/common/util/wrapping_counter.h"
#include "aos/common/stl_mutex.h"
#include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -41,6 +39,7 @@
#include "Compressor.h"
#include "RobotBase.h"
#include "dma.h"
+#include "ControllerPower.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -213,6 +212,9 @@
void operator()() {
::aos::SetCurrentThreadName("SensorReader");
+ my_pid_ = getpid();
+ ds_ = DriverStation::GetInstance();
+
wrist_encoder_.Start();
dma_synchronizer_->Start();
@@ -226,27 +228,26 @@
}
void RunIteration() {
- DriverStation *ds = DriverStation::GetInstance();
+ {
+ auto new_state = ::aos::robot_state.MakeMessage();
- if (ds->IsSysActive()) {
- auto message = ::aos::controls::output_check_received.MakeMessage();
- // TODO(brians): Actually read a pulse value from the roboRIO.
- message->pwm_value = 0;
- message->pulse_length = -1;
- LOG_STRUCT(DEBUG, "received", *message);
- message.Send();
+ new_state->outputs_enabled = ds_->IsSysActive();
+ new_state->browned_out = ds_->IsSysBrownedOut();
+
+ new_state->is_3v3_active = ControllerPower::GetEnabled3V3();
+ new_state->is_5v_active = ControllerPower::GetEnabled5V();
+ new_state->voltage_3v3 = ControllerPower::GetVoltage3V3();
+ new_state->voltage_5v = ControllerPower::GetVoltage5V();
+
+ new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
+ new_state->voltage_battery = ds_->GetBatteryVoltage();
+
+ new_state.Send();
}
drivetrain_queue.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(right_encoder_->GetRaw()))
.left_encoder(-drivetrain_translate(left_encoder_->GetRaw()))
- .battery_voltage(ds->GetBatteryVoltage())
- .Send();
-
- // Signal that we are alive.
- ::aos::controls::sensor_generation.MakeWithBuilder()
- .reader_pid(getpid())
- .cape_resets(0)
.Send();
dma_synchronizer_->RunIteration();
@@ -280,6 +281,9 @@
static const int kPriority = 30;
static const int kInterruptPriority = 55;
+ int32_t my_pid_;
+ DriverStation *ds_;
+
void CopyPotAndIndexPosition(
const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate,