Removed redundant turn_width.
Change-Id: I68d70930c3d25afa05d00e381b73cd34bdfe6f33
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index dda65bd..a2d9fdd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -37,7 +37,6 @@
double v; // Motor velocity constant.
double t; // Torque constant.
- double turn_width; // Robot turn width, in meters.
// Gear ratios, from wheel to motor shaft.
double high_gear_ratio;
double low_gear_ratio;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9d7e223..9d3e6c6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -47,7 +47,6 @@
::y2014::control_loops::drivetrain::kV,
::y2014::control_loops::drivetrain::kT,
- ::y2014::constants::GetValuesForTeam(971).turn_width,
::y2014::constants::GetValuesForTeam(971).high_gear_ratio,
::y2014::constants::GetValuesForTeam(971).low_gear_ratio,
::y2014::constants::GetValuesForTeam(971).left_drive,
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 9fdfe1c..19bbef1 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -139,8 +139,7 @@
}
void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * dt_config_.turn_width) / 2.0;
+ const double offset = (right - left) / 2.0 - gyro * dt_config_.robot_radius;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 4cb69ca..6c7427b 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -11,9 +11,10 @@
#include "aos/common/commonmath.h"
#include "aos/common/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/actors/drivetrain_actor.h"
#include "y2014/constants.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
namespace y2014 {
namespace actors {
@@ -29,7 +30,7 @@
const double yoffset = params.y_offset;
const double turn_offset =
- params.theta_offset * constants::GetValues().turn_width / 2.0;
+ params.theta_offset * control_loops::drivetrain::kRobotRadius;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -41,18 +42,18 @@
profile.set_maximum_acceleration(params.maximum_acceleration);
profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
- constants::GetValues().turn_width /
- 2.0);
+ turn_profile.set_maximum_acceleration(
+ params.maximum_turn_acceleration *
+ control_loops::drivetrain::kRobotRadius);
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- constants::GetValues().turn_width / 2.0);
+ control_loops::drivetrain::kRobotRadius);
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
::frc971::control_loops::drivetrain_queue.status.FetchLatest();
if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto& status = *::frc971::control_loops::drivetrain_queue.status;
+ const auto &status = *::frc971::control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) > 24) {
LOG(DEBUG, "spinning in place\n");
@@ -140,14 +141,16 @@
if (ShouldCancel()) return true;
const double kPositionThreshold = 0.05;
- const double left_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->filtered_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->filtered_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status->robot_speed);
+ const double left_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_left_position -
+ (left_goal_state(0, 0) + params.left_initial_position));
+ const double right_error =
+ ::std::abs(::frc971::control_loops::drivetrain_queue.status
+ ->filtered_right_position -
+ (right_goal_state(0, 0) + params.right_initial_position));
+ const double velocity_error = ::std::abs(
+ ::frc971::control_loops::drivetrain_queue.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
velocity_error < 0.2) {
break;
@@ -162,7 +165,7 @@
}
::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2014::actors::DrivetrainActionParams& params) {
+ const ::y2014::actors::DrivetrainActionParams ¶ms) {
return ::std::unique_ptr<DrivetrainAction>(
new DrivetrainAction(&::y2014::actors::drivetrain_action, params));
}
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index e954637..9c39f27 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -10,12 +10,13 @@
#include "aos/common/actions/actions.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2014/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/actors/shoot_actor.h"
#include "y2014/actors/drivetrain_actor.h"
+#include "y2014/actors/shoot_actor.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/shooter/shooter.q.h"
#include "y2014/queues/auto_mode.q.h"
#include "y2014/queues/hot_goal.q.h"
@@ -83,9 +84,9 @@
void StepDrive(double distance, double theta) {
double left_goal = (left_initial_position + distance -
- theta * constants::GetValues().turn_width / 2.0);
+ theta * control_loops::drivetrain::kRobotRadius);
double right_goal = (right_initial_position + distance +
- theta * constants::GetValues().turn_width / 2.0);
+ theta * control_loops::drivetrain::kRobotRadius);
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(true)
@@ -197,9 +198,9 @@
auto drivetrain_action = actors::MakeDrivetrainAction(params);
drivetrain_action->Start();
left_initial_position +=
- distance - theta * constants::GetValues().turn_width / 2.0;
+ distance - theta * control_loops::drivetrain::kRobotRadius;
right_initial_position +=
- distance + theta * constants::GetValues().turn_width / 2.0;
+ distance + theta * control_loops::drivetrain::kRobotRadius;
return ::std::move(drivetrain_action);
}
diff --git a/y2014/constants.cc b/y2014/constants.cc
index 8cf9076..ad6c3e6 100644
--- a/y2014/constants.cc
+++ b/y2014/constants.cc
@@ -45,8 +45,6 @@
const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15, 3.2, 0.2, 0.7};
const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80, 2.98, 0.2, 0.7};
-const double kRobotWidth = 25.0 / 100.0 * 2.54;
-
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -63,7 +61,6 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
- 0.5,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
@@ -100,7 +97,6 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
- kRobotWidth,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
@@ -146,7 +142,6 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
- kRobotWidth,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
diff --git a/y2014/constants.h b/y2014/constants.h
index a31e9c0..4ad06bc 100644
--- a/y2014/constants.h
+++ b/y2014/constants.h
@@ -38,8 +38,6 @@
ShifterHallEffect left_drive, right_drive;
bool clutch_transmission;
- double turn_width;
-
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index b4ef447..1e18b68 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -25,7 +25,7 @@
drivetrain::kMass, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
drivetrain::kR, drivetrain::kV, drivetrain::kT,
- constants::GetValues().turn_width, constants::GetValues().high_gear_ratio,
+ constants::GetValues().high_gear_ratio,
constants::GetValues().low_gear_ratio,
constants::GetValues().left_drive, constants::GetValues().right_drive};
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
index 94835c8..325f6e4 100644
--- a/y2016/actors/drivetrain_actor.cc
+++ b/y2016/actors/drivetrain_actor.cc
@@ -30,7 +30,7 @@
const double yoffset = params.y_offset;
const double turn_offset =
- params.theta_offset * constants::Values::kTurnWidth / 2.0;
+ params.theta_offset * control_loops::drivetrain::kRobotRadius;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -42,11 +42,11 @@
profile.set_maximum_acceleration(params.maximum_acceleration);
profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
- constants::Values::kTurnWidth /
- 2.0);
+ turn_profile.set_maximum_acceleration(
+ params.maximum_turn_acceleration *
+ control_loops::drivetrain::kRobotRadius);
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- constants::Values::kTurnWidth / 2.0);
+ control_loops::drivetrain::kRobotRadius);
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
index 56844e0..b315223 100644
--- a/y2016/autonomous/auto.cc
+++ b/y2016/autonomous/auto.cc
@@ -15,6 +15,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/actors/drivetrain_actor.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/queues/profile_params.q.h"
using ::aos::time::Time;
@@ -85,9 +86,9 @@
auto drivetrain_action = actors::MakeDrivetrainAction(params);
drivetrain_action->Start();
left_initial_position +=
- distance - theta * constants::Values::kTurnWidth / 2.0;
+ distance - theta * control_loops::drivetrain::kRobotRadius;
right_initial_position +=
- distance + theta * constants::Values::kTurnWidth / 2.0;
+ distance + theta * control_loops::drivetrain::kRobotRadius;
return ::std::move(drivetrain_action);
}
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 8af39a1..f641e98 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -29,7 +29,7 @@
const int Values::kZeroingSampleSize;
constexpr double Values::kDrivetrainEncoderRatio, Values::kLowGearRatio,
- Values::kHighGearRatio, Values::kTurnWidth, Values::kShooterEncoderRatio,
+ Values::kHighGearRatio, Values::kShooterEncoderRatio,
Values::kIntakeEncoderRatio, Values::kShoulderEncoderRatio,
Values::kWristEncoderRatio, Values::kIntakePotRatio,
Values::kShoulderPotRatio, Values::kWristPotRatio,
diff --git a/y2016/constants.h b/y2016/constants.h
index 3b84c44..e4ecf5e 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -37,8 +37,6 @@
static constexpr double kHighGearRatio =
14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0;
- static constexpr double kTurnWidth = 0.601; // Robot width.
-
// Ratios for our subsystems.
static constexpr double kShooterEncoderRatio = 1.0;
static constexpr double kIntakeEncoderRatio =
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 1b11690..2a2f36d 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -38,7 +38,6 @@
drivetrain::kV,
drivetrain::kT,
- constants::Values::kTurnWidth,
constants::Values::kHighGearRatio,
constants::Values::kLowGearRatio,
kThreeStateDriveShifter,
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index b5ce93c..cca7d66 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -592,8 +592,7 @@
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.lower);
- superstructure_plant_.InitializeRelativeWristPosition(
- constants::Values::kWristRange.lower);
+ superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)