Add climber framework.
Change-Id: I807de18bb204f4da921332b2074e4d343d3fd8dd
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 7871732..fb54a95 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -19,6 +19,7 @@
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeTopVoltage = 12.0;
constexpr float kMaxIntakeBottomVoltage = 12.0;
+constexpr float kMaxClimberVoltage = 12.0;
// Aliases to reduce typing.
constexpr double kIntakeEncoderIndexDifference =
@@ -81,15 +82,15 @@
if (wrist_angle > kMaxWristAngleForMovingByIntake ||
wrist_angle < kMinWristAngleForMovingByIntake) {
shoulder_angle_goal =
- ::std::max(original_shoulder_angle_goal,
- kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ ::std::max(original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeInterference + kSafetyMargin);
}
} else {
if (wrist_angle > kMaxWristAngleForMovingByIntake ||
wrist_angle < kMinWristAngleForMovingByIntake) {
- shoulder_angle_goal =
- ::std::max(original_shoulder_angle_goal,
- kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+ shoulder_angle_goal = ::std::max(
+ original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
}
}
if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
@@ -173,8 +174,7 @@
CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
shoulder_angle <=
CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference &&
- intake_angle >
- CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
+ intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n",
intake_angle, CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
@@ -610,7 +610,21 @@
const double max_voltage = (state_ == RUNNING || state_ == LANDING_RUNNING)
? kOperatingVoltage
: kZeroingVoltage;
- arm_.set_max_voltage(max_voltage, max_voltage);
+ if (unsafe_goal) {
+ constexpr float kTriggerThreshold = 12.0 * 0.25 / 0.005;
+
+ if (unsafe_goal->voltage_climber > 1.0) {
+ kill_shoulder_accumulator_ +=
+ ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber);
+ } else {
+ kill_shoulder_accumulator_ = 0.0;
+ }
+
+ if (kill_shoulder_accumulator_ > kTriggerThreshold) {
+ kill_shoulder_ = true;
+ }
+ }
+ arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage, max_voltage);
intake_.set_max_voltage(max_voltage);
if (IsRunning()) {
@@ -650,15 +664,26 @@
output->voltage_shoulder = arm_.shoulder_voltage();
output->voltage_wrist = arm_.wrist_voltage();
- // Logic to run our rollers on the intake.
output->voltage_top_rollers = 0.0;
output->voltage_bottom_rollers = 0.0;
+
+ output->voltage_climber = 0.0;
+ output->unfold_climber = false;
if (unsafe_goal) {
+ // Ball detector lights.
::y2016::sensors::ball_detector.FetchAnother();
bool ball_detected = false;
if (::y2016::sensors::ball_detector.get()) {
ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
}
+
+ // Climber.
+ output->voltage_climber = ::std::max(
+ static_cast<float>(0.0),
+ ::std::min(unsafe_goal->voltage_climber, kMaxClimberVoltage));
+ output->unfold_climber = unsafe_goal->unfold_climber;
+
+ // Intake.
if (unsafe_goal->force_intake || !ball_detected) {
output->voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
@@ -671,6 +696,8 @@
output->voltage_top_rollers = 0.0;
output->voltage_bottom_rollers = 0.0;
}
+
+ // Traverse.
output->traverse_unlatched = unsafe_goal->traverse_unlatched;
output->traverse_down = unsafe_goal->traverse_down;
}
@@ -709,7 +736,8 @@
status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
status->intake.unprofiled_goal_angular_velocity =
intake_.unprofiled_goal(1, 0);
- status->intake.calculated_velocity = (intake_.angle() - last_intake_angle_) / 0.005;
+ status->intake.calculated_velocity =
+ (intake_.angle() - last_intake_angle_) / 0.005;
status->intake.voltage_error = intake_.X_hat(2, 0);
status->intake.estimator_state = intake_.IntakeEstimatorState();
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 9cd570c..4e76a2b 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -232,6 +232,9 @@
float last_wrist_angle_ = 0.0;
float last_intake_angle_ = 0.0;
+ double kill_shoulder_accumulator_ = 0.0;
+ bool kill_shoulder_ = false;
+
// Returns true if the profile has finished, and the joint is within the
// specified tolerance.
bool IsArmNear(double tolerance);
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 2919398..842beb7 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -65,6 +65,11 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ float voltage_climber;
+ // If true, unlatch the climber and allow it to unfold.
+ bool unfold_climber;
+
bool force_intake;
// If true, release the latch which holds the traverse mechanism in the
@@ -118,6 +123,11 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ float voltage_climber;
+ // If true, release the latch to trigger the climber to unfold.
+ bool unfold_climber;
+
// If true, release the latch to hold the traverse mechanism in the middle.
bool traverse_unlatched;
// If true, fire the traverse mechanism down.
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index b3ed0a8..acf820f 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -428,14 +428,14 @@
compressor_ = ::std::move(compressor);
}
- void set_drivetrain_left(
+ void set_drivetrain_shifter(
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- drivetrain_left_ = ::std::move(s);
+ drivetrain_shifter_ = ::std::move(s);
}
- void set_drivetrain_right(
+ void set_climber_trigger(
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- drivetrain_right_ = ::std::move(s);
+ climber_trigger_ = ::std::move(s);
}
void set_traverse(
@@ -487,8 +487,8 @@
drivetrain_.FetchLatest();
if (drivetrain_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(!drivetrain_->left_high);
- drivetrain_right_->Set(!drivetrain_->right_high);
+ drivetrain_shifter_->Set(
+ !(drivetrain_->left_high || drivetrain_->right_high));
}
}
@@ -518,7 +518,10 @@
{
superstructure_.FetchLatest();
if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+
+ climber_trigger_->Set(superstructure_->unfold_climber);
+
traverse_->Set(superstructure_->traverse_down);
traverse_latch_->Set(superstructure_->traverse_unlatched);
}
@@ -542,9 +545,9 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
- drivetrain_right_, shooter_clamp_, shooter_pusher_, lights_, traverse_,
- traverse_latch_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_,
+ shooter_clamp_, shooter_pusher_, lights_, traverse_, traverse_latch_,
+ climber_trigger_;
::std::unique_ptr<Relay> flashlight_;
::std::unique_ptr<Compressor> compressor_;
@@ -642,6 +645,10 @@
bottom_rollers_talon_ = ::std::move(t);
}
+ void set_climber_talon(::std::unique_ptr<Talon> t) {
+ climber_talon_ = ::std::move(t);
+ }
+
private:
virtual void Read() override {
::y2016::control_loops::superstructure_queue.output.FetchAnother();
@@ -661,6 +668,7 @@
12.0);
top_rollers_talon_->Set(-queue->voltage_top_rollers / 12.0);
bottom_rollers_talon_->Set(-queue->voltage_bottom_rollers / 12.0);
+ climber_talon_->Set(-queue->voltage_climber / 12.0);
}
virtual void Stop() override {
@@ -671,7 +679,7 @@
}
::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_,
- top_rollers_talon_, bottom_rollers_talon_;
+ top_rollers_talon_, bottom_rollers_talon_, climber_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -754,19 +762,21 @@
::std::unique_ptr<Talon>(new Talon(1)));
superstructure_writer.set_bottom_rollers_talon(
::std::unique_ptr<Talon>(new Talon(0)));
+ superstructure_writer.set_climber_talon(
+ ::std::unique_ptr<Talon>(new Talon(7)));
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(1));
- solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(0));
+ solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(4));
solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(5));
solenoid_writer.set_lights(pcm->MakeSolenoid(6));
+ solenoid_writer.set_climber_trigger(pcm->MakeSolenoid(1));
solenoid_writer.set_flashlight(make_unique<Relay>(0));
solenoid_writer.set_compressor(make_unique<Compressor>());