Merge changes Ifd9fb09b,Iddc5d76b
* changes:
Deploy www directory when deploying code and fix vision.
Add ball detector indicator webserver.
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 50bab3b..d547c7b 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -183,35 +183,107 @@
}
}
-bool AutonomousActor::WaitForDriveDone() {
+bool AutonomousActor::WaitForMaxBy(double angle) {
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(5) / 2);
+ double max_angle = -M_PI;
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle > max_angle) {
+ max_angle = drivetrain_queue.status->ground_angle;
+ }
+ if (drivetrain_queue.status->ground_angle < max_angle - angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool AutonomousActor::WaitForAboveAngle(double angle) {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle > angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool AutonomousActor::WaitForBelowAngle(double angle) {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (IsDriveDone()) {
+ return true;
+ }
+ if (drivetrain_queue.status.get()) {
+ if (drivetrain_queue.status->ground_angle < angle) {
+ return true;
+ }
+ }
+ }
+}
+
+bool AutonomousActor::IsDriveDone() {
constexpr double kPositionTolerance = 0.02;
constexpr double kVelocityTolerance = 0.10;
constexpr double kProfileTolerance = 0.001;
+ if (drivetrain_queue.status.get()) {
+ if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
+ initial_drivetrain_.left) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
+ initial_drivetrain_.right) < kProfileTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_left_position -
+ initial_drivetrain_.left) < kPositionTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_right_position -
+ initial_drivetrain_.right) < kPositionTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_left_velocity) <
+ kVelocityTolerance &&
+ ::std::abs(drivetrain_queue.status->estimated_right_velocity) <
+ kVelocityTolerance) {
+ LOG(INFO, "Finished drive\n");
+ return true;
+ }
+ }
+ return false;
+}
+
+bool AutonomousActor::WaitForDriveDone() {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+
while (true) {
if (ShouldCancel()) {
return false;
}
phased_loop.SleepUntilNext();
drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
- initial_drivetrain_.left) < kProfileTolerance &&
- ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
- initial_drivetrain_.right) < kProfileTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_left_position -
- initial_drivetrain_.left) < kPositionTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_right_position -
- initial_drivetrain_.right) < kPositionTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_left_velocity) <
- kVelocityTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_right_velocity) <
- kVelocityTolerance) {
- LOG(INFO, "Finished drive\n");
- return true;
- }
+ if (IsDriveDone()) {
+ return true;
}
}
}
@@ -404,7 +476,7 @@
} else {
ready_to_fire = 0;
}
- if (ready_to_fire > 9) {
+ if (ready_to_fire > 15) {
break;
LOG(INFO, "Vision align success!\n");
}
@@ -550,28 +622,31 @@
{10.0, 25.0}, false, 0.0);
}
+void AutonomousActor::FrontLongShot() {
+ LOG(INFO, "Expanding for front long shot\n");
+ MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02, {7.0, 40.0},
+ {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
+}
+
+void AutonomousActor::FrontMiddleShot() {
+ LOG(INFO, "Expanding for front middle shot\n");
+ MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.42, {7.0, 40.0},
+ {4.0, 10.0}, {10.0, 25.0}, true, 0.0);
+}
+
void AutonomousActor::TuckArm(bool low_bar, bool traverse_down) {
MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0, {7.0, 40.0},
{4.0, 10.0}, {10.0, 25.0}, !traverse_down, 0.0);
}
-void AutonomousActor::DoFullShot(bool center) {
- // Get the superstructure to unfold and get ready for shooting.
- LOG(INFO, "Unfolding superstructure\n");
- if (center) {
- BackMiddleShot();
- } else {
- BackLongShot();
- }
-
- // Spin up the shooter wheels.
- LOG(INFO, "Spinning up the shooter wheels\n");
- SetShooterSpeed(640.0);
-
+void AutonomousActor::DoFullShot() {
if (ShouldCancel()) return;
// Make sure that the base is aligned with the base.
LOG(INFO, "Waiting for the superstructure\n");
WaitForSuperstructure();
+
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(1.5));
+
if (ShouldCancel()) return;
LOG(INFO, "Triggering the vision actor\n");
AlignWithVisionGoal();
@@ -585,6 +660,7 @@
WaitForShooterSpeed();
if (ShouldCancel()) return;
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(1.0));
LOG(INFO, "Shoot!\n");
Shoot();
@@ -597,7 +673,7 @@
// Wait for everything to be folded up.
LOG(INFO, "Waiting for superstructure to be folded back down\n");
- WaitForSuperstructure();
+ WaitForSuperstructureLow();
}
void AutonomousActor::LowBarDrive() {
@@ -620,43 +696,54 @@
StartDrive(0, -M_PI / 4.0 - 0.2, kLowBarDrive, kSlowTurn);
}
+void AutonomousActor::TippyDrive(double goal_distance, double tip_distance,
+ double below, double above) {
+ StartDrive(goal_distance, 0.0, kMoatDrive, kSlowTurn);
+ if (!WaitForBelowAngle(below)) return;
+ if (!WaitForAboveAngle(above)) return;
+ // Ok, we are good now. Compensate by moving the goal by the error.
+ // Should be here at 2.7
+ 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);
+ const double distance_to_go = (left_error + right_error) / 2.0;
+ const double distance_compensation =
+ goal_distance - tip_distance - distance_to_go;
+ LOG(INFO, "Going %f further at the bump\n", distance_compensation);
+ StartDrive(distance_compensation, 0.0, kMoatDrive, kSlowTurn);
+ }
+}
+
void AutonomousActor::MiddleDrive() {
TuckArm(false, false);
- StartDrive(4.05, 0.0, kMoatDrive, kSlowTurn);
+ TippyDrive(3.65, 2.7, -0.2, 0.0);
if (!WaitForDriveDone()) return;
- StartDrive(0.0, M_PI, kMoatDrive, kFastTurn);
}
void AutonomousActor::OneFromMiddleDrive(bool left) {
- const double kTurnAngle = left ? M_PI / 2.0 - 0.40 : (-M_PI / 2.0 + 0.40);
- const double kFlipTurnAngle =
- left ? (M_PI - kTurnAngle) : (-M_PI - kTurnAngle);
+ const double kTurnAngle = left ? -0.41 : 0.41;
TuckArm(false, false);
- StartDrive(5.0 - kDistanceShort, 0.0, kMoatDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(0.0, kTurnAngle, kRealignDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(-1.3, 0.0, kRealignDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(0.0, kFlipTurnAngle, kRealignDrive, kFastTurn);
+ TippyDrive(4.05, 2.7, -0.2, 0.0);
if (!WaitForDriveDone()) return;
- StartDrive(0.3, 0.0, kRealignDrive, kFastTurn);
+ StartDrive(0.0, kTurnAngle, kRealignDrive, kFastTurn);
}
void AutonomousActor::TwoFromMiddleDrive() {
- const double kTurnAngle = M_PI / 2.0 - 0.13;
TuckArm(false, false);
- StartDrive(5.0 - kDistanceShort, 0.0, kMoatDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(0.0, kTurnAngle, kMoatDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(-2.6, 0.0, kRealignDrive, kFastTurn);
- if (!WaitForDriveDone()) return;
- StartDrive(0.0, M_PI - kTurnAngle, kMoatDrive, kFastTurn);
+ constexpr double kDriveDistance = 5.10;
+ TippyDrive(kDriveDistance, 2.7, -0.2, 0.0);
+
+ if (!WaitForDriveNear(kDriveDistance - 3.0, 2.0)) return;
+ StartDrive(0, -M_PI / 2 - 0.10, kMoatDrive, kFastTurn);
if (!WaitForDriveDone()) return;
- StartDrive(0.3, 0.0, kRealignDrive, kFastTurn);
+ StartDrive(0, M_PI / 3 + 0.35, kMoatDrive, kFastTurn);
}
void AutonomousActor::CloseIfBall() {
@@ -682,204 +769,251 @@
}
}
+void AutonomousActor::TwoBallAuto() {
+ aos::time::Time start_time = aos::time::Time::Now();
+ OpenShooter();
+ MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
+ false, 12.0);
+ if (ShouldCancel()) return;
+ LOG(INFO, "Waiting for the intake to come down.\n");
+
+ WaitForIntake();
+ LOG(INFO, "Intake done at %f seconds, starting to drive\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ if (ShouldCancel()) return;
+ const double kDriveDistance = 5.05;
+ StartDrive(-kDriveDistance, 0.0, kTwoBallLowDrive, kSlowTurn);
+
+ StartDrive(0.0, 0.4, kTwoBallLowDrive, kSwerveTurn);
+ if (!WaitForDriveNear(kDriveDistance - 0.5, 0.0)) return;
+
+ MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
+ false, 0.0);
+ LOG(INFO, "Shutting off rollers at %f seconds, starting to straighten out\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
+ MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
+ false, 0.0);
+ CloseShooter();
+ if (!WaitForDriveNear(kDriveDistance - 2.4, 0.0)) return;
+
+ // We are now under the low bar. Start lifting.
+ BackLongShotLowBarTwoBall();
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(640.0);
+ StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);
+
+ if (!WaitForDriveNear(1.50, 0.0)) return;
+ constexpr double kShootTurnAngle = -M_PI / 4.0 + 0.05;
+ StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
+ BackLongShotTwoBall();
+
+ if (!WaitForDriveDone()) return;
+ LOG(INFO, "First shot done driving at %f seconds\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+
+ WaitForSuperstructure();
+
+ if (ShouldCancel()) return;
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
+ AlignWithVisionGoal();
+
+ WaitForShooterSpeed();
+ if (ShouldCancel()) return;
+
+ WaitForAlignedWithVision(aos::time::Time::InSeconds(0.2));
+ LOG(INFO, "Shoot!\n");
+ Shoot();
+
+ LOG(INFO, "First shot at %f seconds\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ if (ShouldCancel()) return;
+
+ SetShooterSpeed(0.0);
+ LOG(INFO, "Folding superstructure back down\n");
+ TuckArm(true, true);
+
+ // Undo vision move.
+ StartDrive(0.0, 0.0, kTwoBallFastDrive, kFinishTurn);
+ if (!WaitForDriveDone()) return;
+
+ constexpr double kBackDrive = 3.09 - 0.4;
+ StartDrive(kBackDrive, 0.0, kTwoBallReturnDrive, kSlowTurn);
+ if (!WaitForDriveNear(kBackDrive - 0.19, 0.0)) return;
+ StartDrive(0, -kShootTurnAngle, kTwoBallReturnDrive, kSwerveTurn);
+
+ if (!WaitForDriveNear(0.06, 0.0)) return;
+ LOG(INFO, "At Low Bar %f\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+
+ OpenShooter();
+ constexpr double kSecondBallAfterBarDrive = 2.10;
+ StartDrive(kSecondBallAfterBarDrive, 0.0, kTwoBallBallPickup, kSlowTurn);
+ if (!WaitForDriveNear(kSecondBallAfterBarDrive - 0.5, 0.0)) return;
+ constexpr double kBallSmallWallTurn = -0.11;
+ StartDrive(0, kBallSmallWallTurn, kTwoBallBallPickup, kFinishTurn);
+
+ MoveSuperstructure(0.03, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
+ false, 12.0);
+
+ if (!WaitForDriveProfileDone()) return;
+
+ MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
+ false, 12.0);
+
+ LOG(INFO, "Done backing up %f\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+
+ constexpr double kDriveBackDistance = 5.15 - 0.4;
+ StartDrive(-kDriveBackDistance, 0.0, kTwoBallLowDrive, kFinishTurn);
+ CloseIfBall();
+ if (!WaitForDriveNear(kDriveBackDistance - 0.75, 0.0)) return;
+
+ StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
+ LOG(INFO, "Straightening up at %f\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+
+ CloseIfBall();
+ if (!WaitForDriveNear(kDriveBackDistance - 2.3, 0.0)) return;
+
+ ::y2016::sensors::ball_detector.FetchLatest();
+ if (::y2016::sensors::ball_detector.get()) {
+ const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ if (!ball_detected) {
+ if (!WaitForDriveDone()) return;
+ LOG(INFO, "Aborting, no ball %f\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ return;
+ }
+ }
+ CloseShooter();
+
+ BackLongShotLowBarTwoBall();
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(640.0);
+ StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);
+
+ if (!WaitForDriveNear(1.80, 0.0)) return;
+ StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
+ BackLongShotTwoBall();
+
+ if (!WaitForDriveDone()) return;
+ LOG(INFO, "Second shot done driving at %f seconds\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ WaitForSuperstructure();
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
+ AlignWithVisionGoal();
+ if (ShouldCancel()) return;
+
+ WaitForShooterSpeed();
+ if (ShouldCancel()) return;
+
+ // 2.2 with 0.4 of vision.
+ // 1.8 without any vision.
+ LOG(INFO, "Going to vision align at %f\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ WaitForAlignedWithVision(start_time + aos::time::Time::InSeconds(13.1) -
+ aos::time::Time::Now());
+ LOG(INFO, "Shoot at %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
+ Shoot();
+
+ LOG(INFO, "Second shot at %f seconds\n",
+ (aos::time::Time::Now() - start_time).ToSeconds());
+ if (ShouldCancel()) return;
+
+ SetShooterSpeed(0.0);
+ LOG(INFO, "Folding superstructure back down\n");
+ TuckArm(true, false);
+ LOG(INFO, "Shot %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
+
+ WaitForSuperstructureLow();
+
+ LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
+}
+
bool AutonomousActor::RunAction(const actors::AutonomousActionParams ¶ms) {
+ aos::time::Time start_time = aos::time::Time::Now();
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
InitializeEncoders();
ResetDrivetrain();
- switch (params.mode) {
+ switch (1) {
case 0:
LowBarDrive();
+ if (!WaitForDriveDone()) return true;
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ FrontLongShot();
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(640.0);
+
break;
case 1:
TwoFromMiddleDrive();
+ if (!WaitForDriveDone()) return true;
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ FrontMiddleShot();
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(600.0);
+
break;
case 2:
OneFromMiddleDrive(true);
+ if (!WaitForDriveDone()) return true;
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ FrontMiddleShot();
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(600.0);
+
break;
case 3:
MiddleDrive();
+ if (!WaitForDriveDone()) return true;
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ FrontMiddleShot();
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(600.0);
+
break;
case 4:
OneFromMiddleDrive(false);
+ if (!WaitForDriveDone()) return true;
+ // Get the superstructure to unfold and get ready for shooting.
+ LOG(INFO, "Unfolding superstructure\n");
+ FrontMiddleShot();
+
+ // Spin up the shooter wheels.
+ LOG(INFO, "Spinning up the shooter wheels\n");
+ SetShooterSpeed(600.0);
+
break;
- case 5: {
- aos::time::Time start_time = aos::time::Time::Now();
- OpenShooter();
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0},
- {4.0, 10.0}, {10.0, 25.0}, false, 12.0);
- if (ShouldCancel()) return true;
- LOG(INFO, "Waiting for the intake to come down.\n");
-
- WaitForIntake();
- LOG(INFO, "Intake done at %f seconds, starting to drive\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- if (ShouldCancel()) return true;
- const double kDriveDistance = 5.05;
- StartDrive(-kDriveDistance, 0.0, kTwoBallLowDrive, kSlowTurn);
-
- StartDrive(0.0, 0.4, kTwoBallLowDrive, kSwerveTurn);
- if (!WaitForDriveNear(kDriveDistance - 0.5, 0.0)) return true;
-
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0},
- {10.0, 25.0}, false, 0.0);
- LOG(INFO, "Shutting off rollers at %f seconds, starting to straighten out\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
- MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0},
- {10.0, 25.0}, false, 0.0);
- CloseShooter();
- if (!WaitForDriveNear(kDriveDistance - 2.4, 0.0)) return true;
-
- // We are now under the low bar. Start lifting.
- BackLongShotLowBarTwoBall();
- LOG(INFO, "Spinning up the shooter wheels\n");
- SetShooterSpeed(640.0);
- StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);
-
- if (!WaitForDriveNear(1.50, 0.0)) return true;
- constexpr double kShootTurnAngle = -M_PI / 4.0 - 0.05;
- StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
- BackLongShotTwoBall();
-
- if (!WaitForDriveDone()) return true;
- LOG(INFO, "First shot done driving at %f seconds\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
- WaitForSuperstructure();
-
- if (ShouldCancel()) return true;
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
- AlignWithVisionGoal();
-
- WaitForShooterSpeed();
- if (ShouldCancel()) return true;
-
- WaitForAlignedWithVision(aos::time::Time::InSeconds(0.2));
- LOG(INFO, "Shoot!\n");
- Shoot();
-
- LOG(INFO, "First shot at %f seconds\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- if (ShouldCancel()) return true;
-
- SetShooterSpeed(0.0);
- LOG(INFO, "Folding superstructure back down\n");
- TuckArm(true, true);
-
- // Undo vision move.
- StartDrive(0.0, 0.0, kTwoBallFastDrive, kFinishTurn);
- if (!WaitForDriveDone()) return true;
-
- constexpr double kBackDrive = 3.09 - 0.4;
- StartDrive(kBackDrive, 0.0, kTwoBallReturnDrive, kSlowTurn);
- if (!WaitForDriveNear(kBackDrive - 0.16, 0.0)) return true;
- StartDrive(0, -kShootTurnAngle, kTwoBallReturnDrive, kSwerveTurn);
-
- if (!WaitForDriveNear(0.06, 0.0)) return true;
- LOG(INFO, "At Low Bar %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
- OpenShooter();
- constexpr double kSecondBallAfterBarDrive = 2.10;
- StartDrive(kSecondBallAfterBarDrive, 0.0, kTwoBallBallPickup, kSlowTurn);
- if (!WaitForDriveNear(kSecondBallAfterBarDrive - 0.5, 0.0)) return true;
- constexpr double kBallSmallWallTurn = -0.11;
- StartDrive(0, kBallSmallWallTurn, kTwoBallBallPickup, kFinishTurn);
-
- MoveSuperstructure(0.03, -0.010, 0.0, {8.0, 60.0},
- {4.0, 10.0}, {10.0, 25.0}, false, 12.0);
-
- if (!WaitForDriveProfileDone()) return true;
-
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0},
- {4.0, 10.0}, {10.0, 25.0}, false, 12.0);
-
- LOG(INFO, "Done backing up %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
- constexpr double kDriveBackDistance = 5.15 - 0.4;
- StartDrive(-kDriveBackDistance, 0.0, kTwoBallLowDrive, kFinishTurn);
- CloseIfBall();
- if (!WaitForDriveNear(kDriveBackDistance - 0.75, 0.0)) return true;
-
- StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
- LOG(INFO, "Straightening up at %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
- CloseIfBall();
- if (!WaitForDriveNear(kDriveBackDistance - 2.3, 0.0)) return true;
-
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected =
- ::y2016::sensors::ball_detector->voltage > 2.5;
- if (!ball_detected) {
- if (!WaitForDriveDone()) return true;
- LOG(INFO, "Aborting, no ball %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- return true;
- }
- }
- CloseShooter();
-
- BackLongShotLowBarTwoBall();
- LOG(INFO, "Spinning up the shooter wheels\n");
- SetShooterSpeed(640.0);
- StartDrive(0.0, 0.0, kTwoBallFastDrive, kSwerveTurn);
-
- if (!WaitForDriveNear(1.80, 0.0)) return true;
- StartDrive(0, kShootTurnAngle, kTwoBallFastDrive, kFinishTurn);
- BackLongShotTwoBall();
-
- if (!WaitForDriveDone()) return true;
- LOG(INFO, "Second shot done driving at %f seconds\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- WaitForSuperstructure();
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
- AlignWithVisionGoal();
- if (ShouldCancel()) return true;
-
- WaitForShooterSpeed();
- if (ShouldCancel()) return true;
-
- // 2.2 with 0.4 of vision.
- // 1.8 without any vision.
- LOG(INFO, "Going to vision align at %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- WaitForAlignedWithVision(start_time + aos::time::Time::InSeconds(12.9) -
- aos::time::Time::Now());
- LOG(INFO, "Shoot at %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- Shoot();
-
- LOG(INFO, "Second shot at %f seconds\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
- if (ShouldCancel()) return true;
-
- SetShooterSpeed(0.0);
- LOG(INFO, "Folding superstructure back down\n");
- TuckArm(true, false);
- LOG(INFO, "Shot %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
- WaitForSuperstructureLow();
-
- LOG(INFO, "Done %f\n",
- (aos::time::Time::Now() - start_time).ToSeconds());
-
+ case 5:
+ TwoBallAuto();
return true;
-
- } break;
+ break;
default:
LOG(ERROR, "Invalid auto mode %d\n", params.mode);
return true;
}
+ DoFullShot();
+
+ StartDrive(0.5, 0.0, kMoatDrive, kFastTurn);
if (!WaitForDriveDone()) return true;
- DoFullShot(params.mode != 0);
+ LOG(INFO, "Done %f\n", (aos::time::Time::Now() - start_time).ToSeconds());
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(5) / 2);
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index bda81f0..4da9522 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -34,6 +34,14 @@
// false if it cancels.
bool WaitForDriveDone();
+ // Returns true if the drive has finished.
+ bool IsDriveDone();
+ // Waits until the robot is pitched up above the specified angle, or the move
+ // finishes. Returns true on success, and false if it cancels.
+ bool WaitForAboveAngle(double angle);
+ bool WaitForBelowAngle(double angle);
+ bool WaitForMaxBy(double angle);
+
// Waits until the profile and distance is within distance and angle of the
// goal. Returns true on success, and false when canceled.
bool WaitForDriveNear(double distance, double angle);
@@ -67,6 +75,8 @@
bool IntakeDone();
bool WaitForDriveProfileDone();
+ void FrontLongShot();
+ void FrontMiddleShot();
void BackLongShot();
void BackLongShotTwoBall();
void BackLongShotLowBarTwoBall();
@@ -78,8 +88,10 @@
void CloseIfBall();
bool SuperstructureProfileDone();
bool SuperstructureDone();
+ void TippyDrive(double goal_distance, double tip_distance, double below,
+ double above);
- void DoFullShot(bool center);
+ void DoFullShot();
void LowBarDrive();
// Drive to the middle spot over the middle position. Designed for the rock
// wall, rough terain, or ramparts.
@@ -96,6 +108,8 @@
void AlignWithVisionGoal();
void WaitForAlignedWithVision(aos::time::Time align_duration);
+ void TwoBallAuto();
+
::std::unique_ptr<actors::VisionAlignAction> vision_action_;
};
diff --git a/y2016/constants.h b/y2016/constants.h
index c87559d..972d5a3 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -56,7 +56,7 @@
static constexpr ::frc971::constants::Range kIntakeRange{// Lower hard stop
-0.4,
// Upper hard stop
- 2.75 + 0.05,
+ 2.85 + 0.05,
// Lower soft stop
-0.300,
// Uppper soft stop
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index 3701e57..83eb677 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -77,7 +77,7 @@
# Radius of the robot, in meters (requires tuning by hand)
self.rb = 0.601 / 2.0
# Radius of the wheels, in meters.
- self.r = 0.097155 * 0.9811158901447808
+ self.r = 0.097155 * 0.9811158901447808 / 118.0 * 115.75
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 8575fdf..859a77b 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -124,11 +124,17 @@
const double throttle = -data.GetAxis(kDriveThrottle);
drivetrain_queue.status.FetchLatest();
- if (data.IsPressed(kVisionAlign) && vision_valid_ &&
- !vision_action_running_) {
- actors::VisionAlignActionParams params;
- action_queue_.EnqueueAction(actors::MakeVisionAlignAction(params));
- vision_action_running_ = true;
+ if (data.IsPressed(kVisionAlign)) {
+ if (vision_valid_ && !vision_action_running_) {
+ actors::VisionAlignActionParams params;
+ action_queue_.EnqueueAction(actors::MakeVisionAlignAction(params));
+ vision_action_running_ = true;
+ LOG(INFO, "Starting vision align\n");
+ } else {
+ if (!vision_valid_) {
+ LOG(INFO, "Vision align but not valid\n");
+ }
+ }
}
if (data.NegEdge(kVisionAlign)) {
@@ -212,7 +218,7 @@
if (data.IsPressed(kHigherFrontLong)) {
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.43;
+ wrist_goal_ = M_PI + 0.43 + 0.02;
if (drivetrain_queue.status.get()) {
wrist_goal_ += drivetrain_queue.status->ground_angle;
}
@@ -221,7 +227,7 @@
} else if (data.IsPressed(kFrontLong)) {
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.41;
+ wrist_goal_ = M_PI + 0.41 + 0.02;
if (drivetrain_queue.status.get()) {
wrist_goal_ += drivetrain_queue.status->ground_angle;
}
@@ -230,7 +236,7 @@
} else if (data.IsPressed(kBackLong)) {
// Backwards shot
shoulder_goal_ = M_PI / 2.0 - 0.4;
- wrist_goal_ = -0.62;
+ wrist_goal_ = -0.62 - 0.02;
if (drivetrain_queue.status.get()) {
wrist_goal_ += drivetrain_queue.status->ground_angle;
}
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 04fbac2..3ccb1dd 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -40,7 +40,8 @@
void SelectTargets(const VisionData &left_target,
const VisionData &right_target,
::aos::vision::Vector<2> *center_left,
- ::aos::vision::Vector<2> *center_right) {
+ ::aos::vision::Vector<2> *center_right, double *angle_left,
+ double *angle_right) {
// No good targets. Let the caller decide defaults.
if (right_target.target_size() == 0 || left_target.target_size() == 0) {
return;
@@ -75,6 +76,7 @@
const double angle = h / wid1;
if (min_angle == -1.0 || ::std::abs(angle) < ::std::abs(min_angle)) {
min_angle = angle;
+ *angle_left = angle;
left_index = i;
}
}
@@ -99,6 +101,7 @@
if (min_ang_err == -1.0 || min_ang_err > ang_err) {
min_ang_err = ang_err;
right_index = j;
+ *angle_right = ang;
}
}
@@ -159,16 +162,20 @@
TargetWithTimes last_;
};
-::aos::vision::Vector<2> CalculateFiltered(
- const CameraHandler &older, const CameraHandler &newer,
- const ::aos::vision::Vector<2> &newer_center,
- const ::aos::vision::Vector<2> &last_newer_center) {
+void CalculateFiltered(const CameraHandler &older, const CameraHandler &newer,
+ const ::aos::vision::Vector<2> &newer_center,
+ const ::aos::vision::Vector<2> &last_newer_center,
+ double angle, double last_angle,
+ ::aos::vision::Vector<2> *interpolated_result,
+ double *interpolated_angle) {
const double age_ratio =
(older.capture_time() - newer.last_capture_time()).ToSeconds() /
(newer.capture_time() - newer.last_capture_time()).ToSeconds();
- return ::aos::vision::Vector<2>(
+ interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
+
+ *interpolated_angle = angle * age_ratio + (1 - age_ratio) * last_angle;
}
// Handles calculating drivetrain offsets.
@@ -324,28 +331,39 @@
if (left_image_valid && right_image_valid) {
::aos::vision::Vector<2> center_left(0.0, 0.0);
::aos::vision::Vector<2> center_right(0.0, 0.0);
+ double angle_left;
+ double angle_right;
SelectTargets(left.target(), right.target(), ¢er_left,
- ¢er_right);
+ ¢er_right, &angle_left, &angle_right);
// TODO(Ben): Remember this from last time instead of recalculating it
// each time.
::aos::vision::Vector<2> last_center_left(0.0, 0.0);
::aos::vision::Vector<2> last_center_right(0.0, 0.0);
+ double last_angle_left;
+ double last_angle_right;
SelectTargets(left.last_target(), right.last_target(),
- &last_center_left, &last_center_right);
+ &last_center_left, &last_center_right,
+ &last_angle_left, &last_angle_right);
::aos::vision::Vector<2> filtered_center_left(0.0, 0.0);
::aos::vision::Vector<2> filtered_center_right(0.0, 0.0);
+ double filtered_angle_left;
+ double filtered_angle_right;
if (left.capture_time() < right.capture_time()) {
filtered_center_left = center_left;
+ filtered_angle_left = angle_left;
new_vision_status->target_time = left.capture_time().ToNSec();
- filtered_center_right =
- CalculateFiltered(left, right, center_right, last_center_right);
+ CalculateFiltered(left, right, center_right, last_center_right,
+ angle_right, last_angle_right,
+ &filtered_center_right, &filtered_angle_right);
} else {
filtered_center_right = center_right;
+ filtered_angle_right = angle_right;
new_vision_status->target_time = right.capture_time().ToNSec();
- filtered_center_left =
- CalculateFiltered(right, left, center_left, last_center_left);
+ CalculateFiltered(right, left, center_left, last_center_left,
+ angle_left, last_angle_left, &filtered_center_left,
+ &filtered_angle_left);
}
double distance, horizontal_angle, vertical_angle;
@@ -360,6 +378,8 @@
new_vision_status->horizontal_angle = horizontal_angle;
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
+ new_vision_status->angle =
+ (filtered_angle_left + filtered_angle_right) / 2.0;
}
if (drivetrain_offset.CompleteVisionStatus(new_vision_status.get())) {
diff --git a/y2016/vision/vision.q b/y2016/vision/vision.q
index a0d021c..872c9ae 100644
--- a/y2016/vision/vision.q
+++ b/y2016/vision/vision.q
@@ -20,6 +20,8 @@
double vertical_angle;
// Distance to the target in meters.
double distance;
+ // The angle in radians of the bottom of the target.
+ double angle;
// Capture time of the angle using the clock behind Time::Now().
int64_t target_time;