Fixed auto so that it goes straight.
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 50baa1f..8a4557a 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -50,6 +50,34 @@
.right_velocity_goal(profile_goal_state(1, 0))
.Send();
}
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain status packet, trying to fetch again\n");
+ control_loops::drivetrain.status.FetchNextBlocking();
+ if (ShouldCancel()) return;
+ }
+ while (true) {
+ if (ShouldCancel()) return;
+ const double kPositionThreshold = 0.05;
+
+ const double left_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_left_position -
+ (profile_goal_state(0, 0) + action_q_->goal->left_initial_position));
+ const double right_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_right_position -
+ (profile_goal_state(0, 0) + action_q_->goal->right_initial_position));
+ const double velocity_error =
+ ::std::abs(control_loops::drivetrain.status->robot_speed);
+ if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
+ velocity_error < 0.2) {
+ break;
+ } else {
+ LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
+ velocity_error);
+ }
+ control_loops::drivetrain.status.FetchNextBlocking();
+ }
LOG(INFO, "Done moving\n");
}
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
index 5797378..f61996f 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actions/drivetrain_action.q
@@ -6,9 +6,7 @@
implements frc971.actions.ActionQueueGroup;
message Goal {
- // If true, run this action. If false, cancel the action if it is
- // currently running.
- bool run;
+ uint32_t run;
double left_initial_position;
double right_initial_position;
double y_offset;
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 80ffa17..e6cc9a4 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -175,16 +175,16 @@
}
void InitializeEncoders() {
- control_loops::drivetrain.position.FetchLatest();
- while (!control_loops::drivetrain.position.get()) {
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
LOG(WARNING,
"No previous drivetrain position packet, trying to fetch again\n");
- control_loops::drivetrain.position.FetchNextBlocking();
+ control_loops::drivetrain.status.FetchNextBlocking();
}
left_initial_position =
- control_loops::drivetrain.position->left_encoder;
+ control_loops::drivetrain.status->filtered_left_position;
right_initial_position =
- control_loops::drivetrain.position->right_encoder;
+ control_loops::drivetrain.status->filtered_right_position;
}
@@ -221,6 +221,7 @@
// The front of the robot is 1.854 meters from the wall
const double kShootDistance = 3.15;
const double kPickupDistance = 0.5;
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
ResetDrivetrain();
@@ -228,14 +229,16 @@
InitializeEncoders();
// Turn the claw on, keep it straight up until the ball has been grabbed.
- LOG(INFO, "Claw going up\n");
+ LOG(INFO, "Claw going up at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
PositionClawVertically(12.0, 4.0);
SetShotPower(115.0);
// Wait for the ball to enter the claw.
time::SleepFor(time::Time::InSeconds(0.25));
if (ShouldExitAuto()) return;
- LOG(INFO, "Readying claw for shot\n");
+ LOG(INFO, "Readying claw for shot at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
{
if (ShouldExitAuto()) return;
@@ -247,31 +250,35 @@
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
}
- LOG(INFO, "Shooting\n");
+ LOG(INFO, "Shooting at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
// Shoot.
Shoot();
- time::SleepFor(time::Time::InSeconds(0.1));
+ time::SleepFor(time::Time::InSeconds(0.05));
{
if (ShouldExitAuto()) return;
// Intake the new ball.
- LOG(INFO, "Claw ready for intake\n");
+ LOG(INFO, "Claw ready for intake at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
PositionClawBackIntake();
auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
LOG(INFO, "Waiting until drivetrain is finished\n");
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
- LOG(INFO, "Wait for the claw.\n");
+ LOG(INFO, "Wait for the claw at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
WaitUntilClawDone();
if (ShouldExitAuto()) return;
}
// Drive back.
{
- LOG(INFO, "Driving back\n");
+ LOG(INFO, "Driving back at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
- time::SleepFor(time::Time::InSeconds(0.7));
+ time::SleepFor(time::Time::InSeconds(0.3));
if (ShouldExitAuto()) return;
PositionClawForShot();
LOG(INFO, "Waiting until drivetrain is finished\n");
@@ -280,12 +287,14 @@
if (ShouldExitAuto()) return;
}
- LOG(INFO, "Shooting\n");
+ LOG(INFO, "Shooting at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
// Shoot
Shoot();
if (ShouldExitAuto()) return;
// Get ready to zero when we come back up.
+ time::SleepFor(time::Time::InSeconds(0.05));
PositionClawVertically(0.0, 0.0);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7f50da8..168f87f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -614,6 +614,8 @@
}
status->is_done = done;
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
+ status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index a3706bc..32eb2fb 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -52,6 +52,8 @@
message Status {
bool is_done;
double robot_speed;
+ double filtered_left_position;
+ double filtered_right_position;
};
queue Goal goal;