got the output check stuff actually compiling and closer to working
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 3def0d7..199df99 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -71,7 +71,8 @@
}
double sonar_translate(uint32_t in) {
- return static_cast<double>(in) / 1000.0 * 2.0;
+ return static_cast<double>(in) * 10.0 /*us/tick*/ / 147.0 /*in/us*/ *
+ 0.0254 /*m/in*/;
}
double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index d42cc05..eefd7d6 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -12,11 +12,10 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/queues/output_check.q.h"
using ::aos::util::SimpleLogInterval;
-using ::frc971::control_loops::drivetrain;
-
namespace frc971 {
namespace output {
@@ -39,13 +38,14 @@
values_.solenoid_module = 0;
if (true) {
- drivetrain.output.FetchLatest();
- if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
- SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
- SetSolenoid(7, drivetrain.output->left_high);
- SetSolenoid(8, drivetrain.output->right_high);
+ static auto &drivetrain = ::frc971::control_loops::drivetrain.output;
+ drivetrain.FetchLatest();
+ if (drivetrain.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *drivetrain);
+ SetPWMOutput(3, drivetrain->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain->left_voltage / 12.0, kTalonBounds);
+ SetSolenoid(7, drivetrain->left_high);
+ SetSolenoid(8, drivetrain->right_high);
} else {
DisablePWMOutput(3);
DisablePWMOutput(8);
@@ -59,7 +59,7 @@
::frc971::control_loops::shooter_queue_group.output;
shooter.FetchLatest();
if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *shooter.get());
+ LOG_STRUCT(DEBUG, "will output", *shooter);
SetPWMOutput(7, shooter->voltage / 12.0, kTalonBounds);
SetSolenoid(6, !shooter->latch_piston);
SetSolenoid(5, !shooter->brake_piston);
@@ -75,7 +75,7 @@
static auto &claw = ::frc971::control_loops::claw_queue_group.output;
claw.FetchLatest();
if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *claw.get());
+ LOG_STRUCT(DEBUG, "will output", *claw);
SetPWMOutput(9, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(8, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(1, -claw->bottom_claw_voltage / 12.0, kTalonBounds);
@@ -94,11 +94,15 @@
claw_old_.Print();
}
- ++output_check_;
- if (output_check_ == 0) output_check_ = 1;
- ::frc971::output_check_queue.MakeWithBuilder()
- .pwm_value(output_check_).Send();
- SetPWMOutput(10, output_check_);
+ {
+ auto message = ::frc971::output_check_queue.MakeMessage();
+ ++output_check_;
+ if (output_check_ == 0) output_check_ = 1;
+ SetRawPWMOutput(10, output_check_);
+ message->sent_value = output_check_;
+ LOG_STRUCT(DEBUG, "sending", *message);
+ message.Send();
+ }
}
SimpleLogInterval drivetrain_old_ =
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index bc2d604..de41943 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -53,6 +53,7 @@
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:output_check',
],
},
],
diff --git a/frc971/queues/output_check.q b/frc971/queues/output_check.q
new file mode 100644
index 0000000..0573426
--- /dev/null
+++ b/frc971/queues/output_check.q
@@ -0,0 +1,8 @@
+package frc971;
+
+message OutputCheck {
+ uint8_t sent_value;
+};
+// Each message here represents a value that was sent to the cRIO.
+// The sent timestamp of the message is when the value was sent.
+queue OutputCheck output_check_queue;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 8434437..d71be25 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -22,19 +22,14 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
- 'target_name': 'frc971_queues_so',
- 'type': 'loadable_module',
- 'sources': ['<@(queue_files)'],
+ 'target_name': 'output_check',
+ 'type': 'static_library',
+ 'sources': [
+ 'output_check.q',
+ ],
'variables': {
'header_path': 'frc971/queues',
},
- 'dependencies': [
- ],
- 'direct_dependent_settings': {
- 'variables': {
- 'jni_libs': ['frc971_queues_so'],
- },
- },
'includes': ['../../aos/build/queues.gypi'],
},
],