Added gear + 40 kpa auto and re-tuned 40 kpa auto
We now have much more accurate physical alignment methods for both.
Sabina sights down the side of the robot.
Change-Id: I05be6160ad81664efafcf48aa89dfc166a8390f1
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 51ea775..8149880 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -25,8 +25,17 @@
.count();
}
+const ProfileParameters kGearBallBackDrive = {3.0, 3.5};
+const ProfileParameters kGearDrive = {1.5, 2.0};
+const ProfileParameters kGearFastDrive = {2.0, 2.5};
+const ProfileParameters kGearSlowDrive = {1.0, 2.0};
+const ProfileParameters kGearPlaceDrive = {0.40, 2.0};
const ProfileParameters kSlowDrive = {3.0, 2.0};
const ProfileParameters kSlowTurn = {3.0, 3.0};
+const ProfileParameters kFirstTurn = {1.0, 1.5};
+const ProfileParameters kFirstGearStartTurn = {2.0, 3.0};
+const ProfileParameters kFirstGearTurn = {2.0, 5.0};
+const ProfileParameters kSecondGearTurn = {3.0, 5.0};
const ProfileParameters kSmashTurn = {3.0, 5.0};
} // namespace
@@ -43,37 +52,155 @@
Reset();
switch (params.mode) {
- case 0:
default: {
+ // Red is positive.
+ constexpr double kDriveDirection = -1.0;
+ // Side peg + hopper auto.
+
+ set_intake_goal(0.23);
+ set_turret_goal(-M_PI * kDriveDirection);
+ SendSuperstructureGoal();
+
+ constexpr double kLongPegDrive = 3.025;
+
+ StartDrive(kLongPegDrive, -kDriveDirection * M_PI / 4, kGearDrive,
+ kFirstGearStartTurn);
+ if (!WaitForDriveNear(100.0, M_PI / 8.0)) return true;
+ LOG(INFO, "Turn Middle: %f left to go\n", DriveDistanceLeft());
+
+ StartDrive(0.0, 0.0, kGearDrive, kFirstGearTurn);
+
+ if (!WaitForTurnProfileDone()) return true;
+ LOG(INFO, "Turn profile ended: %f left to go\n", DriveDistanceLeft());
+
+ set_hood_goal(0.43);
+ set_shooter_velocity(364.0);
+ SendSuperstructureGoal();
+
+ constexpr double kTurnDistanceFromStart = 1.08;
+ if (!WaitForDriveNear(kLongPegDrive - kTurnDistanceFromStart, 10.0)) {
+ return true;
+ }
+
+ StartDrive(0.0, kDriveDirection * (M_PI / 4 + 0.3), kGearSlowDrive,
+ kSecondGearTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ StartDrive(0.0, 0.0, kGearFastDrive, kSecondGearTurn);
+
+ if (!WaitForDriveNear(0.3, 0.0)) return true;
+
+ set_vision_track(true);
+ SendSuperstructureGoal();
+
+ StartDrive(0.19, 0.0, kGearPlaceDrive, kSecondGearTurn);
+
+ if (!WaitForDriveNear(0.07, 0.0)) return true;
+ set_gear_servo(0.3);
+ SendSuperstructureGoal();
+
+ // Shoot from the peg.
+ //set_indexer_angular_velocity(-2.1 * M_PI);
+ //SendSuperstructureGoal();
+ //this_thread::sleep_for(chrono::milliseconds(1750));
+
+ //this_thread::sleep_for(chrono::milliseconds(500));
+ this_thread::sleep_for(chrono::milliseconds(750));
+ set_indexer_angular_velocity(0.0);
+ set_vision_track(false);
+ set_turret_goal(0.0);
+
+ set_hood_goal(0.37);
+ set_shooter_velocity(351.0);
+ set_intake_goal(0.18);
+ set_gear_servo(0.4);
+
+ SendSuperstructureGoal();
+ LOG(INFO, "Starting drive back %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(-2.69, kDriveDirection * 1.30, kSlowDrive,
+ kFirstGearStartTurn);
+ if (!WaitForDriveNear(2.4, 0.0)) return true;
+
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.0, 0.0, kGearBallBackDrive, kFirstGearStartTurn);
+
+ if (!WaitForDriveNear(0.2, 0.0)) return true;
+ this_thread::sleep_for(chrono::milliseconds(200));
+ StartDrive(0.0, kDriveDirection * 1.10, kSlowDrive,
+ kSmashTurn);
+
+ if (!WaitForDriveNear(0.2, 0.35)) return true;
+ set_vision_track(true);
+ set_use_vision_for_shots(true);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveNear(0.2, 0.2)) return true;
+ StartDrive(0.0, -kDriveDirection * 0.15, kSlowDrive,
+ kSmashTurn);
+
+ LOG(INFO, "Starting second shot %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_indexer_angular_velocity(-2.15 * M_PI);
+ SendSuperstructureGoal();
+ if (!WaitForDriveNear(0.2, 0.1)) return true;
+
+ this_thread::sleep_for(start_time + chrono::seconds(11) -
+ monotonic_clock::now());
+ if (ShouldCancel()) return true;
+ set_intake_max_velocity(0.05);
+ set_intake_goal(0.08);
+
+ this_thread::sleep_for(start_time + chrono::seconds(15) -
+ monotonic_clock::now());
+ if (ShouldCancel()) return true;
+
+ set_intake_max_velocity(0.50);
+ set_intake_goal(0.18);
+ set_shooter_velocity(0.0);
+ set_indexer_angular_velocity(0.0);
+ SendSuperstructureGoal();
+
+ } break;
+
+ case 500: {
+ // Red is positive.
constexpr double kDriveDirection = 1.0;
- // Test case autonomous mode.
- // Drives forward 1.0 meters and then turns 180 degrees.
+ // Side hopper auto.
set_intake_goal(0.07);
SendSuperstructureGoal();
- StartDrive(-3.7, 0.0, kSlowDrive, kSlowTurn);
- if (!WaitForDriveNear(2.75, 0.0)) return true;
+
+ StartDrive(-3.3, kDriveDirection * M_PI / 10, kSlowDrive, kFirstTurn);
+ if (!WaitForDriveNear(3.30, 0.0)) return true;
+ LOG(INFO, "Turn ended: %f left to go\n", DriveDistanceLeft());
+ // We can go to 2.50 before we hit the previous profile.
+
+ if (!WaitForDriveNear(2.42, 0.0)) return true;
+ LOG(INFO, "%f left to go\n", DriveDistanceLeft());
set_intake_goal(0.23);
set_turret_goal(0.0);
// Values good for blue:
// TODO(austin): Drive these off the auto switch.
- //set_hood_goal(0.355 + 0.01 + 0.005);
- //set_shooter_velocity(355.0);
- set_hood_goal(0.355 + 0.01 + 0.005 + 0.01 + 0.01);
- set_shooter_velocity(351.0);
+
+ set_hood_goal(0.37);
+ set_shooter_velocity(353.0);
SendSuperstructureGoal();
- StartDrive(0.0, -M_PI / 4.0 * kDriveDirection, kSlowDrive, kSlowTurn);
- if (!WaitForDriveNear(0.05, 0.0)) return true;
- this_thread::sleep_for(chrono::milliseconds(100));
+ StartDrive(0.0, -M_PI / 8.0 * kDriveDirection, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(0.20, 0.0)) return true;
- StartDrive(0.0, (M_PI / 4.0 + 0.20) * kDriveDirection, kSlowDrive,
+ this_thread::sleep_for(chrono::milliseconds(300));
+
+ StartDrive(0.0, (M_PI / 8.0 + 0.20) * kDriveDirection, kSlowDrive,
kSmashTurn);
if (!WaitForDriveNear(0.05, 0.2)) return true;
set_vision_track(true);
+ set_use_vision_for_shots(true);
- set_indexer_angular_velocity(-1.8 * M_PI);
+ set_indexer_angular_velocity(-2.1 * M_PI);
SendSuperstructureGoal();
this_thread::sleep_for(chrono::milliseconds(200));
@@ -86,7 +213,6 @@
this_thread::sleep_for(start_time + chrono::seconds(9) -
monotonic_clock::now());
- StartDrive(0.0, (-0.05) * kDriveDirection, kSlowDrive, kSlowTurn);
if (ShouldCancel()) return true;
set_intake_max_velocity(0.05);
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index fcb17e9..8dadf0c 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -32,6 +32,8 @@
shooter_velocity_ = 0.0;
indexer_angular_velocity_ = 0.0;
intake_max_velocity_ = 0.5;
+ gear_servo_ = 0.66;
+ use_vision_for_shots_ = false;
InitializeEncoders();
ResetDrivetrain();
SendSuperstructureGoal();
@@ -44,6 +46,8 @@
double shooter_velocity_ = 0.0;
double indexer_angular_velocity_ = 0.0;
double intake_max_velocity_ = 0.5;
+ double gear_servo_ = 0.66;
+ bool use_vision_for_shots_ = false;
void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
@@ -58,6 +62,12 @@
void set_intake_max_velocity(double intake_max_velocity) {
intake_max_velocity_ = intake_max_velocity;
}
+ void set_gear_servo(double gear_servo) {
+ gear_servo_ = gear_servo;
+ }
+ void set_use_vision_for_shots(bool use_vision_for_shots) {
+ use_vision_for_shots_ = use_vision_for_shots;
+ }
void SendSuperstructureGoal() {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
@@ -80,6 +90,8 @@
new_superstructure_goal->intake.voltage_rollers = 0.0;
new_superstructure_goal->lights_on = true;
new_superstructure_goal->intake.disable_intake = false;
+ new_superstructure_goal->intake.gear_servo = gear_servo_;
+ new_superstructure_goal->use_vision_for_shots = use_vision_for_shots_;
new_superstructure_goal->indexer.angular_velocity =
indexer_angular_velocity_;