Merge branch 'sensor-data-timing' into hardware-interface-integration
Conflicts:
aos/common/time.h
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
index 9fbee44..bdcd07e 100644
--- a/frc971/input/GyroReader.cc
+++ b/frc971/input/GyroReader.cc
@@ -4,6 +4,7 @@
#include <fcntl.h>
#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
#include "frc971/queues/GyroAngle.q.h"
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
deleted file mode 100644
index fd14df4..0000000
--- a/frc971/input/SensorReader.cc
+++ /dev/null
@@ -1,44 +0,0 @@
-#define __STDC_LIMIT_MACROS
-
-#include <arpa/inet.h>
-
-#include "aos/aos_core.h"
-#include "aos/common/inttypes.h"
-#include "aos/common/input/SensorInput.h"
-
-#include "frc971/control_loops/DriveTrain.q.h"
-#include "frc971/queues/sensor_values.h"
-
-#define M_PI 3.14159265358979323846
-
-using ::frc971::control_loops::drivetrain;
-
-namespace frc971 {
-
-namespace {
-inline double drivetrain_translate(int32_t in) {
- // TODO(2013) fix the math
- return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
- (3.5 * 2.54 / 100.0 * M_PI);
-}
-} // namespace
-
-class SensorReader : public aos::SensorInput<sensor_values> {
- virtual void RunIteration(sensor_values &sensors) {
- for (size_t i = 0; i < sizeof(sensors.encoders) / sizeof(sensors.encoders[0]); ++i) {
- sensors.encoders[i] = ntohl(sensors.encoders[i]);
- }
-
- // TODO(aschuh): Convert to meters.
- const double left_encoder = drivetrain_translate(sensors.lencoder);
- const double right_encoder = drivetrain_translate(sensors.rencoder);
- drivetrain.position.MakeWithBuilder()
- .left_encoder(left_encoder)
- .right_encoder(right_encoder)
- .Send();
- }
-};
-
-} // namespace frc971
-
-AOS_RUN(frc971::SensorReader)
diff --git a/frc971/input/SensorWriter.cc b/frc971/input/SensorWriter.cc
deleted file mode 100644
index 1fb34db..0000000
--- a/frc971/input/SensorWriter.cc
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <arpa/inet.h>
-
-#include "WPILib/Task.h"
-#include "WPILib/Encoder.h"
-#include "WPILib/DigitalInput.h"
-#include "WPILib/Counter.h"
-
-#include "aos/aos_core.h"
-#include "aos/crio/motor_server/SensorOutput.h"
-#include "aos/common/inttypes.h"
-#include "aos/common/mutex.h"
-#include "aos/crio/shared_libs/interrupt_notifier.h"
-
-#include "frc971/queues/sensor_values.h"
-
-using ::aos::MutexLocker;
-
-namespace frc971 {
-
-class SensorWriter : public aos::SensorOutput<sensor_values> {
- Encoder lencoder;
- Encoder rencoder;
-
- public:
- SensorWriter() : lencoder(1, 2), rencoder(3, 4) {
- lencoder.Start();
- rencoder.Start();
-
- printf("frc971::SensorWriter started\n");
- }
-
- virtual void RunIteration(sensor_values &vals) {
- vals.lencoder = htonl(-lencoder.GetRaw());
- vals.rencoder = -htonl(-rencoder.GetRaw());
- }
-};
-
-} // namespace frc971
-
-AOS_RUN(frc971::SensorWriter)
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index c62365b..27d6ae9 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -8,7 +8,6 @@
'header_path': 'frc971/input',
},
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/common/common.gyp:controls',
],
'includes': ['../../aos/build/queues.gypi'],
@@ -20,9 +19,7 @@
'JoystickReader.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/atom_code/input/input.gyp:joystick',
- '<(AOS)/common/network/network.gyp:socket',
'actions',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
@@ -30,35 +27,50 @@
],
},
{
- 'target_name': 'SensorReader',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_unpacker',
+ 'type': 'static_library',
'sources': [
- 'SensorReader.cc',
+ 'sensor_unpacker.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/network/network.gyp:socket',
- ],
- 'conditions': [
- ['OS!="crio"', {
- 'dependencies': [
- '<(AOS)/atom_code/atom_code.gyp:init',
- ],
- }],
],
},
{
- 'target_name': 'SensorWriter',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_receiver',
+ 'type': 'executable',
'sources': [
- 'SensorWriter.cc',
+ 'sensor_receiver.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'sensor_unpacker',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'sensor_packer',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensor_packer.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
],
},
{
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
new file mode 100644
index 0000000..865b25b
--- /dev/null
+++ b/frc971/input/sensor_packer.cc
@@ -0,0 +1,83 @@
+#include "frc971/input/sensor_packer.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+#include "aos/common/time.h"
+
+#include "frc971/control_loops/index/index.h"
+
+using ::aos::MutexLocker;
+
+namespace frc971 {
+
+SensorPacker::SensorPacker() : lencoder(1, 2), rencoder(3, 4) {
+ lencoder.Start();
+ rencoder.Start();
+
+ printf("frc971::SensorPacker started\n");
+}
+
+void SensorPacker::ReadEncoder(EncoderReadData *data) {
+ int32_t value = data->counter->Get();
+ {
+ ::aos::MutexLocker locker(data->sync);
+ *data->output = value;
+ }
+}
+
+void SensorPacker::TopDiscEdgeReader() {
+ while (true) {
+ top_disc_posedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_posedge_position_ = position;
+ ++top_disc_posedge_count_;
+ }
+ top_disc_negedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_negedge_position_ = position;
+ ++top_disc_negedge_count_;
+ }
+ }
+}
+
+void SensorPacker::BottomDiscEdgeReader() {
+ static const aos::time::Time kBottomDiscNegedgeSleep =
+ aos::time::Time::InSeconds(
+ control_loops::IndexMotor::kBottomDiscIndexDelay);
+ while (true) {
+ bottom_disc_->source()->WaitForInterrupt(kInterruptTimeout);
+ if (bottom_disc_->Get()) {
+ aos::time::Time start = aos::time::Time::Now();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_negedge_count_;
+ }
+ aos::time::SleepUntil(start + kBottomDiscNegedgeSleep);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ bottom_disc_negedge_wait_position_ = position;
+ ++bottom_disc_negedge_wait_count_;
+ }
+ } else {
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_posedge_count_;
+ }
+ }
+ }
+}
+
+void SensorPacker::PackInto(sensor_values *values) {
+ values->lencoder = htonl(-lencoder.GetRaw());
+ values->rencoder = -htonl(-rencoder.GetRaw());
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_packer.h b/frc971/input/sensor_packer.h
new file mode 100644
index 0000000..78f6bf6
--- /dev/null
+++ b/frc971/input/sensor_packer.h
@@ -0,0 +1,106 @@
+#ifndef FRC971_INPUT_SENSOR_PACKER_H_
+#define FRC971_INPUT_SENSOR_PACKER_H_
+
+#include "aos/common/libstdc++/memory"
+
+#include "WPILib/Task.h"
+#include "WPILib/Encoder.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/Counter.h"
+#include "WPILib/CounterBase.h"
+#include "WPILib/AnalogChannel.h"
+#include "WPILib/AnalogTrigger.h"
+
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+#include "aos/common/sensors/sensor_packer.h"
+#include "aos/crio/hardware/counter.h"
+#include "aos/crio/hardware/digital_source.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorPacker
+ : public ::aos::sensors::SensorPackerInterface<sensor_values> {
+ public:
+ SensorPacker();
+
+ virtual void PackInto(sensor_values *values);
+
+ private:
+ // Used for passing ReadEncoder all of the information that it needs.
+ struct EncoderReadData {
+ EncoderReadData(::aos::crio::hardware::Counter *counter,
+ int32_t *output,
+ ::aos::Mutex *sync)
+ : counter(counter), output(output), sync(sync) {}
+
+ ::aos::crio::hardware::Counter *const counter;
+ int32_t *const output;
+ ::aos::Mutex *const sync;
+ };
+
+ // Handler function for an ::aos::crio::InterruptNotifier that reads an
+ // encoder and (synchronized on a ::aos::Mutex) writes the value into a
+ // pointer.
+ static void ReadEncoder(EncoderReadData *data);
+
+ // A whole day.
+ // Used for passing to WPILib's WaitForInterrupts.
+ static const float kInterruptTimeout = 60 * 60 * 24;
+
+ static void StaticTopDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->TopDiscEdgeReader();
+ }
+ static void StaticBottomDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->BottomDiscEdgeReader();
+ }
+
+ void TopDiscEdgeReader();
+ void BottomDiscEdgeReader();
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_left_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_right_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> wrist_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> wrist_hall_effect_;
+ int32_t wrist_edge_position_;
+ ::aos::Mutex wrist_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData> wrist_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> angle_adjust_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_middle_hall_effect_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_bottom_hall_effect_;
+ int32_t angle_adjust_middle_edge_position_;
+ int32_t angle_adjust_bottom_edge_position_;
+ ::aos::Mutex angle_adjust_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_middle_notifier_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_bottom_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> shooter_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::CounterCounter> index_counter_;
+ Task top_disc_edge_task_;
+ ::aos::Mutex top_disc_edge_sync_;
+ ::std::unique_ptr< ::AnalogTrigger> top_disc_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_posedge_output_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_negedge_output_;
+ int32_t top_disc_posedge_count_;
+ int32_t top_disc_negedge_count_;
+ int32_t top_disc_posedge_position_;
+ int32_t top_disc_negedge_position_;
+ Task bottom_disc_edge_task_;
+ ::aos::Mutex bottom_disc_edge_sync_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> bottom_disc_;
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_PACKER_H_
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
new file mode 100644
index 0000000..a0b7f8f
--- /dev/null
+++ b/frc971/input/sensor_receiver.cc
@@ -0,0 +1,16 @@
+#include "aos/common/sensors/sensor_receiver.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/sensor_values.h"
+#include "frc971/input/sensor_unpacker.h"
+
+int main() {
+ ::aos::Init();
+ ::frc971::SensorUnpacker unpacker;
+ ::aos::sensors::NetworkSensorReceiver< ::frc971::sensor_values>
+ receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
new file mode 100644
index 0000000..8695785
--- /dev/null
+++ b/frc971/input/sensor_unpacker.cc
@@ -0,0 +1,42 @@
+#include "frc971/input/sensor_unpacker.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+
+#include "frc971/control_loops/DriveTrain.q.h"
+
+#define M_PI 3.14159265358979323846
+
+using ::frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ // TODO(2013) fix the math
+ return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
+ (3.5 * 2.54 / 100.0 * M_PI);
+}
+
+} // namespace
+
+SensorUnpacker::SensorUnpacker() {}
+
+void SensorUnpacker::UnpackFrom(sensor_values *values) {
+ for (size_t i = 0; i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
+ values->encoders[i] = ntohl(values->encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(
+ values->drive_left_encoder);
+ const double right_encoder = drivetrain_translate(
+ values->drive_right_encoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_unpacker.h b/frc971/input/sensor_unpacker.h
new file mode 100644
index 0000000..b15244a
--- /dev/null
+++ b/frc971/input/sensor_unpacker.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_INPUT_SENSOR_UNPACKER_H_
+#define FRC971_INPUT_SENSOR_UNPACKER_H_
+
+#include "aos/common/sensors/sensor_unpacker.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorUnpacker
+ : public ::aos::sensors::SensorUnpackerInterface<sensor_values> {
+ public:
+ SensorUnpacker();
+
+ virtual void UnpackFrom(sensor_values *values);
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_UNPACKER_H_