redid the sensor reading code for the bbb+cape
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 1e1ccc4..6fdd2e6 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -3,6 +3,9 @@
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/time.h"
+
+#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
@@ -10,7 +13,6 @@
#include "frc971/control_loops/index/index_motor.q.h"
#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/input/usb_receiver.h"
#include "frc971/constants.h"
#ifndef M_PI
@@ -81,107 +83,108 @@
return (voltage - k.low) / (k.high - k.low);
}
+WrappingCounter top_rise_;
+WrappingCounter top_fall_;
+WrappingCounter bottom_rise_;
+WrappingCounter bottom_fall_delay_;
+WrappingCounter bottom_fall_;
+void ProcessData(const ::bbb::DataStruct *data, bool bad_gyro) {
+ if (!bad_gyro) {
+ gyro.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
+ .Send();
+ }
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
+ .left_shifter_position(hall_translate(constants::GetValues().left_drive,
+ data->main.left_drive_hall))
+ .right_shifter_position(hall_translate(
+ constants::GetValues().right_drive, data->main.right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage))
+ .Send();
+
+ wrist.position.MakeWithBuilder()
+ .pos(wrist_translate(data->main.wrist))
+ .hall_effect(data->main.wrist_hall_effect)
+ .calibration(wrist_translate(data->main.capture_wrist_rise))
+ .Send();
+
+ angle_adjust.position.MakeWithBuilder()
+ .angle(angle_adjust_translate(data->main.shooter_angle))
+ .bottom_hall_effect(data->main.angle_adjust_bottom_hall_effect)
+ .middle_hall_effect(false)
+ .bottom_calibration(angle_adjust_translate(
+ data->main.capture_shooter_angle_rise))
+ .middle_calibration(angle_adjust_translate(
+ 0))
+ .Send();
+
+ shooter.position.MakeWithBuilder()
+ .position(shooter_translate(data->main.shooter))
+ .Send();
+
+ index_loop.position.MakeWithBuilder()
+ .index_position(index_translate(data->main.indexer))
+ .top_disc_detect(data->main.top_disc)
+ .top_disc_posedge_count(top_rise_.Update(data->main.top_rise_count))
+ .top_disc_posedge_position(
+ index_translate(data->main.capture_top_rise))
+ .top_disc_negedge_count(top_fall_.Update(data->main.top_fall_count))
+ .top_disc_negedge_position(
+ index_translate(data->main.capture_top_fall))
+ .bottom_disc_detect(data->main.bottom_disc)
+ .bottom_disc_posedge_count(
+ bottom_rise_.Update(data->main.bottom_rise_count))
+ .bottom_disc_negedge_count(
+ bottom_fall_.Update(data->main.bottom_fall_count))
+ .bottom_disc_negedge_wait_position(index_translate(
+ data->main.capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(
+ bottom_fall_delay_.Update(data->main.bottom_fall_delay_count))
+ .loader_top(data->main.loader_top)
+ .loader_bottom(data->main.loader_bottom)
+ .Send();
+}
+
+void PacketReceived(const ::bbb::DataStruct *data,
+ const ::aos::time::Time &cape_timestamp) {
+ LOG(DEBUG, "cape timestamp %010" PRId32 ".%09" PRId32 "s\n",
+ cape_timestamp.sec(), cape_timestamp.nsec());
+ bool bad_gyro;
+ if (data->uninitialized_gyro) {
+ LOG(DEBUG, "uninitialized gyro\n");
+ bad_gyro = true;
+ } else if (data->zeroing_gyro) {
+ LOG(DEBUG, "zeroing gyro\n");
+ bad_gyro = true;
+ } else if (data->bad_gyro) {
+ LOG(ERROR, "bad gyro\n");
+ bad_gyro = true;
+ gyro.MakeWithBuilder().angle(0).Send();
+ } else if (data->old_gyro_reading) {
+ LOG(WARNING, "old/bad gyro reading\n");
+ bad_gyro = true;
+ } else {
+ bad_gyro = false;
+ }
+ static int i = 0;
+ ++i;
+ if (i == 5) {
+ i = 0;
+ ProcessData(data, bad_gyro);
+ }
+}
+
} // namespace
-
-class GyroSensorReceiver : public USBReceiver {
- public:
- GyroSensorReceiver() : USBReceiver(2) {}
-
- virtual void PacketReceived(const ::aos::time::Time &/*timestamp*/) override {
- if (data()->uninitialized_gyro) {
- LOG(DEBUG, "uninitialized gyro\n");
- bad_gyro_ = true;
- } else if (data()->zeroing_gyro) {
- LOG(DEBUG, "zeroing gyro\n");
- bad_gyro_ = true;
- } else if (data()->bad_gyro) {
- LOG(ERROR, "bad gyro\n");
- bad_gyro_ = true;
- gyro.MakeWithBuilder().angle(0).Send();
- } else if (data()->old_gyro_reading) {
- LOG(WARNING, "old/bad gyro reading\n");
- bad_gyro_ = true;
- } else {
- bad_gyro_ = false;
- }
- }
-
- virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
- if (!bad_gyro_) {
- gyro.MakeWithBuilder()
- .angle(gyro_translate(data()->gyro_angle))
- .Send();
- }
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data()->main.right_drive))
- .left_encoder(-drivetrain_translate(data()->main.left_drive))
- .left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data()->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data()->main.right_drive_hall))
- .battery_voltage(battery_translate(data()->main.battery_voltage))
- .Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data()->main.wrist))
- .hall_effect(data()->main.wrist_hall_effect)
- .calibration(wrist_translate(data()->main.capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data()->main.shooter_angle))
- .bottom_hall_effect(data()->main.angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data()->main.capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data()->main.shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data()->main.indexer))
- .top_disc_detect(data()->main.top_disc)
- .top_disc_posedge_count(top_rise_.Update(data()->main.top_rise_count))
- .top_disc_posedge_position(
- index_translate(data()->main.capture_top_rise))
- .top_disc_negedge_count(top_fall_.Update(data()->main.top_fall_count))
- .top_disc_negedge_position(
- index_translate(data()->main.capture_top_fall))
- .bottom_disc_detect(data()->main.bottom_disc)
- .bottom_disc_posedge_count(
- bottom_rise_.Update(data()->main.bottom_rise_count))
- .bottom_disc_negedge_count(
- bottom_fall_.Update(data()->main.bottom_fall_count))
- .bottom_disc_negedge_wait_position(index_translate(
- data()->main.capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(
- bottom_fall_delay_.Update(data()->main.bottom_fall_delay_count))
- .loader_top(data()->main.loader_top)
- .loader_bottom(data()->main.loader_bottom)
- .Send();
- }
-
- bool bad_gyro_;
-
- WrappingCounter top_rise_;
- WrappingCounter top_fall_;
- WrappingCounter bottom_rise_;
- WrappingCounter bottom_fall_delay_;
- WrappingCounter bottom_fall_;
-};
-
} // namespace frc971
int main() {
- ::aos::Init(frc971::USBReceiver::kRelativePriority);
- ::frc971::GyroSensorReceiver receiver;
+ ::aos::Init(::bbb::SensorReader::kRelativePriority);
+ ::bbb::SensorReader reader("main");
while (true) {
- receiver.RunIteration();
+ ::frc971::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
}
::aos::Cleanup();
}