Add X/Y profiling to the fridge.
The fridge queue now accepts parameters for an additional profiling
type. If you set `profiling_type` to 1 then you can specify the goals
in X/Y co-ordinates.
This is very useful for the scoring actions because it lets us move
the fridge horizontally with nicely profiled motions.
Change-Id: Idc70d5f6737a18a76448af0ffe24079cac616d06
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index 5761c79..d1fbc68 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -8,6 +8,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/zeroing/zeroing.h"
+#include "aos/common/util/kinematics.h"
namespace frc971 {
namespace control_loops {
@@ -61,6 +62,13 @@
ESTOP = 5,
};
+ enum class ProfilingType : int32_t {
+ // Use angle/height to specify the fridge goal.
+ ANGLE_HEIGHT_PROFILING = 0,
+ // Use x/y co-ordinates to specify the fridge goal.
+ X_Y_PROFILING = 1,
+ };
+
State state() const { return state_; }
protected:
@@ -139,13 +147,22 @@
double elevator_goal_ = 0.0;
double arm_goal_ = 0.0;
+ double arm_goal_velocity_ = 0.0;
+ double elevator_goal_velocity_ = 0.0;
+
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
control_loops::FridgeQueue::Position current_position_;
+ ProfilingType last_profiling_type_;
+ aos::util::ElevatorArmKinematics kinematics_;
+
aos::util::TrapezoidProfile arm_profile_;
aos::util::TrapezoidProfile elevator_profile_;
+
+ aos::util::TrapezoidProfile x_profile_;
+ aos::util::TrapezoidProfile y_profile_;
};
} // namespace control_loops