redid driving in auto
diff --git a/aos/common/util/trapezoid_profile.h b/aos/common/util/trapezoid_profile.h
index 1051eec..92e3dbe 100644
--- a/aos/common/util/trapezoid_profile.h
+++ b/aos/common/util/trapezoid_profile.h
@@ -24,6 +24,15 @@
const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
double goal_velocity);
+ // Useful for preventing windup etc.
+ void MoveGoal(double dx) {
+ output_(0, 0) += dx;
+ }
+
+ void SetGoal(double x) {
+ output_(0, 0) = x;
+ }
+
void set_maximum_acceleration(double maximum_acceleration) {
maximum_acceleration_ = maximum_acceleration;
}
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 24a766c..07a2abb 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -6,6 +6,7 @@
#include "aos/common/util/phased_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
#include "frc971/actions/drivetrain_action.h"
#include "frc971/constants.h"
@@ -18,6 +19,8 @@
: actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
void DrivetrainAction::RunAction() {
+ static const auto K = constants::GetValues().make_drivetrain_loop().K();
+
const double yoffset = action_q_->goal->y_offset;
const double turn_offset =
action_q_->goal->theta_offset * constants::GetValues().turn_width / 2.0;
@@ -46,6 +49,56 @@
left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
+ control_loops::drivetrain.status.FetchLatest();
+ if (control_loops::drivetrain.status.get()) {
+ const auto &status = *control_loops::drivetrain.status;
+ if (::std::abs(status.uncapped_left_voltage -
+ status.uncapped_right_voltage) >
+ 24) {
+ // They're more than 24V apart, so stop moving forwards and let it deal
+ // with spinning first.
+ profile.SetGoal(
+ (status.filtered_left_position + status.filtered_right_position) /
+ 2.0);
+ } else {
+ static const double divisor = K(0, 1) + K(0, 3);
+ double dx_left, dx_right;
+ if (status.uncapped_left_voltage > 12.0) {
+ dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
+ } else if (status.uncapped_left_voltage < -12.0) {
+ dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
+ } else {
+ dx_left = 0;
+ }
+ if (status.uncapped_right_voltage > 12.0) {
+ dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
+ } else if (status.uncapped_right_voltage < -12.0) {
+ dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
+ } else {
+ dx_right = 0;
+ }
+ double dx;
+ if (dx_left == 0 && dx_right == 0) {
+ dx = 0;
+ } else if (dx_left != 0 && dx_right != 0 &&
+ ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
+ // Both saturating in opposite directions. Don't do anything.
+ dx = 0;
+ } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
+ dx = dx_left;
+ } else {
+ dx = dx_right;
+ }
+ if (dx != 0) {
+ LOG(DEBUG, "adjusting goal by %f\n", dx);
+ profile.MoveGoal(-dx);
+ }
+ }
+ } else {
+ // If we ever get here, that's bad and we should just give up
+ LOG(FATAL, "no drivetrain status!\n");
+ }
+
if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
break;
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 82f690d..ad8350e 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -247,7 +247,7 @@
{
if (ShouldExitAuto()) return;
// Drive to the goal.
- auto drivetrain_action = SetDriveGoal(-kShootDistance);
+ auto drivetrain_action = SetDriveGoal(-kShootDistance, 2.5);
time::SleepFor(time::Time::InSeconds(0.75));
PositionClawForShot();
LOG(INFO, "Waiting until drivetrain is finished\n");
@@ -281,7 +281,8 @@
LOG(INFO, "Claw ready for intake at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
PositionClawBackIntake();
- auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
+ auto drivetrain_action =
+ SetDriveGoal(kShootDistance + kPickupDistance, 2.5);
LOG(INFO, "Waiting until drivetrain is finished\n");
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
@@ -295,7 +296,8 @@
{
LOG(INFO, "Driving back at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
- auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
+ auto drivetrain_action =
+ SetDriveGoal(-(kShootDistance + kPickupDistance), 2.5);
time::SleepFor(time::Time::InSeconds(0.3));
if (ShouldExitAuto()) return;
PositionClawForShot();
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 299437e..e755f3d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,11 +41,16 @@
T_inverse = T.inverse();
}
+ bool output_was_capped() const {
+ return output_was_capped_;
+ }
+
private:
virtual void CapU() {
const Eigen::Matrix<double, 4, 1> error = R - X_hat;
- if (U(0, 0) > 12.0 || U(1, 0) > 12.0) {
+ if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+ output_was_capped_ = true;
LOG_MATRIX(DEBUG, "U at start", U);
Eigen::Matrix<double, 2, 2> position_K;
@@ -113,11 +118,14 @@
LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
U = velocity_K * velocity_error + position_K * T * adjusted_pos_error;
LOG_MATRIX(DEBUG, "U is now", U);
+ } else {
+ output_was_capped_ = false;
}
}
const ::aos::controls::HPolytope<2> U_Poly_;
Eigen::Matrix<double, 2, 2> T, T_inverse;
+ bool output_was_capped_;
};
DrivetrainMotorsSS()
@@ -130,8 +138,8 @@
raw_left_(0.0),
raw_right_(0.0),
control_loop_driving_(false) {
- // High gear on both.
- loop_->set_controller_index(3);
+ // Low gear on both.
+ loop_->set_controller_index(0);
}
void SetGoal(double left, double left_velocity, double right,
@@ -176,20 +184,24 @@
LOG_MATRIX(DEBUG, "E", E);
}
- double GetEstimatedRobotSpeed() {
+ double GetEstimatedRobotSpeed() const {
// lets just call the average of left and right velocities close enough
return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
}
- double GetEstimatedLeftEncoder() {
+ double GetEstimatedLeftEncoder() const {
return loop_->X_hat(0, 0);
}
- double GetEstimatedRightEncoder() {
+ double GetEstimatedRightEncoder() const {
return loop_->X_hat(2, 0);
}
- void SendMotors(Drivetrain::Output *output) {
+ bool OutputWasCapped() const {
+ return loop_->output_was_capped();
+ }
+
+ void SendMotors(Drivetrain::Output *output) const {
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -198,6 +210,8 @@
}
}
+ const LimitedDrivetrainLoop &loop() const { return *loop_; }
+
private:
::std::unique_ptr<LimitedDrivetrainLoop> loop_;
@@ -707,6 +721,9 @@
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->output_was_capped = dt_closedloop.OutputWasCapped();
+ status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
+ status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 32eb2fb..0774fbc 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -23,15 +23,15 @@
implements aos.control_loops.ControlLoop;
message Goal {
- float steering;
- float throttle;
+ double steering;
+ double throttle;
bool highgear;
bool quickturn;
bool control_loop_driving;
- float left_goal;
- float left_velocity_goal;
- float right_goal;
- float right_velocity_goal;
+ double left_goal;
+ double left_velocity_goal;
+ double right_goal;
+ double right_velocity_goal;
};
message Position {
@@ -43,17 +43,22 @@
};
message Output {
- float left_voltage;
- float right_voltage;
+ double left_voltage;
+ double right_voltage;
bool left_high;
bool right_high;
};
message Status {
- bool is_done;
double robot_speed;
double filtered_left_position;
double filtered_right_position;
+
+ double uncapped_left_voltage;
+ double uncapped_right_voltage;
+ bool output_was_capped;
+
+ bool is_done;
};
queue Goal goal;