Added 40 kpa auto
Change-Id: I1985625515b02bac1d909900c9593b9c88b3d333
diff --git a/y2017/actors/BUILD b/y2017/actors/BUILD
index b1cacc9..35dbe01 100644
--- a/y2017/actors/BUILD
+++ b/y2017/actors/BUILD
@@ -25,6 +25,7 @@
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/control_loops/drivetrain:drivetrain_config',
'//y2017/control_loops/drivetrain:drivetrain_base',
+ '//y2017/control_loops/superstructure:superstructure_queue',
],
)
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 5fbb1e3..701f648 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -25,8 +25,9 @@
.count();
}
-const ProfileParameters kSlowDrive = {2.0, 2.0};
+const ProfileParameters kSlowDrive = {3.0, 2.0};
const ProfileParameters kSlowTurn = {3.0, 3.0};
+const ProfileParameters kSmashTurn = {3.0, 5.0};
} // namespace
@@ -39,43 +40,63 @@
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
monotonic_clock::time_point start_time = monotonic_clock::now();
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
- InitializeEncoders();
- ResetDrivetrain();
+ Reset();
switch (params.mode) {
case 0:
- default:
- while (true) {
- constexpr auto kDelayTime = chrono::milliseconds(1);
- // Test case autonomous mode.
- // Drives forward 1.0 meters and then turns 180 degrees.
- StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
- if (!WaitForDriveDone()) return true;
+ default: {
+ constexpr double kDriveDirection = 1.0;
+ // Test case autonomous mode.
+ // Drives forward 1.0 meters and then turns 180 degrees.
+ set_intake_goal(0.07);
+ SendSuperstructureGoal();
+ StartDrive(-3.7, 0.0, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(2.75, 0.0)) return true;
- this_thread::sleep_for(kDelayTime);
- if (ShouldCancel()) return true;
+ set_intake_goal(0.23);
+ SendSuperstructureGoal();
+ StartDrive(0.0, -M_PI / 4.0 * kDriveDirection, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(0.05, 0.0)) return true;
- StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
- if (!WaitForDriveDone()) return true;
+ this_thread::sleep_for(chrono::milliseconds(100));
- this_thread::sleep_for(kDelayTime);
- if (ShouldCancel()) return true;
+ StartDrive(0.0, (M_PI / 4.0 + 0.20) * kDriveDirection, kSlowDrive,
+ kSmashTurn);
+ if (!WaitForDriveNear(0.05, 0.2)) return true;
- StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
- if (!WaitForDriveDone()) return true;
+ set_vision_track(true);
+ set_turret_goal(0.0);
+ set_hood_goal(0.63);
+ set_shooter_velocity(371.0);
+ SendSuperstructureGoal();
- this_thread::sleep_for(kDelayTime);
- if (ShouldCancel()) return true;
+ this_thread::sleep_for(chrono::milliseconds(200));
- StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
- if (!WaitForDriveDone()) return true;
+ StartDrive(0.0, (-0.15) * kDriveDirection, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(0.05, 0.02)) return true;
+ set_indexer_angular_velocity(-1.8 * M_PI);
+ SendSuperstructureGoal();
+ LOG(INFO, "Started shooting at %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
- this_thread::sleep_for(kDelayTime);
- if (ShouldCancel()) return true;
- }
+ this_thread::sleep_for(start_time + chrono::seconds(9) -
+ monotonic_clock::now());
+ StartDrive(0.0, (-0.05) * kDriveDirection, kSlowDrive, kSlowTurn);
+ if (ShouldCancel()) return true;
- break;
+ set_intake_max_velocity(0.05);
+ set_intake_goal(0.08);
+ SendSuperstructureGoal();
+ this_thread::sleep_for(start_time + chrono::seconds(15) -
+ monotonic_clock::now());
+ if (ShouldCancel()) return true;
+
+ set_shooter_velocity(0.0);
+ set_indexer_angular_velocity(0.0);
+ SendSuperstructureGoal();
+
+ } break;
}
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index 3c179ed..fcb17e9 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -9,11 +9,14 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
namespace actors {
using ::frc971::ProfileParameters;
+using ::y2017::control_loops::superstructure_queue;
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
@@ -21,7 +24,77 @@
bool RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
private:
- // TODO(phil): Implement this.
+ void Reset() {
+ intake_goal_ = 0.0;
+ turret_goal_ = 0.0;
+ vision_track_ = false;
+ hood_goal_ = 0.6;
+ shooter_velocity_ = 0.0;
+ indexer_angular_velocity_ = 0.0;
+ intake_max_velocity_ = 0.5;
+ InitializeEncoders();
+ ResetDrivetrain();
+ SendSuperstructureGoal();
+ }
+
+ double intake_goal_ = 0.0;
+ double turret_goal_ = 0.0;
+ bool vision_track_ = false;
+ double hood_goal_ = 0.6;
+ double shooter_velocity_ = 0.0;
+ double indexer_angular_velocity_ = 0.0;
+ double intake_max_velocity_ = 0.5;
+
+ void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
+ void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
+ void set_vision_track(bool vision_track) { vision_track_ = vision_track; }
+ void set_hood_goal(double hood_goal) { hood_goal_ = hood_goal; }
+ void set_shooter_velocity(double shooter_velocity) {
+ shooter_velocity_ = shooter_velocity;
+ }
+ void set_indexer_angular_velocity(double indexer_angular_velocity) {
+ indexer_angular_velocity_ = indexer_angular_velocity;
+ }
+ void set_intake_max_velocity(double intake_max_velocity) {
+ intake_max_velocity_ = intake_max_velocity;
+ }
+
+ void SendSuperstructureGoal() {
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->intake.distance = intake_goal_;
+ new_superstructure_goal->intake.disable_intake = false;
+ new_superstructure_goal->turret.angle = turret_goal_;
+ new_superstructure_goal->turret.track = vision_track_;
+ new_superstructure_goal->hood.angle = hood_goal_;
+ new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+
+ new_superstructure_goal->intake.profile_params.max_velocity =
+ intake_max_velocity_;
+ new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
+ new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+
+ new_superstructure_goal->intake.profile_params.max_acceleration = 5.0;
+ new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
+ new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
+
+ new_superstructure_goal->intake.voltage_rollers = 0.0;
+ new_superstructure_goal->lights_on = true;
+ new_superstructure_goal->intake.disable_intake = false;
+
+ new_superstructure_goal->indexer.angular_velocity =
+ indexer_angular_velocity_;
+
+ if (indexer_angular_velocity_ < -0.1) {
+ new_superstructure_goal->indexer.voltage_rollers = 12.0;
+ new_superstructure_goal->intake.voltage_rollers = 6.0;
+ } else {
+ new_superstructure_goal->indexer.voltage_rollers = 0.0;
+ }
+
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
};
} // namespace actors