commit f4cd764cc8161cfd455fc38e9d7a8379498b1fc0
author Comran Morshed <comranmorsh@gmail.com> Mon Feb 15 20:40:49 2016 +0000
committer Comran Morshed <comranmorsh@gmail.com> Tue Feb 16 12:48:42 2016 +0000
tree fb01bf35fa7e3aa3d4afb8f89de0651de62eb023
parent 0119eb14c2321968a6622d9b25aef0acc404b0fa

Add intake rollers.

Change-Id: Ib6f78956950d61fb048e7370a609a3f2f9f3c876

y2016/control_loops/superstructure/superstructure.cc

diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index a2b7a9e..096ed58 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,14 +14,14 @@
 namespace superstructure {
 
 namespace {
+constexpr double kZeroingVoltage = 4.0;
+
 constexpr double kIntakeEncoderIndexDifference =
     constants::Values::kIntakeEncoderIndexDifference;
 constexpr double kWristEncoderIndexDifference =
     constants::Values::kWristEncoderIndexDifference;
 constexpr double kShoulderEncoderIndexDifference =
     constants::Values::kShoulderEncoderIndexDifference;
-
-constexpr double kZeroingVoltage = 4.0;
 }  // namespace
 
 // ///// CollisionAvoidance /////
@@ -454,6 +454,13 @@
     output->voltage_intake = intake_.intake_voltage();
     output->voltage_shoulder = arm_.shoulder_voltage();
     output->voltage_wrist = arm_.wrist_voltage();
+
+    // Logic to run our rollers on the intake.
+    output->voltage_rollers = 0.0;
+    if (unsafe_goal) {
+      output->voltage_rollers =
+          ::std::max(-8.0, ::std::min(8.0, unsafe_goal->voltage_rollers));
+    }
   }
 
   // Save debug/internal state.

y2016/control_loops/superstructure/superstructure.q

diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 63fdcf0..1422dde 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -49,6 +49,9 @@
     float max_angular_acceleration_intake;
     float max_angular_acceleration_shoulder;
     float max_angular_acceleration_wrist;
+
+    // Voltage to send to the rollers. Positive is sucking in.
+    float voltage_rollers;
   };
 
   message Status {
@@ -86,6 +89,8 @@
     float voltage_intake;
     float voltage_shoulder;
     float voltage_wrist;
+
+    float voltage_rollers;
   };
 
   queue Goal goal;

y2016/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index f46611c..d43af31 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -551,6 +551,10 @@
     wrist_talon_ = ::std::move(t);
   }
 
+  void set_rollers_talon(::std::unique_ptr<Talon> t) {
+    rollers_talon_ = ::std::move(t);
+  }
+
  private:
   virtual void Read() override {
     ::y2016::control_loops::superstructure_queue.output.FetchAnother();
@@ -562,6 +566,7 @@
     intake_talon_->Set(queue->voltage_intake / 12.0);
     shoulder_talon_->Set(-queue->voltage_shoulder / 12.0);
     wrist_talon_->Set(queue->voltage_wrist / 12.0);
+    rollers_talon_->Set(queue->voltage_rollers / 12.0);
   }
 
   virtual void Stop() override {
@@ -571,7 +576,8 @@
     wrist_talon_->Disable();
   }
 
-  ::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_;
+  ::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_,
+      rollers_talon_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -640,6 +646,8 @@
         ::std::unique_ptr<Talon>(new Talon(0)));
     superstructure_writer.set_wrist_talon(
         ::std::unique_ptr<Talon>(new Talon(0)));
+    superstructure_writer.set_rollers_talon(
+        ::std::unique_ptr<Talon>(new Talon(0)));
     ::std::thread superstructure_writer_thread(
         ::std::ref(superstructure_writer));