started cleanup up the socket mess
removed unused #include + dependency
more formatting fixes + fixed users of ReceiveSocket
cleaned more stuff up (converted from references to pointers is one)
wip. started rewriting everything, not quite finished
got everything except SensorOutput done (I think...)
got everything compiling except for missing SensorReceiver
worked on implementing the logic. didn't finish
made everything compile and finished implementing SensorReceiver
pulling over Austin's mock time stuff
added IncrementMockTime
finished up and started on tests
remembered something else
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
new file mode 100644
index 0000000..95e30da
--- /dev/null
+++ b/frc971/input/sensor_unpacker.cc
@@ -0,0 +1,40 @@
+#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->lencoder);
+ const double right_encoder = drivetrain_translate(values->rencoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+}
+
+} // namespace frc971