Adjusted indexer for the lower gearing.
Change-Id: Ia163de29c8c247f0490bdd26ff2f9ddf2c66bf09
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index d115246..5311810 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -266,6 +266,54 @@
}
}
+bool BaseAutonomousActor::WaitForTurnProfileDone() {
+ ::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();
+ 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");
+ return true;
+ }
+ }
+ }
+}
+
+double BaseAutonomousActor::DriveDistanceLeft() {
+ using ::frc971::control_loops::drivetrain_queue;
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ const double left_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->estimated_left_position);
+ const double right_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->estimated_right_position);
+
+ return (left_error + right_error) / 2.0;
+ } else {
+ return 0;
+ }
+}
+
::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
const AutonomousActionParams ¶ms) {
return ::std::unique_ptr<AutonomousAction>(
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 69e2e5a..b8db4e5 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -46,6 +46,11 @@
bool WaitForDriveProfileDone();
+ bool WaitForTurnProfileDone();
+
+ // Returns the distance left to go.
+ double DriveDistanceLeft();
+
const control_loops::drivetrain::DrivetrainConfig dt_config_;
// Initial drivetrain positions.
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 702e66e..53b856c 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -40,8 +40,8 @@
self.J_inner = 0.0269
self.J_outer = 0.0952
# Gear ratios for the inner and outer parts.
- self.G_inner = (12.0 / 48.0) * (18.0 / 36.0) * (12.0 / 84.0)
- self.G_outer = (12.0 / 48.0) * (18.0 / 36.0) * (24.0 / 420.0)
+ self.G_inner = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (12.0 / 84.0)
+ self.G_outer = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (24.0 / 420.0)
# Motor inertia in kg m^2
self.motor_inertia = 0.00001187
@@ -332,7 +332,8 @@
'IntegralIndexer', [integral_indexer], namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer', voltage_error_noise=1.5)
+ stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer',
+ voltage_error_noise=1.5)
stuck_integral_loop_writer = control_loop.ControlLoopWriter(
'StuckIntegralIndexer', [stuck_integral_indexer], namespaces=namespaces)
stuck_integral_loop_writer.Write(argv[5], argv[6])
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 9ee07b5..346a5d5 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -335,7 +335,7 @@
}
}
bool ColumnProfiledSubsystem::IsIndexerStuck() const {
- return IndexerStuckVoltage() > 6.0;
+ return IndexerStuckVoltage() > 4.0;
}
void ColumnProfiledSubsystem::PartialIndexerReset() {
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index d5ae2e0..659f11c 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -436,7 +436,7 @@
CopyPosition(indexer_counter_, &superstructure_message->column.indexer,
Values::kIndexerEncoderCountsPerRevolution,
- Values::kIndexerEncoderRatio, false);
+ Values::kIndexerEncoderRatio, true);
superstructure_message->theta_shooter =
encoder_translate(shooter_encoder_->GetRaw(),