Added support for both sets of rollers
Change-Id: I93a0f01eefe15364b510bd09c5dfc15f4df26456
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 759c5ca..140bf28 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -18,7 +18,8 @@
constexpr double kOperatingVoltage = 12.0;
constexpr double kLandingShoulderDownVoltage = -2.0;
// The maximum voltage the intake roller will be allowed to use.
-constexpr float kMaxIntakeVoltage = 8.0;
+constexpr float kMaxIntakeTopVoltage = 8.0;
+constexpr float kMaxIntakeBottomVoltage = 8.0;
// Aliases to reduce typing.
constexpr double kIntakeEncoderIndexDifference =
@@ -568,11 +569,15 @@
output->voltage_wrist = arm_.wrist_voltage();
// Logic to run our rollers on the intake.
- output->voltage_rollers = 0.0;
+ output->voltage_top_rollers = 0.0;
+ output->voltage_bottom_rollers = 0.0;
if (unsafe_goal) {
- output->voltage_rollers = ::std::max(
- -kMaxIntakeVoltage,
- ::std::min(unsafe_goal->voltage_rollers, kMaxIntakeVoltage));
+ output->voltage_top_rollers = ::std::max(
+ -kMaxIntakeTopVoltage,
+ ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
+ output->voltage_bottom_rollers = ::std::max(
+ -kMaxIntakeBottomVoltage,
+ ::std::min(unsafe_goal->voltage_bottom_rollers, kMaxIntakeBottomVoltage));
}
}
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 1422dde..8a3b518 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -51,7 +51,8 @@
float max_angular_acceleration_wrist;
// Voltage to send to the rollers. Positive is sucking in.
- float voltage_rollers;
+ float voltage_top_rollers;
+ float voltage_bottom_rollers;
};
message Status {
@@ -90,7 +91,8 @@
float voltage_shoulder;
float voltage_wrist;
- float voltage_rollers;
+ float voltage_top_rollers;
+ float voltage_bottom_rollers;
};
queue Goal goal;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 190692f..450eaa1 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -165,7 +165,8 @@
new_superstructure_goal->max_angular_acceleration_intake = 0.05;
new_superstructure_goal->max_angular_acceleration_shoulder = 0.05;
new_superstructure_goal->max_angular_acceleration_wrist = 0.05;
- new_superstructure_goal->voltage_rollers = 0.0;
+ new_superstructure_goal->voltage_top_rollers = 0.0;
+ new_superstructure_goal->voltage_bottom_rollers = 0.0;
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 4eb2e0d..59ff2e8 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -541,8 +541,12 @@
wrist_talon_ = ::std::move(t);
}
- void set_rollers_talon(::std::unique_ptr<Talon> t) {
- rollers_talon_ = ::std::move(t);
+ void set_top_rollers_talon(::std::unique_ptr<Talon> t) {
+ top_rollers_talon_ = ::std::move(t);
+ }
+
+ void set_bottom_rollers_talon(::std::unique_ptr<Talon> t) {
+ bottom_rollers_talon_ = ::std::move(t);
}
private:
@@ -556,7 +560,8 @@
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);
+ top_rollers_talon_->Set(queue->voltage_top_rollers / 12.0);
+ bottom_rollers_talon_->Set(queue->voltage_bottom_rollers / 12.0);
}
virtual void Stop() override {
@@ -567,7 +572,7 @@
}
::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_,
- rollers_talon_;
+ top_rollers_talon_, bottom_rollers_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -636,7 +641,9 @@
::std::unique_ptr<Talon>(new Talon(0)));
superstructure_writer.set_wrist_talon(
::std::unique_ptr<Talon>(new Talon(0)));
- superstructure_writer.set_rollers_talon(
+ superstructure_writer.set_top_rollers_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ superstructure_writer.set_bottom_rollers_talon(
::std::unique_ptr<Talon>(new Talon(0)));
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));