Made down offset a constant.
Change-Id: Ic0e3aa3867dbdce4037cbac23dc26999487c1489
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 70d47a6..f2b14a9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -225,7 +225,7 @@
status->right_voltage_error = kf_.X_hat(5, 0);
status->estimated_angular_velocity_error = kf_.X_hat(6, 0);
status->estimated_heading = integrated_kf_heading_;
- status->ground_angle = down_estimator_.X_hat(0, 0);
+ status->ground_angle = down_estimator_.X_hat(0, 0) + dt_config_.down_offset;
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index a0b6ac0..ea76ca9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -48,6 +48,8 @@
// Variable that holds the default gear ratio. We use this in ZeroOutputs().
// (ie. true means high gear is default).
bool default_high_gear;
+
+ double down_offset;
};
} // namespace drivetrain
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 8b76c3b..a4f5964 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -71,6 +71,7 @@
0.0, 0.3},
},
+ 0.0,
"practice",
};
break;
@@ -106,6 +107,7 @@
-1.040454, 1.5},
},
+ 0.0,
"competition",
};
break;
@@ -137,6 +139,7 @@
-0.005145, 1.3},
},
+ 0.011,
"practice",
};
break;
diff --git a/y2016/constants.h b/y2016/constants.h
index 7416a3f..c87559d 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -99,6 +99,7 @@
};
Wrist wrist;
+ const double down_error;
const char *vision_name;
};
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 2e64090..9aeb4c8 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -36,7 +36,8 @@
drivetrain::kLowGearRatio,
kThreeStateDriveShifter,
kThreeStateDriveShifter,
- true};
+ true,
+ constants::GetValues().down_error};
return kDrivetrainConfig;
};
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index dcc528f..8575fdf 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -66,7 +66,6 @@
const ButtonLocation kChevalDeFrise(3, 8);
const ButtonLocation kVisionAlign(3, 4);
-constexpr double kDownOffset = 0.011;
class Reader : public ::aos::input::JoystickInput {
public:
@@ -215,7 +214,7 @@
shoulder_goal_ = M_PI / 2.0 + 0.1;
wrist_goal_ = M_PI + 0.43;
if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+ wrist_goal_ += drivetrain_queue.status->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -224,7 +223,7 @@
shoulder_goal_ = M_PI / 2.0 + 0.1;
wrist_goal_ = M_PI + 0.41;
if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+ wrist_goal_ += drivetrain_queue.status->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -233,7 +232,7 @@
shoulder_goal_ = M_PI / 2.0 - 0.4;
wrist_goal_ = -0.62;
if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+ wrist_goal_ += drivetrain_queue.status->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;