Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/actors/fridge_profile_lib.h b/y2015/actors/fridge_profile_lib.h
new file mode 100644
index 0000000..ce75f75
--- /dev/null
+++ b/y2015/actors/fridge_profile_lib.h
@@ -0,0 +1,285 @@
+#ifndef Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_
+#define Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_
+
+#include <cmath>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/util/phased_loop.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+
+namespace frc971 {
+namespace actors {
+
+struct ProfileParams {
+ double velocity;
+ double acceleration;
+};
+
+// Base class to provide helper utilities to all Actors who want to control the
+// fridge.
+template <typename T>
+class FridgeActorBase : public aos::common::actions::ActorBase<T> {
+ public:
+ FridgeActorBase(T *queues) : aos::common::actions::ActorBase<T>(queues) {}
+
+ protected:
+ void DoFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool grabbers) {
+ DoFridgeProfile(height, angle, elevator_parameters, arm_parameters,
+ grabbers, grabbers, grabbers);
+ }
+
+ bool StartFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 0;
+ new_fridge_goal->max_velocity = elevator_parameters.velocity;
+ new_fridge_goal->max_acceleration = elevator_parameters.acceleration;
+ new_fridge_goal->height = height;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_parameters.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_parameters.acceleration;
+ new_fridge_goal->angle = angle;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+ LOG(INFO, "Starting profile to %f, %f\n", height, angle);
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return false;
+ }
+ return true;
+ }
+
+ enum ProfileStatus { RUNNING, DONE, CANCELED };
+
+ ProfileStatus IterateProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (this->ShouldCancel()) {
+ LOG(INFO, "Canceling fridge movement\n");
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(WARNING, "no fridge status so can't really cancel\n");
+ return CANCELED;
+ }
+
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 0;
+ new_fridge_goal->max_velocity = elevator_parameters.velocity;
+ new_fridge_goal->max_acceleration = elevator_parameters.acceleration;
+ new_fridge_goal->height =
+ control_loops::fridge_queue.status->height +
+ (control_loops::fridge_queue.status->goal_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_velocity)) /
+ (2.0 * new_fridge_goal->max_acceleration);
+ height = new_fridge_goal->height;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_parameters.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_parameters.acceleration;
+ new_fridge_goal->angle =
+ control_loops::fridge_queue.status->angle +
+ (control_loops::fridge_queue.status->goal_angular_velocity *
+ ::std::abs(
+ control_loops::fridge_queue.status->goal_angular_velocity)) /
+ (2.0 * new_fridge_goal->max_angular_acceleration);
+ angle = new_fridge_goal->angle;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ }
+ return CANCELED;
+ }
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kAngleEpsilon = 0.02, kHeightEpsilon = 0.015;
+
+ if (control_loops::fridge_queue.status->state != 4) {
+ LOG(ERROR, "Fridge no longer running, aborting action\n");
+ return CANCELED;
+ }
+
+ if (::std::abs(control_loops::fridge_queue.status->goal_angle - angle) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_height - height) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_angular_velocity) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::fridge_queue.status->angle - angle) <
+ kAngleEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->height -
+ height) < kHeightEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return DONE;
+ }
+ }
+
+ return RUNNING;
+ }
+
+ void DoFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (!StartFridgeProfile(height, angle, elevator_parameters, arm_parameters,
+ top_grabbers, front_grabbers, back_grabbers)) {
+ return;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateProfile(height, angle, elevator_parameters, arm_parameters,
+ top_grabbers, front_grabbers, back_grabbers);
+ if (status == DONE || status == CANCELED) {
+ return;
+ }
+ }
+ }
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool grabbers) {
+ DoFridgeXYProfile(x, y, x_parameters, y_parameters, grabbers, grabbers,
+ grabbers);
+ }
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (!StartFridgeXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers)) {
+ return;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers);
+ if (status == DONE || status == CANCELED) {
+ return;
+ }
+ }
+ }
+
+ void CancelXYMotion(ProfileParams x_parameters, ProfileParams y_parameters,
+ bool top_grabbers, bool front_grabbers,
+ bool back_grabbers) {
+ LOG(INFO, "Canceling fridge movement\n");
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(WARNING, "no fridge status so can't really cancel\n");
+ return;
+ }
+
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x =
+ control_loops::fridge_queue.status->x +
+ (control_loops::fridge_queue.status->goal_x_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity)) /
+ (2.0 * new_fridge_goal->max_x_acceleration);
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y =
+ control_loops::fridge_queue.status->y +
+ (control_loops::fridge_queue.status->goal_y_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity)) /
+ (2.0 * new_fridge_goal->max_y_acceleration);
+ new_fridge_goal->y_velocity = 0.0;
+
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ }
+ }
+
+ ProfileStatus IterateXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (this->ShouldCancel()) {
+ CancelXYMotion(x_parameters, y_parameters, top_grabbers, front_grabbers,
+ back_grabbers);
+ return CANCELED;
+ }
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kXEpsilon = 0.02, kYEpsilon = 0.02;
+
+ if (control_loops::fridge_queue.status->state != 4) {
+ LOG(ERROR, "Fridge no longer running, aborting action\n");
+ return CANCELED;
+ }
+
+ if (::std::abs(control_loops::fridge_queue.status->goal_x - x) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y - y) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::fridge_queue.status->x - x) < kXEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->y - y) < kYEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return DONE;
+ }
+ }
+
+ return RUNNING;
+ }
+
+ bool StartFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x = x;
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y = y;
+ new_fridge_goal->y_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+ LOG(INFO, "Starting xy profile to %f, %f\n", x, y);
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return false;
+ }
+ return true;
+ }
+};
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_