copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
new file mode 100644
index 0000000..fd14df4
--- /dev/null
+++ b/frc971/input/SensorReader.cc
@@ -0,0 +1,44 @@
+#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)