redid the sensor reading code for the bbb+cape
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 7bf884e..d60b26b 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -23,8 +23,7 @@
'../output/output.gyp:MotorWriter',
'../output/output.gyp:CameraServer',
#'camera/camera.gyp:frc971',
- '../../gyro_board/src/libusb-driver/libusb-driver.gyp:get',
- #'../input/input.gyp:gyro_sensor_receiver',
+ '../input/input.gyp:sensor_receiver',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:*',
],
'copies': [
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 8d5ad90..d26f851 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -22,10 +22,10 @@
],
},
{
- 'target_name': 'gyro_sensor_receiver',
+ 'target_name': 'sensor_receiver',
'type': 'executable',
'sources': [
- 'gyro_sensor_receiver.cc',
+ 'sensor_receiver.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
@@ -37,32 +37,9 @@
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
- 'usb_receiver',
'<(DEPTH)/frc971/frc971.gyp:constants',
- ],
- },
- {
- 'target_name': 'usb_receiver',
- 'type': 'static_library',
- 'sources': [
- 'usb_receiver.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_reader',
'<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/common/common.gyp:time',
- ],
- 'variables': {
- # TODO(brians): Add dependency on this file too (or something).
- 'checksum': '<!(<(DEPTH)/gyro_board/src/usb/data_struct_checksum.sh)',
- },
- 'defines': [
- 'GYRO_BOARD_DATA_CHECKSUM=<(checksum)',
],
},
],
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();
}