Servo now routed for gear.
Change-Id: Ifee31cb80bbdfbcf4ef900fe701f578987e9d7e5
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index e0017c1..63ab691 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -109,6 +109,9 @@
status->intake.zeroed && status->hood.zeroed && status->turret.zeroed;
if (output && unsafe_goal) {
+ output->gear_servo =
+ ::std::min(1.0, ::std::max(0.0, unsafe_goal->intake.gear_servo));
+
output->voltage_intake_rollers =
::std::max(-kMaxIntakeRollerVoltage,
::std::min(unsafe_goal->intake.voltage_rollers,
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index dbd4819..c7283b2 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -21,6 +21,9 @@
// If true, disable the intake so we can hang.
bool disable_intake;
+
+ // The gear servo value.
+ double gear_servo;
};
struct IndexerGoal {
@@ -231,6 +234,8 @@
double voltage_turret;
double voltage_hood;
+ double gear_servo;
+
// If true, the lights are on.
bool lights_on;
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 246150f..acb21a7 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -226,6 +226,13 @@
new_superstructure_goal->hood.angle = hood_goal_;
new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+ if (data.IsPressed(kIntakeUp)) {
+ new_superstructure_goal->intake.gear_servo = 0.37;
+ } else {
+ // Clamp the gear
+ new_superstructure_goal->intake.gear_servo = 0.64;
+ }
+
new_superstructure_goal->intake.profile_params.max_velocity = 0.50;
new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 65b2246..293be55 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -13,6 +13,7 @@
#include "Counter.h"
#include "Encoder.h"
#include "VictorSP.h"
+#include "Servo.h"
#include "Relay.h"
#include "DriverStation.h"
#include "AnalogInput.h"
@@ -570,6 +571,9 @@
indexer_roller_victor_ = ::std::move(t);
}
+ void set_gear_servo(::std::unique_ptr<::frc::Servo> t) {
+ gear_servo_ = ::std::move(t);
+ }
void set_shooter_victor(::std::unique_ptr<VictorSP> t) {
shooter_victor_ = ::std::move(t);
}
@@ -615,6 +619,8 @@
red_light_->Set(queue->red_light_on);
green_light_->Set(queue->green_light_on);
blue_light_->Set(queue->blue_light_on);
+
+ gear_servo_->Set(queue->gear_servo);
}
virtual void Stop() override {
@@ -627,6 +633,8 @@
hood_victor_->SetDisabled();
shooter_victor_->SetDisabled();
+ gear_servo_->SetOffline();
+
red_light_->Set(true);
green_light_->Set(true);
blue_light_->Set(true);
@@ -636,6 +644,8 @@
indexer_victor_, indexer_roller_victor_, shooter_victor_,
turret_victor_, hood_victor_;
+ ::std::unique_ptr<::frc::Servo> gear_servo_;
+
::std::unique_ptr<DigitalOutput> red_light_, green_light_, blue_light_;
};
@@ -714,6 +724,9 @@
superstructure_writer.set_shooter_victor(
::std::unique_ptr<VictorSP>(new VictorSP(8)));
+ superstructure_writer.set_gear_servo(
+ ::std::unique_ptr<Servo>(new Servo(0)));
+
superstructure_writer.set_red_light(
::std::unique_ptr<DigitalOutput>(new DigitalOutput(5)));
superstructure_writer.set_green_light(