Expose drivetrain voltage limit.
The drivetrain control loop was assuming it could always use 12 volts.
That's not always a good idea (there are cases where we want to limit to
like 4 volts so we can tolerate stalling the drivetrain for extended
periods). Make it configurable over the goal queue!
Change-Id: Ia13542d0ce0523dae31f30e10ae44c1872e06d1e
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index abb91a7..4493e3c 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -27,6 +27,7 @@
void BaseAutonomousActor::ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
+ max_drivetrain_voltage_ = 12.0;
drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
.highgear(true)
@@ -36,6 +37,7 @@
.left_velocity_goal(0)
.right_goal(initial_drivetrain_.right)
.right_velocity_goal(0)
+ .max_ss_voltage(max_drivetrain_voltage_)
.Send();
}
@@ -64,6 +66,7 @@
drivetrain_message->left_velocity_goal = 0;
drivetrain_message->right_goal = initial_drivetrain_.right;
drivetrain_message->right_velocity_goal = 0;
+ drivetrain_message->max_ss_voltage = max_drivetrain_voltage_;
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
@@ -243,22 +246,29 @@
}
}
-bool BaseAutonomousActor::WaitForDriveProfileDone() {
+bool BaseAutonomousActor::WaitForDriveProfileNear(double tolerance) {
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
- constexpr double kProfileTolerance = 0.001;
-
while (true) {
if (ShouldCancel()) {
return false;
}
phased_loop.SleepUntilNext();
drivetrain_queue.status.FetchLatest();
+
+ const Eigen::Matrix<double, 7, 1> current_error =
+ (Eigen::Matrix<double, 7, 1>()
+ << initial_drivetrain_.left -
+ drivetrain_queue.status->profiled_left_position_goal,
+ 0.0, initial_drivetrain_.right -
+ drivetrain_queue.status->profiled_right_position_goal,
+ 0.0, 0.0, 0.0, 0.0)
+ .finished();
+ const Eigen::Matrix<double, 2, 1> linear_error =
+ dt_config_.LeftRightToLinear(current_error);
+
if (drivetrain_queue.status.get()) {
- if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
- initial_drivetrain_.left) < kProfileTolerance &&
- ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
- initial_drivetrain_.right) < kProfileTolerance) {
+ if (::std::abs(linear_error(0)) < tolerance) {
LOG(INFO, "Finished drive\n");
return true;
}
@@ -266,37 +276,46 @@
}
}
-bool BaseAutonomousActor::WaitForTurnProfileDone() {
+bool BaseAutonomousActor::WaitForDriveProfileDone() {
+ constexpr double kProfileTolerance = 0.001;
+ return WaitForDriveProfileNear(kProfileTolerance);
+}
+
+bool BaseAutonomousActor::WaitForTurnProfileNear(double tolerance) {
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
- constexpr double kProfileTolerance = 0.001;
-
while (true) {
if (ShouldCancel()) {
return false;
}
phased_loop.SleepUntilNext();
drivetrain_queue.status.FetchLatest();
+
+ const Eigen::Matrix<double, 7, 1> current_error =
+ (Eigen::Matrix<double, 7, 1>()
+ << initial_drivetrain_.left -
+ drivetrain_queue.status->profiled_left_position_goal,
+ 0.0, initial_drivetrain_.right -
+ drivetrain_queue.status->profiled_right_position_goal,
+ 0.0, 0.0, 0.0, 0.0)
+ .finished();
+ const Eigen::Matrix<double, 2, 1> angular_error =
+ dt_config_.LeftRightToAngular(current_error);
+
if (drivetrain_queue.status.get()) {
- const double left_profile_error =
- (initial_drivetrain_.left -
- drivetrain_queue.status->profiled_left_position_goal);
- const double right_profile_error =
- (initial_drivetrain_.right -
- drivetrain_queue.status->profiled_right_position_goal);
-
- const double profile_angle_to_go =
- (right_profile_error - left_profile_error) /
- (dt_config_.robot_radius * 2.0);
-
- if (::std::abs(profile_angle_to_go) < kProfileTolerance) {
- LOG(INFO, "Finished turn profile\n");
+ if (::std::abs(angular_error(0)) < tolerance) {
+ LOG(INFO, "Finished turn\n");
return true;
}
}
}
}
+bool BaseAutonomousActor::WaitForTurnProfileDone() {
+ constexpr double kProfileTolerance = 0.001;
+ return WaitForTurnProfileNear(kProfileTolerance);
+}
+
double BaseAutonomousActor::DriveDistanceLeft() {
using ::frc971::control_loops::drivetrain_queue;
drivetrain_queue.status.FetchLatest();
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index b8db4e5..980be2c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -44,8 +44,10 @@
// goal. Returns true on success, and false when canceled.
bool WaitForDriveNear(double distance, double angle);
+ bool WaitForDriveProfileNear(double tolerance);
bool WaitForDriveProfileDone();
+ bool WaitForTurnProfileNear(double tolerance);
bool WaitForTurnProfileDone();
// Returns the distance left to go.
@@ -59,6 +61,13 @@
double right;
};
InitialDrivetrain initial_drivetrain_;
+
+ void set_max_drivetrain_voltage(double max_drivetrain_voltage) {
+ max_drivetrain_voltage_ = max_drivetrain_voltage;
+ }
+
+ private:
+ double max_drivetrain_voltage_ = 12.0;
};
using AutonomousAction =
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index a756ad2..a1c60c0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -68,6 +68,8 @@
double left_velocity_goal;
double right_velocity_goal;
+ double max_ss_voltage;
+
// Motion profile parameters.
// The control loop will profile if these are all non-zero.
.frc971.ProfileParameters linear;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 4aeb74b..ed99034 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -1,24 +1,24 @@
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
+#include "aos/common/controls/polytope.h"
#include "aos/common/logging/matrix_logging.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
- output_was_capped_ =
- ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
+ output_was_capped_ = ::std::abs((*U)(0, 0)) > max_voltage_ ||
+ ::std::abs((*U)(1, 0)) > max_voltage_;
if (output_was_capped_) {
- *U *= 12.0 / kf_->U_uncapped().lpNorm<Eigen::Infinity>();
+ *U *= max_voltage_ / kf_->U_uncapped().lpNorm<Eigen::Infinity>();
}
}
@@ -27,8 +27,8 @@
// out-of-range voltages could lead to things like running out of CPU under
// certain situations, which would be bad.
void DrivetrainMotorsSS::PolyCapU(Eigen::Matrix<double, 2, 1> *U) {
- output_was_capped_ =
- ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
+ output_was_capped_ = ::std::abs((*U)(0, 0)) > max_voltage_ ||
+ ::std::abs((*U)(1, 0)) > max_voltage_;
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
@@ -57,10 +57,10 @@
U_poly_.static_H() * position_K * T_,
U_poly_.static_H() *
(-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
- U_poly_.static_k(),
+ (U_poly_.static_k() * max_voltage_),
(position_K * T_).inverse() *
::aos::controls::ShiftPoints<2, 4>(
- U_poly_.StaticVertices(),
+ (U_poly_.StaticVertices() * max_voltage_),
-velocity_K * velocity_error + U_integral - kf_->ff_U()));
Eigen::Matrix<double, 2, 1> adjusted_pos_error;
@@ -125,12 +125,12 @@
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
/*[*/ 0, -1 /*]]*/).finished(),
- (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
- /*[*/ 12 /*]*/,
- /*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished(),
- (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
- /*[*/ -12, 12, 12, -12 /*]*/).finished()),
+ (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]*/,
+ /*[*/ 1.0 /*]]*/).finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
+ /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/).finished()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
integrated_kf_heading_(integrated_kf_heading) {
@@ -144,6 +144,11 @@
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,
goal.right_velocity_goal, 0.0, 0.0, 0.0;
+ if (::std::abs(goal.max_ss_voltage) < 0.01) {
+ max_voltage_ = kMaxVoltage;
+ } else {
+ max_voltage_ = goal.max_ss_voltage;
+ }
use_profile_ =
!kf_->controller().Kff().isZero(0) &&
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index ce12b19..db255d7 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -48,6 +48,9 @@
double last_gyro_to_wheel_offset_ = 0;
+ constexpr static double kMaxVoltage = 12.0;
+ double max_voltage_ = kMaxVoltage;
+
// Reprsents +/- full power on each motor in U-space, aka the square from
// (-12, -12) to (12, 12).
const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;