got 2 balls in 2 goal auto working
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e6cc9a4..82f690d 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -160,17 +160,20 @@
}
::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
-SetDriveGoal(double distance, double maximum_velocity = 1.7) {
+SetDriveGoal(double distance, double maximum_velocity = 1.7, double theta = 0) {
LOG(INFO, "Driving to %f\n", distance);
auto drivetrain_action = actions::MakeDrivetrainAction();
drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->theta_offset = theta;
drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
drivetrain_action->Start();
- // Uncomment to make relative again.
- left_initial_position += distance;
- right_initial_position += distance;
+ left_initial_position +=
+ distance - theta * constants::GetValues().turn_width / 2.0;
+ right_initial_position +=
+ distance + theta * constants::GetValues().turn_width / 2. -
+ theta * constants::GetValues().turn_width / 2.00;
return ::std::move(drivetrain_action);
}
@@ -219,8 +222,9 @@
void HandleAuto() {
// The front of the robot is 1.854 meters from the wall
- const double kShootDistance = 3.15;
- const double kPickupDistance = 0.5;
+ static const double kShootDistance = 3.15;
+ static const double kPickupDistance = 0.5;
+ static const double kTurnAngle = 0.3;
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
ResetDrivetrain();
@@ -250,15 +254,29 @@
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
}
- LOG(INFO, "Shooting at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
+
+ {
+ if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
// Shoot.
+ LOG(INFO, "Shooting at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
Shoot();
time::SleepFor(time::Time::InSeconds(0.05));
{
if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, -kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
+ {
+ if (ShouldExitAuto()) return;
// Intake the new ball.
LOG(INFO, "Claw ready for intake at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
@@ -287,6 +305,13 @@
if (ShouldExitAuto()) return;
}
+ {
+ if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, -kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
LOG(INFO, "Shooting at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
// Shoot