commit 687f524a494fd16d083cbb643d4d3e2759351a2e
author Brian Silverman <brians> Sat Mar 16 13:57:59 2013 -0700
committer Brian Silverman <brians> Sat Mar 30 16:35:08 2013 -0700
tree 6e7538a610ee18028ff3eaf1eb8e507654aa8193
parent 8806ae9749e60a479d3ef0c392b465639a8138d5

2013 hardware interface work

frc971/input/sensor_unpacker.cc

diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
index 8695785..bb99e2a 100644
--- a/frc971/input/sensor_unpacker.cc
+++ b/frc971/input/sensor_unpacker.cc
@@ -1,14 +1,25 @@
 #include "frc971/input/sensor_unpacker.h"
 
 #include <arpa/inet.h>
+#include <math.h>
 
 #include "aos/common/inttypes.h"
 
 #include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
 
+#ifndef M_PI
 #define M_PI 3.14159265358979323846
+#endif
 
 using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::wrist;
+using ::frc971::control_loops::angle_adjust;
+using ::frc971::control_loops::shooter;
+using ::frc971::control_loops::index_loop;
 
 namespace frc971 {
 namespace {
@@ -19,12 +30,35 @@
       (3.5 * 2.54 / 100.0 * M_PI);
 }
 
+inline double wrist_translate(int32_t in) {
+  return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+      (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
+}
+
+inline double angle_adjust_translate(int32_t in) {
+  static const double kCableDiameter = 0.060;
+  return -static_cast<double>(in) / (256.0 /*cpr*/ * 2.0 /*2x*/) *
+      ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
+      (2 * M_PI);
+}
+
+inline double shooter_translate(int32_t in) {
+ return -static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
+      (15.0 / 34.0) /*gears*/ * (2 * M_PI);
+}
+
+inline double index_translate(int32_t in) {
+  return -static_cast<double>(in) / (128.0 /*cpr*/ * 2.0 /*2x*/) *
+      (1.0) /*gears*/ * (2 * 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) {
+  for (size_t i = 0;
+       i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
     values->encoders[i] = ntohl(values->encoders[i]);
   }
 
@@ -37,6 +71,43 @@
       .left_encoder(left_encoder)
       .right_encoder(right_encoder)
       .Send();
+
+  wrist.position.MakeWithBuilder()
+      .pos(wrist_translate(values->wrist_position))
+      .hall_effect(!values->wrist_hall_effect)
+      .calibration(wrist_translate(values->wrist_edge_position))
+      .Send();
+
+  angle_adjust.position.MakeWithBuilder()
+      .angle(angle_adjust_translate(values->angle_adjust_position))
+     .bottom_hall_effect(!values->angle_adjust_bottom_hall_effect)
+      .middle_hall_effect(!values->angle_adjust_middle_hall_effect && false)
+      .bottom_calibration(angle_adjust_translate(
+              values->angle_adjust_bottom_edge_position))
+      .middle_calibration(angle_adjust_translate(
+             values->angle_adjust_middle_edge_position))
+      .Send();
+
+  shooter.position.MakeWithBuilder()
+      .position(shooter_translate(values->shooter_encoder))
+      .Send();
+
+  index_loop.position.MakeWithBuilder()
+      .index_position(index_translate(values->index_encoder))
+      .top_disc_detect(!values->top_disc)
+      .top_disc_posedge_count(values->top_disc_posedge_count)
+      .top_disc_posedge_position(index_translate(
+              values->top_disc_posedge_position))
+      .top_disc_negedge_count(values->top_disc_negedge_count)
+      .top_disc_negedge_position(index_translate(
+              values->top_disc_negedge_position))
+      .bottom_disc_detect(!values->bottom_disc)
+      .bottom_disc_posedge_count(values->bottom_disc_posedge_count)
+      .bottom_disc_negedge_count(values->bottom_disc_negedge_count)
+      .bottom_disc_negedge_wait_position(index_translate(
+              values->bottom_disc_negedge_wait_position))
+      .bottom_disc_negedge_wait_count(values->bottom_disc_negedge_wait_count)
+      .Send();
 }
 
 }  // namespace frc971