Add targets for the turret aiming to shoot at
This adds a basic aiming algorithm that aims at the 3-point shot when we
think it can make it, and a 2-point shot otherwise.
Note: This also formalizes the fact that we are using the origin that is
defined by sift.fbs this year.
Change-Id: I04de0d01e2596bfa75226c6ac00af1d1d08d487f
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index c9f3873..675eea3 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -2,7 +2,9 @@
#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
#include "aos/flatbuffers.h"
+#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
@@ -11,6 +13,12 @@
namespace superstructure {
namespace turret {
+// Returns the port that we want to score on given our current alliance. The yaw
+// of the port will be such that the positive x axis points out the back of the
+// target.
+frc971::control_loops::Pose InnerPortPose(aos::Alliance alliance);
+frc971::control_loops::Pose OuterPortPose(aos::Alliance alliance);
+
// This class manages taking in drivetrain status messages and generating turret
// goals so that it gets aimed at the goal.
class Aimer {
@@ -19,14 +27,18 @@
Goal;
typedef frc971::control_loops::drivetrain::Status Status;
Aimer();
- void Update(const Status *status);
+ void Update(const Status *status, aos::Alliance alliance);
const Goal *TurretGoal() const { return &goal_.message(); }
+ // Returns the distance to the goal, in meters.
+ double DistanceToGoal() const { return distance_; }
flatbuffers::Offset<AimerStatus> PopulateStatus(
flatbuffers::FlatBufferBuilder *fbb) const;
private:
aos::FlatbufferDetachedBuffer<Goal> goal_;
+ bool aiming_for_inner_port_ = false;
+ double distance_ = 0.0;
};
} // namespace turret