redid driving in auto
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;