drop shifter support from the drivetrain
Everything is just commented/preprocessored out so we can easily
re-enable it next year.
Change-Id: I4387856548954cf617a525b96a8ffa573c6417a1
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 5b994e0..352ec62 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -112,7 +112,7 @@
right_goal_state(0, 0) + action_q_->goal->right_initial_position);
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(true)
- .highgear(false)
+ //.highgear(false)
.left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
.right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
.left_velocity_goal(left_goal_state(1, 0))
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e340354..e1cfddc 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -48,7 +48,7 @@
LOG(INFO, "resetting the drivetrain\n");
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(false)
- .highgear(false)
+ //.highgear(false)
.steering(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
@@ -85,7 +85,7 @@
right_initial_position + driveTrainState(0, 0));
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(true)
- .highgear(false)
+ //.highgear(false)
.left_goal(left_initial_position - driveTrainState(0, 0))
.right_goal(right_initial_position + driveTrainState(0, 0))
.left_velocity_goal(-driveTrainState(1, 0))
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index db2d31e..218ab8c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -20,6 +20,10 @@
#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
+// A consistent way to mark code that goes away without shifters. It's still
+// here because we will have shifters again in the future.
+#define HAVE_SHIFTERS 0
+
using frc971::sensors::gyro_reading;
namespace frc971 {
@@ -401,6 +405,7 @@
stale_count_ = 0;
}
+#if HAVE_SHIFTERS
if (position) {
GearLogging gear_logging;
// Switch to the correct controller.
@@ -452,6 +457,9 @@
gear_logging.right_state = right_gear_;
LOG_STRUCT(DEBUG, "state", gear_logging);
}
+#else
+ (void) values;
+#endif
}
double FilterVelocity(double throttle) {
@@ -505,6 +513,7 @@
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
+#if HAVE_SHIFTERS
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
@@ -540,8 +549,15 @@
LOG_STRUCT(DEBUG, "currently", logging);
}
+#else
+ (void) values;
+#endif
+#if HAVE_SHIFTERS
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+#else
+ {
+#endif
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
loop_->B().inverse() *
@@ -597,6 +613,7 @@
// TODO(austin): Feed back?
loop_->mutable_X_hat() =
loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+#if HAVE_SHIFTERS
} else {
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
@@ -614,6 +631,7 @@
(R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-12.0, 12.0);
loop_->mutable_U() *= 12.0 / position_.battery_voltage;
+#endif
}
}
@@ -674,7 +692,9 @@
double wheel = goal->steering;
double throttle = goal->throttle;
bool quickturn = goal->quickturn;
+#if HAVE_SHIFTERS
bool highgear = goal->highgear;
+#endif
bool control_loop_driving = goal->control_loop_driving;
double left_goal = goal->left_goal;
@@ -694,7 +714,11 @@
}
}
dt_openloop.SetPosition(position);
+#if HAVE_SHIFTERS
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+#else
+ dt_openloop.SetGoal(wheel, throttle, quickturn, false);
+#endif
dt_openloop.Update();
if (control_loop_driving) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 0774fbc..45ced74 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -25,7 +25,7 @@
message Goal {
double steering;
double throttle;
- bool highgear;
+ //bool highgear;
bool quickturn;
bool control_loop_driving;
double left_goal;
@@ -37,8 +37,8 @@
message Position {
double left_encoder;
double right_encoder;
- double left_shifter_position;
- double right_shifter_position;
+ //double left_shifter_position;
+ //double right_shifter_position;
double battery_voltage;
};
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index c96734c..6d2f1d4 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -174,7 +174,7 @@
if (!drivetrain.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
- .highgear(is_high_gear_)
+ //.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
.control_loop_driving(is_control_loop_driving)
.left_goal(left_goal)
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index adec494..42d5882 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -97,13 +97,9 @@
message.Send();
}
- // TODO(austin): Calibrate the shifter constants again.
- // TODO(sensors): Hook up the new dog position sensors.
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(right_encoder_->GetRaw()))
.left_encoder(-drivetrain_translate(left_encoder_->GetRaw()))
- .left_shifter_position(0)
- .right_shifter_position(0)
.battery_voltage(ds->GetBatteryVoltage())
.Send();
@@ -117,8 +113,6 @@
void Quit() { run_ = false; }
private:
- ::std::unique_ptr<AnalogInput> auto_selector_analog_;
-
::std::unique_ptr<Encoder> left_encoder_;
::std::unique_ptr<Encoder> right_encoder_;