Removed all uses of the fridge profile actor.
Change-Id: Ic59aec97efcd91f268e8d5d3bc5fb112d77a6199
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 5ecdd46..bff37e8 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -52,73 +52,20 @@
],
},
{
- 'target_name': 'fridge_profile_action_queue',
- 'type': 'static_library',
- 'sources': ['fridge_profile_action.q'],
- 'variables': {
- 'header_path': 'frc971/actors',
- },
- 'dependencies': [
- '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'target_name' : 'fridge_profile_lib',
+ 'type' : 'static_library',
+ 'sources' : [
+ 'fridge_profile_lib.cc',
],
- 'export_dependent_settings': [
- '<(AOS)/common/actions/actions.gyp:action_queue',
- ],
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'fridge_profile_action_lib',
- 'type': 'static_library',
- 'sources': [
- 'fridge_profile_actor.cc',
- ],
- 'dependencies': [
- 'fridge_profile_action_queue',
- '<(DEPTH)/frc971/frc971.gyp:constants',
+ 'dependencies' : [
'<(AOS)/common/common.gyp:time',
- '<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
- '<(EXTERNALS):eigen',
- '<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
],
- 'export_dependent_settings': [
- '<(EXTERNALS):eigen',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- 'fridge_profile_action_queue',
- ],
- },
- {
- 'target_name': 'fridge_profile_action',
- 'type': 'executable',
- 'sources': [
- 'fridge_profile_actor_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- 'fridge_profile_action_queue',
- 'fridge_profile_action_lib',
- ],
- },
- {
- 'target_name': 'fridge_profile_action_test',
- 'type': 'executable',
- 'sources': [
- 'fridge_profile_actor_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/common.gyp:queues',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/linux_code/linux_code.gyp:init',
+ 'export_dependent_settings' : [
'<(AOS)/common/actions/actions.gyp:action_lib',
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
- 'fridge_profile_action_queue',
- 'fridge_profile_action_lib',
],
},
{
@@ -143,13 +90,14 @@
'score_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
+ 'fridge_profile_lib',
'score_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
'<(AOS)/common/controls/controls.gyp:control_loop',
],
'export_dependent_settings': [
+ 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'score_action_queue',
],
@@ -209,7 +157,6 @@
'pickup_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
'pickup_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
@@ -256,7 +203,7 @@
'can_pickup_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
+ 'fridge_profile_lib',
'can_pickup_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:phased_loop',
@@ -266,6 +213,7 @@
'<(AOS)/common/controls/controls.gyp:control_loop',
],
'export_dependent_settings': [
+ 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'can_pickup_action_queue',
],
@@ -305,7 +253,7 @@
'horizontal_can_pickup_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
+ 'fridge_profile_lib',
'horizontal_can_pickup_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:phased_loop',
@@ -315,6 +263,7 @@
'<(AOS)/common/controls/controls.gyp:control_loop',
],
'export_dependent_settings': [
+ 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'horizontal_can_pickup_action_queue',
],
@@ -374,7 +323,7 @@
'stack_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
+ 'fridge_profile_lib',
'stack_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:phased_loop',
@@ -383,6 +332,7 @@
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
],
'export_dependent_settings': [
+ 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'stack_action_queue',
],
@@ -422,7 +372,7 @@
'lift_actor.cc',
],
'dependencies': [
- 'fridge_profile_action_lib',
+ 'fridge_profile_lib',
'lift_action_queue',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/actions/actions.gyp:action_lib',
@@ -430,6 +380,7 @@
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
],
'export_dependent_settings': [
+ 'fridge_profile_lib',
'<(AOS)/common/actions/actions.gyp:action_lib',
'lift_action_queue',
],
@@ -519,79 +470,5 @@
'claw_action_lib',
],
},
- {
- 'target_name': 'intake_action_queue',
- 'type': 'static_library',
- 'sources': ['intake_action.q'],
- 'variables': {
- 'header_path': 'frc971/actors',
- },
- 'dependencies': [
- '<(AOS)/common/actions/actions.gyp:action_queue',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/actions/actions.gyp:action_queue',
- ],
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'intake_action_lib',
- 'type': 'static_library',
- 'sources': [
- 'intake_actor.cc',
- ],
- 'dependencies': [
- 'intake_action_queue',
- 'claw_action_lib',
- 'stack_action_lib',
- 'fridge_profile_action_lib',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- 'export_dependent_settings': [
- 'claw_action_lib',
- 'fridge_profile_action_lib',
- 'intake_action_queue',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- ],
- },
- {
- 'target_name': 'intake_action',
- 'type': 'executable',
- 'sources': [
- 'intake_actor_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- 'intake_action_queue',
- 'intake_action_lib',
- ],
- },
- {
- 'target_name': 'intake_action_test',
- 'type': 'executable',
- 'sources': [
- 'intake_actor_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/common.gyp:queues',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- '<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
- '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
- 'intake_action_queue',
- 'intake_action_lib',
- ],
- },
],
}
diff --git a/frc971/actors/can_pickup_actor.cc b/frc971/actors/can_pickup_actor.cc
index 7ee6d4f..7b43694 100644
--- a/frc971/actors/can_pickup_actor.cc
+++ b/frc971/actors/can_pickup_actor.cc
@@ -4,71 +4,38 @@
#include "aos/common/util/phased_loop.h"
#include "frc971/actors/can_pickup_actor.h"
-#include "frc971/actors/fridge_profile_actor.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
namespace frc971 {
namespace actors {
namespace {
-
-static constexpr double kArmVelocity = 1.00;
-static constexpr double kArmAcceleration = 1.6;
-static constexpr double kElevatorVelocity = 0.6;
-static constexpr double kElevatorAcceleration = 2.2;
-
+constexpr ProfileParams kArmMove{1.0, 1.6};
+constexpr ProfileParams kElevatorMove{0.6, 2.2};
} // namespace
-// Elevator, arm
-// Go to 0.3, 0.0
-// Go to 0.35, -1.1
-
CanPickupActor::CanPickupActor(CanPickupActionQueueGroup *queues)
- : aos::common::actions::ActorBase<CanPickupActionQueueGroup>(queues) {}
-
-void CanPickupActor::DoProfile(double height, double angle, bool grabbers) {
- FridgeProfileParams params;
-
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorVelocity;
- params.elevator_max_acceleration = kElevatorAcceleration;
-
- params.arm_angle = angle;
- params.arm_max_velocity = kArmVelocity;
- params.arm_max_acceleration = kArmAcceleration;
-
- params.top_front_grabber = grabbers;
- params.top_back_grabber = grabbers;
- params.bottom_front_grabber = grabbers;
- params.bottom_back_grabber = grabbers;
-
- ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
- profile->Start();
- while (!profile->CheckIteration()) {
- // wait until next Xms tick
- ::aos::time::PhasedLoopXMS(5, 2500);
- if (ShouldCancel()) {
- profile->Cancel();
- return;
- }
- }
-}
+ : FridgeActorBase<CanPickupActionQueueGroup>(queues) {}
bool CanPickupActor::RunAction(const CanPickupParams ¶ms) {
// Go around the can.
- DoProfile(params.pickup_height, params.pickup_angle, false);
+ DoFridgeProfile(params.pickup_height, params.pickup_angle, kElevatorMove,
+ kArmMove, false);
if (ShouldCancel()) return true;
// Lift and grab.
- DoProfile(params.lift_height, params.pickup_angle, true);
+ DoFridgeProfile(params.lift_height, params.pickup_angle, kElevatorMove,
+ kArmMove, true);
if (ShouldCancel()) return true;
// Pull it back in.
- DoProfile(params.lift_height, params.end_angle, true);
+ DoFridgeProfile(params.lift_height, params.end_angle, kElevatorMove, kArmMove,
+ true);
if (ShouldCancel()) return true;
// Pull it back in.
- DoProfile(params.end_height, params.end_angle, true);
+ DoFridgeProfile(params.end_height, params.end_angle, kElevatorMove, kArmMove,
+ true);
if (ShouldCancel()) return true;
return true;
diff --git a/frc971/actors/can_pickup_actor.h b/frc971/actors/can_pickup_actor.h
index 407de93..f343214 100644
--- a/frc971/actors/can_pickup_actor.h
+++ b/frc971/actors/can_pickup_actor.h
@@ -8,17 +8,15 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
#include "frc971/actors/can_pickup_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class CanPickupActor
- : public aos::common::actions::ActorBase<CanPickupActionQueueGroup> {
+class CanPickupActor : public FridgeActorBase<CanPickupActionQueueGroup> {
public:
explicit CanPickupActor(CanPickupActionQueueGroup *queues);
- void DoProfile(double height, double angle, bool grabbers);
-
bool RunAction(const CanPickupParams ¶ms) override;
};
diff --git a/frc971/actors/fridge_profile_action.q b/frc971/actors/fridge_profile_action.q
deleted file mode 100644
index fb6d54a..0000000
--- a/frc971/actors/fridge_profile_action.q
+++ /dev/null
@@ -1,39 +0,0 @@
-package frc971.actors;
-
-import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct FridgeProfileParams {
- double arm_angle;
- double arm_max_velocity;
- double arm_max_acceleration;
- double elevator_height;
- double elevator_max_velocity;
- double elevator_max_acceleration;
- bool top_front_grabber;
- bool top_back_grabber;
- bool bottom_front_grabber;
- bool bottom_back_grabber;
-};
-
-// Debug errors.
-struct ErrorToLog {
- float arm_error;
- float profile_error_arm;
- float elevator_error;
- float profile_error_elevator;
-};
-
-queue_group FridgeProfileActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- FridgeProfileParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group FridgeProfileActionQueueGroup fridge_profile_action;
diff --git a/frc971/actors/fridge_profile_actor.cc b/frc971/actors/fridge_profile_actor.cc
deleted file mode 100644
index 43bdb4b..0000000
--- a/frc971/actors/fridge_profile_actor.cc
+++ /dev/null
@@ -1,213 +0,0 @@
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/common/commonmath.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/queue_logging.h"
-#include "aos/common/actions/actor.h"
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/util/trapezoid_profile.h"
-
-#include "frc971/constants.h"
-#include "frc971/actors/fridge_profile_actor.h"
-#include "frc971/control_loops/fridge/fridge.q.h"
-
-namespace frc971 {
-namespace actors {
-
-FridgeProfileActor::FridgeProfileActor(actors::FridgeProfileActionQueueGroup* s)
- : aos::common::actions::ActorBase<actors::FridgeProfileActionQueueGroup>(
- s) {}
-
-bool FridgeProfileActor::InitializeProfile(double angle_max_vel,
- double angle_max_accel,
- double height_max_vel,
- double height_max_accel) {
- // if they have no vel/accel they will not move
- if (angle_max_vel != 0.0 && angle_max_accel != 0.0) {
- // Initialize arm profile.
- arm_profile_.reset(
- new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
- arm_profile_->set_maximum_velocity(angle_max_vel);
- arm_profile_->set_maximum_acceleration(angle_max_accel);
- } else {
- arm_profile_.reset();
- }
-
- // if they have no vel/accel they will not move
- if (height_max_vel != 0.0 && height_max_accel != 0.0) {
- // Initialize elevator profile.
- elevator_profile_.reset(
- new ::aos::util::TrapezoidProfile(::aos::time::Time::InMS(5)));
- elevator_profile_->set_maximum_velocity(height_max_vel);
- elevator_profile_->set_maximum_acceleration(height_max_accel);
- } else {
- elevator_profile_.reset();
- }
-
- return true;
-}
-
-bool FridgeProfileActor::IterateProfile(double goal_angle, double goal_height,
- double* next_angle, double* next_height,
- double* next_angle_velocity,
- double* next_height_velocity) {
- CHECK_NOTNULL(next_angle);
- CHECK_NOTNULL(next_height);
- CHECK_NOTNULL(next_angle_velocity);
- CHECK_NOTNULL(next_height_velocity);
- ::Eigen::Matrix<double, 2, 1> goal_state;
-
- if (!arm_profile_) {
- *next_angle = arm_start_angle_;
- *next_angle_velocity = 0.0;
- } else {
- goal_state = arm_profile_->Update(goal_angle, 0.0);
- *next_angle = goal_state(0, 0);
- *next_angle_velocity = goal_state(1, 0);
- }
-
- if (!elevator_profile_) {
- *next_height = elev_start_height_;
- *next_height_velocity = 0.0;
- } else {
- goal_state = elevator_profile_->Update(goal_height, 0.0);
- *next_height = goal_state(0, 0);
- *next_height_velocity = goal_state(1, 0);
- }
-
- return true;
-}
-
-bool FridgeProfileActor::RunAction(const FridgeProfileParams ¶ms) {
- const double goal_angle = params.arm_angle;
- const double goal_height = params.elevator_height;
- bool top_front = params.top_front_grabber;
- bool top_back = params.top_back_grabber;
- bool bottom_front = params.bottom_front_grabber;
- bool bottom_back = params.bottom_back_grabber;
- LOG(INFO,
- "Fridge profile goal: arm (%f) elev (%f) with grabbers(%d,%d,%d,%d).\n",
- goal_angle, goal_height, top_front, top_back, bottom_front, bottom_back);
-
- // defines finished
- constexpr double kAngleEpsilon = 0.02, kHeightEpsilon = 0.015;
- constexpr double kAngleProfileEpsilon = 0.0001, kHeightProfileEpsilon = 0.0001;
-
- // Initialize arm profile.
- if (!InitializeProfile(params.arm_max_velocity, params.arm_max_acceleration,
- params.elevator_max_velocity,
- params.elevator_max_acceleration)) {
- return false;
- }
-
- control_loops::fridge_queue.status.FetchLatest();
- if (control_loops::fridge_queue.status.get()) {
- if (!control_loops::fridge_queue.status->zeroed) {
- LOG(ERROR, "We are not running actions on an unzeroed fridge!\n");
- return false;
- }
- arm_start_angle_ = control_loops::fridge_queue.status->goal_angle;
- elev_start_height_ = control_loops::fridge_queue.status->goal_height;
- } else {
- LOG(ERROR, "No fridge status!\n");
- return false;
- }
-
- if (arm_profile_) {
- Eigen::Matrix<double, 2, 1> arm_current;
- arm_current.setZero();
- arm_current << arm_start_angle_, 0.0;
- arm_profile_->MoveCurrentState(arm_current);
- }
- if (elevator_profile_) {
- Eigen::Matrix<double, 2, 1> elevator_current;
- elevator_current.setZero();
- elevator_current << elev_start_height_, 0.0;
- elevator_profile_->MoveCurrentState(elevator_current);
- }
-
- while (true) {
- // wait until next Xms tick
- ::aos::time::PhasedLoopXMS(5, 2500);
-
- double current_goal_angle, current_goal_height;
- double angle_vel, height_vel;
- if (!IterateProfile(goal_angle, goal_height, ¤t_goal_angle,
- ¤t_goal_height, &angle_vel, &height_vel)) {
- return false;
- }
-
- // check if we should stop before we send
- if (ShouldCancel()) return true;
-
- auto message = control_loops::fridge_queue.goal.MakeMessage();
- message->angle = current_goal_angle;
- message->angular_velocity = angle_vel;
- message->height = current_goal_height;
- message->velocity = height_vel;
- message->grabbers.top_front = top_front;
- message->grabbers.top_back = top_back;
- message->grabbers.bottom_front = bottom_front;
- message->grabbers.bottom_back = bottom_back;
-
- LOG_STRUCT(DEBUG, "Sending fridge goal", *message);
- message.Send();
-
- control_loops::fridge_queue.status.FetchLatest();
- if (!control_loops::fridge_queue.status.get()) {
- return false;
- }
- double current_angle = control_loops::fridge_queue.status->angle;
- double current_height = control_loops::fridge_queue.status->height;
- LOG_STRUCT(DEBUG, "Got fridge status", *control_loops::fridge_queue.status);
-
- if (testing_) {
- current_angle = goal_angle;
- current_height = goal_height;
- }
-
- const double arm_error = ::std::abs(current_goal_angle - current_angle);
- const double profile_error_arm =
- ::std::abs(current_goal_angle - goal_angle);
-
- const double profile_error_elevator =
- ::std::abs(current_goal_height - goal_height);
- const double elevator_error =
- ::std::abs(current_goal_height - current_height);
-
- if ((!arm_profile_ || (arm_error < kAngleEpsilon &&
- profile_error_arm < kAngleProfileEpsilon)) &&
- (!elevator_profile_ ||
- (elevator_error < kHeightEpsilon &&
- profile_error_elevator < kHeightProfileEpsilon))) {
- break;
- } else {
- ErrorToLog error_to_log;
- error_to_log.arm_error = arm_error;
- error_to_log.profile_error_arm = profile_error_arm;
- error_to_log.profile_error_elevator = profile_error_elevator;
- error_to_log.elevator_error = elevator_error;
- LOG_STRUCT(DEBUG, "error", error_to_log);
- }
- }
-
- arm_profile_.reset();
- arm_profile_.reset();
- arm_start_angle_ = 0.0;
- elev_start_height_ = 0.0;
-
- LOG(INFO, "Fridge profile done moving.\n");
- return true;
-}
-
-::std::unique_ptr<FridgeAction> MakeFridgeProfileAction(
- const FridgeProfileParams& p) {
- return ::std::unique_ptr<FridgeAction>(
- new FridgeAction(&::frc971::actors::fridge_profile_action, p));
-}
-
-} // namespace actors
-} // namespace frc971
diff --git a/frc971/actors/fridge_profile_actor.h b/frc971/actors/fridge_profile_actor.h
deleted file mode 100644
index 3b3d818..0000000
--- a/frc971/actors/fridge_profile_actor.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef FRC971_ACTIONS_FRIDGE_PROFILE_ACTION_H_
-#define FRC971_ACTIONS_FRIDGE_PROFILE_ACTION_H_
-
-#include <memory>
-
-#include "frc971/actors/fridge_profile_action.q.h"
-#include "aos/common/actions/actor.h"
-#include "aos/common/actions/actions.h"
-#include "aos/common/util/trapezoid_profile.h"
-
-namespace frc971 {
-namespace actors {
-
-class FridgeProfileActor
- : public aos::common::actions::ActorBase<FridgeProfileActionQueueGroup> {
- public:
- explicit FridgeProfileActor(FridgeProfileActionQueueGroup *s);
-
- // sets up profiles. Returns false if things have already been setup
- bool InitializeProfile(double angle_max_vel, double angle_max_accel,
- double height_max_vel, double height_max_accel);
-
- // Takes a goal and computes the next step toward that goal. Returns false if
- // things are broken.
- bool IterateProfile(double goal_angle, double goal_height, double *next_angle,
- double *next_height, double *next_angle_velocity,
- double *next_angle_accel);
-
- bool RunAction(const FridgeProfileParams ¶ms) override;
-
- // only for unit test
- void SetTesting() { testing_ = true; }
-
- private:
- ::std::unique_ptr<aos::util::TrapezoidProfile> arm_profile_;
- ::std::unique_ptr<aos::util::TrapezoidProfile> elevator_profile_;
- double arm_start_angle_ = 0.0;
- double elev_start_height_ = 0.0;
- bool testing_ = false;
-};
-
-typedef aos::common::actions::TypedAction<FridgeProfileActionQueueGroup>
- FridgeAction;
-
-// Makes a new FridgeProfileActor action.
-::std::unique_ptr<FridgeAction> MakeFridgeProfileAction(
- const FridgeProfileParams &fridge_params);
-
-} // namespace actors
-} // namespace frc971
-
-#endif
diff --git a/frc971/actors/fridge_profile_actor_main.cc b/frc971/actors/fridge_profile_actor_main.cc
deleted file mode 100644
index 24cf75e..0000000
--- a/frc971/actors/fridge_profile_actor_main.cc
+++ /dev/null
@@ -1,19 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "frc971/actors/fridge_profile_action.q.h"
-#include "frc971/actors/fridge_profile_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
-
- frc971::actors::FridgeProfileActor fridge_profile(
- &::frc971::actors::fridge_profile_action);
- fridge_profile.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/actors/fridge_profile_actor_test.cc b/frc971/actors/fridge_profile_actor_test.cc
deleted file mode 100644
index b054e8d..0000000
--- a/frc971/actors/fridge_profile_actor_test.cc
+++ /dev/null
@@ -1,402 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "aos/common/actions/actor.h"
-#include "frc971/actors/fridge_profile_action.q.h"
-#include "frc971/actors/fridge_profile_actor.h"
-#include "frc971/control_loops/fridge/fridge.q.h"
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace actors {
-namespace testing {
-
-class FridgeProfileTest : public ::testing::Test {
- protected:
- FridgeProfileTest() {
- frc971::actors::fridge_profile_action.goal.Clear();
- frc971::actors::fridge_profile_action.status.Clear();
- control_loops::fridge_queue.status.Clear();
- control_loops::fridge_queue.goal.Clear();
- }
-
- virtual ~FridgeProfileTest() {
- frc971::actors::fridge_profile_action.goal.Clear();
- frc971::actors::fridge_profile_action.status.Clear();
- control_loops::fridge_queue.status.Clear();
- control_loops::fridge_queue.goal.Clear();
- }
-
- // Bring up and down Core.
- ::aos::common::testing::GlobalCoreInstance my_core;
-};
-
-void GetVelAccel(double new_val, double* last_two, double* vel, double* accel) {
- *vel = new_val - last_two[0];
- *accel = (*vel) - (last_two[0] - last_two[1]);
- last_two[1] = last_two[0];
- last_two[0] = new_val;
-}
-
-// A very long manual test that checks every step of a profile given ridiculous
-// values that generate a simple profile. Note that next_*_vel is the predicted
-// velocity for the step (in m/s), while *_vel is the observed velocity of the
-// last step (in m/step).
-TEST_F(FridgeProfileTest, ProfileValid) {
- FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
- EXPECT_TRUE(fridge_profile.InitializeProfile(200.0, 20000.0, 200.0, 20000.0));
- double last_angle[2] = {0.0, 0.0};
- double last_height[2] = {0.0, 0.0};
- double angle_vel = 0, angle_accel = 0, height_vel = 0, height_accel = 0;
- double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
- next_height_vel = 0.0;
-
- // Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(0.25, next_angle);
- EXPECT_EQ(100.0, next_angle_vel);
- EXPECT_EQ(0.25, angle_vel);
- EXPECT_EQ(0.25, angle_accel);
- EXPECT_EQ(0.25, next_height);
- EXPECT_EQ(100.0, next_height_vel);
- EXPECT_EQ(0.25, height_vel);
- EXPECT_EQ(0.25, height_accel);
-
- // Angle (1.000000, 0.750000, 0.50) Height (1.000000, 0.750000, 0.50)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(1.0, next_angle);
- EXPECT_EQ(200.0, next_angle_vel);
- EXPECT_EQ(0.75, angle_vel);
- EXPECT_EQ(0.50, angle_accel);
- EXPECT_EQ(1.0, next_height);
- EXPECT_EQ(200.0, next_height_vel);
- EXPECT_EQ(0.75, height_vel);
- EXPECT_EQ(0.50, height_accel);
-
- // Angle (2.000000, 1.000000, 0.25) Height (2.000000, 1.000000, 0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(2.0, next_angle);
- EXPECT_EQ(200.0, next_angle_vel);
- EXPECT_EQ(1.0, angle_vel);
- EXPECT_EQ(0.25, angle_accel);
- EXPECT_EQ(2.0, next_height);
- EXPECT_EQ(200.0, next_height_vel);
- EXPECT_EQ(1.0, height_vel);
- EXPECT_EQ(0.25, height_accel);
-
- // Angle (3.000000, 1.000000, 0.00) Height (3.000000, 1.000000, 0.00)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(3.0, next_angle);
- EXPECT_EQ(200.0, next_angle_vel);
- EXPECT_EQ(1.0, angle_vel);
- EXPECT_EQ(0.0, angle_accel);
- EXPECT_EQ(3.0, next_height);
- EXPECT_EQ(200.0, next_height_vel);
- EXPECT_EQ(1.0, height_vel);
- EXPECT_EQ(0.0, height_accel);
-
- // Angle (4.000000, 1.000000, 0.00) Height (4.000000, 1.000000, 0.00)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(4.0, next_angle);
- EXPECT_EQ(200.0, next_angle_vel);
- EXPECT_EQ(1.0, angle_vel);
- EXPECT_EQ(0.0, angle_accel);
- EXPECT_EQ(4.0, next_height);
- EXPECT_EQ(200.0, next_height_vel);
- EXPECT_EQ(1.0, height_vel);
- EXPECT_EQ(0.0, height_accel);
-
- // Angle (4.750000, 0.750000, -0.25) Height (4.750000, 0.750000, -0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(4.75, next_angle);
- EXPECT_EQ(100.0, next_angle_vel);
- EXPECT_EQ(0.75, angle_vel);
- EXPECT_EQ(-0.25, angle_accel);
- EXPECT_EQ(4.75, next_height);
- EXPECT_EQ(100.0, next_height_vel);
- EXPECT_EQ(0.75, height_vel);
- EXPECT_EQ(-0.25, height_accel);
-
- // Angle (5.000000, 0.250000, -0.50) Height (5.000000, 0.250000, -0.50)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(5.0, next_angle);
- EXPECT_EQ(0.0, next_angle_vel);
- EXPECT_EQ(0.25, angle_vel);
- EXPECT_EQ(-0.50, angle_accel);
- EXPECT_EQ(5.0, next_height);
- EXPECT_EQ(0.0, next_height_vel);
- EXPECT_EQ(0.25, height_vel);
- EXPECT_EQ(-0.50, height_accel);
-
- // Angle (5.000000, 0.000000, -0.25) Height (5.000000, 0.000000, -0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(5.0, next_angle);
- EXPECT_EQ(0.0, next_angle_vel);
- EXPECT_EQ(0.0, angle_vel);
- EXPECT_EQ(-0.25, angle_accel);
- EXPECT_EQ(5.0, next_height);
- EXPECT_EQ(0.0, next_height_vel);
- EXPECT_EQ(0.0, height_vel);
- EXPECT_EQ(-0.25, height_accel);
-
- // Angle (5.000000, 0.000000, 0.00) Height (5.000000, 0.000000, 0.00)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_EQ(5.0, next_angle);
- EXPECT_EQ(0.0, next_angle_vel);
- EXPECT_EQ(0.0, angle_vel);
- EXPECT_EQ(0.0, angle_accel);
- EXPECT_EQ(5.0, next_height);
- EXPECT_EQ(0.0, next_height_vel);
- EXPECT_EQ(0.0, height_vel);
- EXPECT_EQ(0.0, height_accel);
-}
-
-// Tests that we get to our first goal, then change the goal and get there under
-// constraints.
-TEST_F(FridgeProfileTest, ProfileChangeGoal) {
- FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
- EXPECT_TRUE(fridge_profile.InitializeProfile(200.0, 20000.0, 200.0, 20000.0));
- double last_angle[2] = {0.0, 0.0};
- double last_height[2] = {0.0, 0.0};
- double angle_vel = 0, angle_accel = 0, height_vel = 0, height_accel = 0;
- double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
- next_height_vel = 0.0;
-
- for (int i = 0; i < 7; i++) {
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle,
- &next_height, &next_angle_vel,
- &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_GE(1.0, angle_vel);
- EXPECT_GE(0.5, angle_accel);
- EXPECT_LE(-1.0, angle_vel);
- EXPECT_LE(-0.5, angle_accel);
- }
-
- EXPECT_EQ(5.0, next_angle);
-
- for (int i = 0; i < 7; i++) {
- EXPECT_TRUE(fridge_profile.IterateProfile(10.0, 10.0, &next_angle,
- &next_height, &next_angle_vel,
- &next_height_vel));
- GetVelAccel(next_angle, last_angle, &angle_vel, &angle_accel);
- GetVelAccel(next_height, last_height, &height_vel, &height_accel);
- EXPECT_GE(1.0, angle_vel);
- EXPECT_GE(0.5, angle_accel);
- EXPECT_LE(-1.0, angle_vel);
- EXPECT_LE(-0.5, angle_accel);
- }
-
- EXPECT_EQ(10.0, next_angle);
-}
-
-// Use our simple little profile with a queue to check we get the same result
-TEST_F(FridgeProfileTest, ProfileQueueValid) {
- FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
-
- FridgeProfileParams params;
- params.arm_angle = 5.0;
- params.arm_max_velocity = 200.0;
- params.arm_max_acceleration = 20000.0;
- params.elevator_height = 5.0;
- params.elevator_max_velocity = 200.0;
- params.elevator_max_acceleration = 20000.0;
- params.top_front_grabber = true;
- params.top_back_grabber = false;
- params.bottom_front_grabber = true;
- params.bottom_back_grabber = false;
-
- frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
- .run(true)
- .params(params)
- .Send();
-
- // tell it the fridge is zeroed
- control_loops::fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send();
-
- fridge_profile.SetTesting();
-
- // do the action and it will post to the goal queue
- fridge_profile.WaitForActionRequest();
- fridge_profile.RunAction(params);
-
- // a= 0.250000, e= 0.250000, av= 100.000000, ev= 100.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(0.25, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(0.25, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(100.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(100.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 1.000000, e= 1.000000, av= 200.000000, ev= 200.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(1.0, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(1.0, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 2.000000, e= 2.000000, av= 200.000000, ev= 200.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(2.0, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(2.0, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 3.000000, e= 3.000000, av= 200.000000, ev= 200.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(3.0, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(3.0, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 4.000000, e= 4.000000, av= 200.000000, ev= 200.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(4.0, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(4.0, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(200.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 4.750000, e= 4.750000, av= 100.000000, ev= 100.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(4.75, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(4.75, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(100.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(100.0, control_loops::fridge_queue.goal->velocity);
-
- // a= 5.000000, e= 5.000000, av= 0.000000, ev= 0.000000
- EXPECT_TRUE(control_loops::fridge_queue.goal.FetchNext());
- EXPECT_TRUE(control_loops::fridge_queue.goal.get());
- EXPECT_EQ(5.0, control_loops::fridge_queue.goal->angle);
- EXPECT_EQ(5.0, control_loops::fridge_queue.goal->height);
- EXPECT_EQ(0.0, control_loops::fridge_queue.goal->angular_velocity);
- EXPECT_EQ(0.0, control_loops::fridge_queue.goal->velocity);
-
- // that should be all
- EXPECT_FALSE(control_loops::fridge_queue.goal.FetchNext());
-}
-
-// Make sure that giving 0 velocity+acceleration makes it not move at all.
-TEST_F(FridgeProfileTest, ProfileNoVel) {
- FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
-
- FridgeProfileParams params;
- params.arm_angle = 5.0;
- params.arm_max_velocity = 200.0;
- params.arm_max_acceleration = 20000.0;
- params.elevator_height = 5.0;
- params.elevator_max_velocity = 0.0;
- params.elevator_max_acceleration = 0.0;
- params.top_front_grabber = true;
- params.top_back_grabber = false;
- params.bottom_front_grabber = true;
- params.bottom_back_grabber = false;
-
- frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
- .run(true)
- .params(params)
- .Send();
-
- // tell it the fridge is zeroed
- control_loops::fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send();
-
- fridge_profile.SetTesting();
-
- // do the action and it will post to the goal queue
- fridge_profile.WaitForActionRequest();
- fridge_profile.RunAction(params);
-
- while (control_loops::fridge_queue.goal.FetchNext()) {
- EXPECT_EQ(0.0, control_loops::fridge_queue.goal->height);
- }
-}
-
-// Make sure that should cancel gets set by pushing to queue.
-TEST_F(FridgeProfileTest, ProfileShouldCancel) {
- FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
- double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
- next_height_vel = 0.0;
- FridgeProfileParams params;
- params.arm_angle = 5.0;
- params.arm_max_velocity = 200.0;
- params.arm_max_acceleration = 20000.0;
- frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
- .run(true)
- .params(params)
- .Send();
-
- // tell it the fridge is zeroed
- control_loops::fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send();
-
- fridge_profile.SetTesting();
-
- fridge_profile.WaitForActionRequest();
-
- // Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
- EXPECT_FALSE(fridge_profile.ShouldCancel());
- frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
- .run(false)
- .params(params)
- .Send();
-
- EXPECT_TRUE(fridge_profile.ShouldCancel());
-
- // Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
- EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
- &next_angle_vel, &next_height_vel));
-}
-
-} // namespace testing.
-} // namespace actors.
-} // namespace frc971.
diff --git a/frc971/actors/fridge_profile_lib.cc b/frc971/actors/fridge_profile_lib.cc
new file mode 100644
index 0000000..c6ad8e3
--- /dev/null
+++ b/frc971/actors/fridge_profile_lib.cc
@@ -0,0 +1 @@
+#include "frc971/actors/fridge_profile_lib.h"
diff --git a/frc971/actors/fridge_profile_lib.h b/frc971/actors/fridge_profile_lib.h
new file mode 100644
index 0000000..7c26f9d
--- /dev/null
+++ b/frc971/actors/fridge_profile_lib.h
@@ -0,0 +1,112 @@
+#ifndef FRC971_ACTORS_FRIDGE_PROFILE_LIB_H_
+#define FRC971_ACTORS_FRIDGE_PROFILE_LIB_H_
+
+#include <cmath>
+
+#include "aos/common/actions/actor.h"
+#include "frc971/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);
+ }
+
+ void DoFridgeProfile(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->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;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return;
+ }
+
+ while (true) {
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kAngleEpsilon = 0.02, kHeightEpsilon = 0.015;
+
+ 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 &&
+ ::std::abs(control_loops::fridge_queue.status->angle - angle) <
+ kAngleEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->height - height) <
+ kHeightEpsilon) {
+ return;
+ }
+
+ if (this->ShouldCancel()) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ 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 +
+ ::std::pow(control_loops::fridge_queue.status->goal_velocity, 2.0) /
+ (2.0 * new_fridge_goal->max_acceleration);
+ 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 +
+ ::std::pow(
+ control_loops::fridge_queue.status->goal_angular_velocity,
+ 2.0) /
+ (2.0 * new_fridge_goal->max_angular_acceleration);
+ 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;
+ }
+ }
+ }
+ }
+};
+
+} // namespace actors
+} // namespace frc971
+
+#endif // FRC971_ACTORS_FRIDGE_PROFILE_LIB_H_
diff --git a/frc971/actors/horizontal_can_pickup_actor.cc b/frc971/actors/horizontal_can_pickup_actor.cc
index 81c89e0..1e87315 100644
--- a/frc971/actors/horizontal_can_pickup_actor.cc
+++ b/frc971/actors/horizontal_can_pickup_actor.cc
@@ -5,7 +5,7 @@
#include "aos/common/util/phased_loop.h"
#include "frc971/actors/horizontal_can_pickup_actor.h"
-#include "frc971/actors/fridge_profile_actor.h"
+#include "frc971/actors/fridge_profile_lib.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
@@ -15,10 +15,8 @@
constexpr double kClawPickupVelocity = 3.00;
constexpr double kClawPickupAcceleration = 4.0;
-constexpr double kArmVelocity = 1.00;
-constexpr double kArmAcceleration = 1.6;
-constexpr double kElevatorVelocity = 0.6;
-constexpr double kElevatorAcceleration = 2.2;
+constexpr ProfileParams kArmMove{1.0, 1.6};
+constexpr ProfileParams kElevatorMove{0.6, 2.2};
constexpr double kAngleEpsilon = 0.10;
@@ -26,43 +24,7 @@
HorizontalCanPickupActor::HorizontalCanPickupActor(
HorizontalCanPickupActionQueueGroup *queues)
- : aos::common::actions::ActorBase<HorizontalCanPickupActionQueueGroup>(
- queues) {}
-
-void HorizontalCanPickupActor::DoProfile(double height, double angle,
- bool grabbers) {
- DoProfile(height, angle, grabbers, grabbers, grabbers);
-}
-
-void HorizontalCanPickupActor::DoProfile(double height, double angle,
- bool top_grabbers, bool front_grabbers,
- bool back_grabbers) {
- FridgeProfileParams params;
-
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorVelocity;
- params.elevator_max_acceleration = kElevatorAcceleration;
-
- params.arm_angle = angle;
- params.arm_max_velocity = kArmVelocity;
- params.arm_max_acceleration = kArmAcceleration;
-
- params.top_front_grabber = top_grabbers;
- params.top_back_grabber = top_grabbers;
- params.bottom_front_grabber = front_grabbers;
- params.bottom_back_grabber = back_grabbers;
-
- ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
- profile->Start();
- while (!profile->CheckIteration()) {
- // wait until next Xms tick
- ::aos::time::PhasedLoopXMS(5, 2500);
- if (ShouldCancel()) {
- profile->Cancel();
- return;
- }
- }
-}
+ : FridgeActorBase<HorizontalCanPickupActionQueueGroup>(queues) {}
bool HorizontalCanPickupActor::WaitOrCancel(::aos::time::Time duration) {
::aos::time::Time end_time = ::aos::time::Time::Now() + duration;
@@ -104,7 +66,8 @@
bool HorizontalCanPickupActor::RunAction(
const HorizontalCanPickupParams ¶ms) {
// Go around the can.
- DoProfile(params.elevator_height, 0.0, false, false, true);
+ DoFridgeProfile(params.elevator_height, 0.0, kElevatorMove, kArmMove, false,
+ false, true);
if (ShouldCancel()) return true;
MoveArm(params.pickup_angle, 0.0);
@@ -137,7 +100,9 @@
return true;
}
- DoProfile(params.elevator_height, 0.0, false, true, true);
+ DoFridgeProfile(params.elevator_height, 0.0, kElevatorMove, kArmMove, false,
+ true, true);
+ if (ShouldCancel()) return true;
MoveArm(params.claw_end_angle, 7.0);
diff --git a/frc971/actors/horizontal_can_pickup_actor.h b/frc971/actors/horizontal_can_pickup_actor.h
index 288127c..88f2987 100644
--- a/frc971/actors/horizontal_can_pickup_actor.h
+++ b/frc971/actors/horizontal_can_pickup_actor.h
@@ -8,12 +8,13 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
#include "frc971/actors/horizontal_can_pickup_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class HorizontalCanPickupActor : public aos::common::actions::ActorBase<
- HorizontalCanPickupActionQueueGroup> {
+class HorizontalCanPickupActor
+ : public FridgeActorBase<HorizontalCanPickupActionQueueGroup> {
public:
explicit HorizontalCanPickupActor(
HorizontalCanPickupActionQueueGroup *queues);
@@ -21,10 +22,6 @@
bool RunAction(const HorizontalCanPickupParams ¶ms) override;
private:
- void DoProfile(double height, double angle, bool grabbers);
- void DoProfile(double height, double angle, bool top_grabbers,
- bool front_grabbers, bool back_grabbers);
-
// Waits until the duration has elapsed, or we are asked to cancel.
// Returns false if we should cancel.
bool WaitOrCancel(::aos::time::Time duration);
diff --git a/frc971/actors/intake_action.q b/frc971/actors/intake_action.q
deleted file mode 100644
index eb5201e..0000000
--- a/frc971/actors/intake_action.q
+++ /dev/null
@@ -1,30 +0,0 @@
-package frc971.actors;
-
-import "aos/common/actions/actions.q";
-
-struct IntakeParams {
- // If grab is true, we're intaking the tote.
- // If it is false, we're pulling the claw up and pushing the tote backwards
- // into the robot. If we're trying to intake from the HP, grab need never be
- // false. If we're trying to intake from the ground, however, the action
- // should be run twice: First, with grab true in order to grab the tote, and
- // then again with grab false in order to send it back into the robot.
- bool grab;
- // If ground is true, we're intaking from the ground. Otherwise, we'e intaking
- // from the HP.
- bool ground;
-};
-
-queue_group IntakeActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- IntakeParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group IntakeActionQueueGroup intake_action;
diff --git a/frc971/actors/intake_actor.cc b/frc971/actors/intake_actor.cc
deleted file mode 100644
index ce8eca3..0000000
--- a/frc971/actors/intake_actor.cc
+++ /dev/null
@@ -1,168 +0,0 @@
-#include "frc971/actors/intake_actor.h"
-
-#include <math.h>
-
-#include "aos/common/controls/control_loop.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/time.h"
-#include "aos/common/util/phased_loop.h"
-#include "frc971/actors/claw_actor.h"
-#include "frc971/constants.h"
-#include "frc971/actors/fridge_profile_actor.h"
-#include "frc971/actors/stack_actor.h"
-
-namespace frc971 {
-namespace actors {
-namespace {
-
-using ::aos::time::Time;
-
-// The angle the claw should be when intaking from the Human Player station.
-// TODO(danielp): Make this right.
-constexpr double kHpIntakeAngle = M_PI / 4.0;
-// How long we spit backwards for when we have a tote and want to send it into
-// the robot.
-const Time kBackSpitTime = Time::InSeconds(0.5);
-
-constexpr double kClawVelocity = 1.0;
-constexpr double kClawAcceleration = 4.0;
-constexpr double kClawIntakeVoltage = 12.0;
-constexpr double kClawBackSpitVoltage = 12.0;
-constexpr double kElevatorVelocity = 0.3;
-constexpr double kElevatorAccel = 0.3;
-constexpr double kArmVelocity = 0.3;
-constexpr double kArmAccel = 0.3;
-
-} // namespace
-
-IntakeActor::IntakeActor(IntakeActionQueueGroup *queues)
- : aos::common::actions::ActorBase<IntakeActionQueueGroup>(queues) {}
-
-namespace {
-
-// Creates and runs a claw action.
-// angle: The angle we want to move the claw to.
-// intake_voltage: The voltage to run the intake rollers at.
-::std::unique_ptr<ClawAction> MoveClaw(double angle, double intake_voltage) {
- ClawParams params;
- params.angle = angle;
- params.max_velocity = kClawVelocity;
- params.max_acceleration = kClawAcceleration;
- params.intake_voltage = intake_voltage;
- params.rollers_closed = true;
-
- ::std::unique_ptr<ClawAction> claw_action = MakeClawAction(params);
- claw_action->Start();
- return claw_action;
-}
-
-// Creates and runs a fridge action.
-// height: The height we want to move the elevator to.
-::std::unique_ptr<FridgeAction> MoveFridge(double height) {
- FridgeProfileParams params;
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorVelocity;
- params.elevator_max_acceleration = kElevatorAccel;
-
- params.arm_angle = 0.0;
- params.arm_max_velocity = kArmVelocity;
- params.arm_max_acceleration = kArmAccel;
-
- params.top_front_grabber = true;
- params.top_back_grabber = true;
- params.bottom_front_grabber = true;
- params.bottom_back_grabber = true;
-
- ::std::unique_ptr<FridgeAction> fridge_action =
- MakeFridgeProfileAction(params);
- fridge_action->Start();
- return fridge_action;
-}
-
-} // namespace
-
-void IntakeActor::WaitForSystems(FridgeAction *fridge, ClawAction *claw) {
- while ((fridge && !fridge->CheckIteration()) ||
- (claw && !claw->CheckIteration())) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
-
- if (ShouldCancel()) {
- if (fridge) {
- fridge->Cancel();
- }
- if (claw) {
- claw->Cancel();
- }
- LOG(WARNING, "Cancelling fridge and claw.\n");
- return;
- }
- }
-}
-
-bool IntakeActor::RunAction(const IntakeParams ¶ms) {
- const auto &values = constants::GetValues();
-
- if (params.grab) {
- // Grab the tote.
- LOG(INFO, "Grabbing tote.\n");
-
- {
- // Move the fridge up to make space for the tote we're intaking.
- auto fridge = MoveFridge(values.tote_height);
-
- // Move the claw and start intaking.
- const double claw_angle = params.ground ? 0.0 : kClawIntakeVoltage;
- auto claw = MoveClaw(claw_angle, kClawIntakeVoltage);
- WaitForSystems(fridge.get(), claw.get());
- if (ShouldCancel()) return true;
- }
- } else {
- // Send it back into the robot.
- LOG(INFO, "Spitting tote backwards.\n");
-
- {
- // Pull the claw up.
- auto claw = MoveClaw(kHpIntakeAngle, 0.0);
- WaitForSystems(nullptr, claw.get());
- if (ShouldCancel()) return true;
- }
-
- {
- // Spit backwards for awhile.
- auto claw = MoveClaw(kHpIntakeAngle, kClawBackSpitVoltage);
- WaitForSystems(nullptr, claw.get());
- ::aos::time::SleepFor(kBackSpitTime);
- MoveClaw(kHpIntakeAngle, 0.0);
- WaitForSystems(nullptr, claw.get());
- if (ShouldCancel()) return true;
- }
-
- {
- // Stack the new tote.
- StackParams params;
- params.claw_out_angle = M_PI / 4;
- ::std::unique_ptr<StackAction> stack_action = MakeStackAction(params);
- stack_action->Start();
- while (!stack_action->CheckIteration()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
- 2500);
- if (ShouldCancel()) {
- stack_action->Cancel();
- LOG(WARNING, "Cancelling stack action.\n");
- return true;
- }
- }
- }
- }
-
- LOG(INFO, "Done running intake action.\n");
- return true;
-}
-
-::std::unique_ptr<IntakeAction> MakeIntakeAction(const IntakeParams ¶ms) {
- return ::std::unique_ptr<IntakeAction>(
- new IntakeAction(&::frc971::actors::intake_action, params));
-}
-
-} // actors
-} // frc971
diff --git a/frc971/actors/intake_actor.h b/frc971/actors/intake_actor.h
deleted file mode 100644
index 46df3cd..0000000
--- a/frc971/actors/intake_actor.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef FRC971_ACTORS_INTAKE_ACTOR_H_
-#define FRC971_ACTORS_INTAKE_ACTOR_H_
-
-#include <stdint.h>
-
-#include <memory>
-
-#include "aos/common/actions/actions.h"
-#include "aos/common/actions/actor.h"
-#include "frc971/actors/claw_actor.h"
-#include "frc971/actors/fridge_profile_actor.h"
-#include "frc971/actors/intake_action.q.h"
-
-namespace frc971 {
-namespace actors {
-
-class IntakeActor
- : public aos::common::actions::ActorBase<IntakeActionQueueGroup> {
- public:
- explicit IntakeActor(IntakeActionQueueGroup *queues);
-
- bool RunAction(const IntakeParams ¶ms) override;
-
- private:
- // Goes and waits for the claw and fridge actions to complete, handling
- // cancellation properly.
- // fridge: The fridge action to wait on, or nullptr if we don't want to wait
- // for the fridge.
- // claw: The claw action to wait on, or nullptr if we don't want to wait for
- // the claw.
- void WaitForSystems(FridgeAction *fridge, ClawAction *claw);
-};
-
-typedef aos::common::actions::TypedAction<IntakeActionQueueGroup> IntakeAction;
-
-// Makes a new IntakeActor action.
-::std::unique_ptr<IntakeAction> MakeIntakeAction(const IntakeParams ¶ms);
-
-} // namespace actors
-} // namespace frc971
-
-#endif
diff --git a/frc971/actors/intake_actor_main.cc b/frc971/actors/intake_actor_main.cc
deleted file mode 100644
index 577b89f..0000000
--- a/frc971/actors/intake_actor_main.cc
+++ /dev/null
@@ -1,15 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "frc971/actors/intake_action.q.h"
-#include "frc971/actors/intake_actor.h"
-
-int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
-
- ::frc971::actors::IntakeActor intake(&::frc971::actors::intake_action);
- intake.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/actors/intake_actor_test.cc b/frc971/actors/intake_actor_test.cc
deleted file mode 100644
index 39370f5..0000000
--- a/frc971/actors/intake_actor_test.cc
+++ /dev/null
@@ -1,79 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "aos/common/actions/actor.h"
-#include "frc971/actors/intake_action.q.h"
-#include "frc971/actors/intake_actor.h"
-#include "frc971/control_loops/fridge/fridge.q.h"
-#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/control_loops/team_number_test_environment.h"
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace actors {
-namespace testing {
-
-class IntakeActionTest : public ::testing::Test {
- protected:
- IntakeActionTest() {
- frc971::actors::intake_action.goal.Clear();
- frc971::actors::intake_action.status.Clear();
- control_loops::fridge_queue.status.Clear();
- control_loops::fridge_queue.goal.Clear();
- control_loops::claw_queue.status.Clear();
- control_loops::claw_queue.goal.Clear();
- }
-
- virtual ~IntakeActionTest() {
- frc971::actors::intake_action.goal.Clear();
- frc971::actors::intake_action.status.Clear();
- control_loops::fridge_queue.status.Clear();
- control_loops::fridge_queue.goal.Clear();
- control_loops::claw_queue.status.Clear();
- control_loops::claw_queue.goal.Clear();
- }
-
- // Bring up and down Core.
- ::aos::common::testing::GlobalCoreInstance my_core;
-};
-
-// Tests that cancel stops not only the intake action, but also the underlying
-// profile action.
-TEST_F(IntakeActionTest, IntakeCancel) {
- IntakeActor intake(&frc971::actors::intake_action);
-
- frc971::actors::intake_action.goal.MakeWithBuilder().run(true).Send();
-
- // tell it the fridge and claw are zeroed
- control_loops::fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send();
- control_loops::claw_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .Send();
-
- // do the action and it will post to the goal queue
- intake.WaitForActionRequest();
-
- // the action has started, so now cancel it and it should cancel
- // the underlying profile
- frc971::actors::intake_action.goal.MakeWithBuilder().run(false).Send();
-
- // let the action start running, if we return from this call it has worked.
- const IntakeParams params = {true, true};
- intake.RunAction(params);
-
- SUCCEED();
-}
-
-} // namespace testing
-} // namespace actors
-} // namespace frc971
diff --git a/frc971/actors/lift_actor.cc b/frc971/actors/lift_actor.cc
index 8e8de47..25213ca 100644
--- a/frc971/actors/lift_actor.cc
+++ b/frc971/actors/lift_actor.cc
@@ -2,54 +2,25 @@
#include "aos/common/time.h"
#include "frc971/actors/lift_actor.h"
-#include "frc971/actors/fridge_profile_actor.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
namespace {
-
-static constexpr double kArmVelocity = 0.40;
-static constexpr double kArmAcceleration = 1.0;
-static constexpr double kElevatorVelocity = 0.5;
-static constexpr double kElevatorAcceleration = 2.2;
-
+constexpr ProfileParams kArmMove{0.6, 2.0};
+constexpr ProfileParams kElevatorMove{0.9, 3.0};
} // namespace
LiftActor::LiftActor(LiftActionQueueGroup *queues)
- : aos::common::actions::ActorBase<LiftActionQueueGroup>(queues) {}
-
-namespace {
-
-void DoProfile(double height, double angle, bool grabbers) {
- FridgeProfileParams params;
-
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorVelocity;
- params.elevator_max_acceleration = kElevatorAcceleration;
-
- params.arm_angle = angle;
- params.arm_max_velocity = kArmVelocity;
- params.arm_max_acceleration = kArmAcceleration;
-
- params.top_front_grabber = grabbers;
- params.top_back_grabber = grabbers;
- params.bottom_front_grabber = grabbers;
- params.bottom_back_grabber = grabbers;
-
- ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
- profile->Start();
- profile->WaitUntilDone();
-}
-
-} // namespace
+ : FridgeActorBase<LiftActionQueueGroup>(queues) {}
bool LiftActor::RunAction(const LiftParams ¶ms) {
// Lift the box straight up.
- DoProfile(params.lift_height, 0.0, true);
+ DoFridgeProfile(params.lift_height, 0.0, kElevatorMove, kArmMove, true);
// Move it back to the storage location.
- DoProfile(params.lift_height, params.lift_arm, true);
+ DoFridgeProfile(params.lift_height, params.lift_arm, kElevatorMove, kArmMove,
+ true);
return true;
}
diff --git a/frc971/actors/lift_actor.h b/frc971/actors/lift_actor.h
index 5f5f57e..86af35c 100644
--- a/frc971/actors/lift_actor.h
+++ b/frc971/actors/lift_actor.h
@@ -8,11 +8,12 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
#include "frc971/actors/lift_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class LiftActor : public aos::common::actions::ActorBase<LiftActionQueueGroup> {
+class LiftActor : public FridgeActorBase<LiftActionQueueGroup> {
public:
explicit LiftActor(LiftActionQueueGroup *queues);
diff --git a/frc971/actors/pickup_actor.cc b/frc971/actors/pickup_actor.cc
index f0ea383..c0c3b2a 100644
--- a/frc971/actors/pickup_actor.cc
+++ b/frc971/actors/pickup_actor.cc
@@ -1,13 +1,11 @@
#include "frc971/actors/pickup_actor.h"
-#include <math.h>
+#include <cmath>
#include "aos/common/logging/logging.h"
#include "aos/common/controls/control_loop.h"
#include "aos/common/util/phased_loop.h"
#include "aos/common/time.h"
-#include "frc971/actors/fridge_profile_actor.h"
-#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
namespace frc971 {
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index 4137537..85c3eff 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -5,68 +5,38 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/phased_loop.h"
-#include "frc971/actors/fridge_profile_actor.h"
+#include "frc971/actors/fridge_profile_lib.h"
#include "frc971/constants.h"
namespace frc971 {
namespace actors {
namespace {
-
-// TODO(danielp): Real numbers!
-constexpr double kElevatorMaxVelocity = 0.5;
-constexpr double kArmMaxVelocity = 0.5;
-constexpr double kElevatorMaxAccel = 0.25;
-constexpr double kArmMaxAccel = 0.25;
-
+constexpr ProfileParams kElevatorMove{0.5, 2.0};
+constexpr ProfileParams kArmMove{0.5, 1.0};
} // namespace
ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
- : aos::common::actions::ActorBase<ScoreActionQueueGroup>(queues) {}
-
-void ScoreActor::DoProfile(double height, double angle, bool grabbers) {
- FridgeProfileParams params;
-
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorMaxVelocity;
- params.elevator_max_acceleration = kElevatorMaxAccel;
-
- params.arm_angle = angle;
- params.arm_max_velocity = kArmMaxVelocity;
- params.arm_max_acceleration = kArmMaxAccel;
-
- params.top_front_grabber = grabbers;
- params.top_back_grabber = grabbers;
- params.bottom_front_grabber = grabbers;
- params.bottom_back_grabber = grabbers;
-
- ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
- profile->Start();
- while (!profile->CheckIteration()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
- if (ShouldCancel()) {
- LOG(WARNING, "Cancelling profile.\n");
- profile->Cancel();
- return;
- }
- }
-}
+ : FridgeActorBase<ScoreActionQueueGroup>(queues) {}
bool ScoreActor::RunAction(const ScoreParams& params) {
const auto& values = constants::GetValues();
// We're going to move the elevator first so we don't crash the fridge into
// the ground.
- DoProfile(values.fridge.arm_zeroing_height, 0.0, true);
+ DoFridgeProfile(values.fridge.arm_zeroing_height, 0.0, kElevatorMove,
+ kArmMove, true);
if (ShouldCancel()) return true;
// Now move them both together.
- DoProfile(params.height, M_PI / 2.0, true);
+ DoFridgeProfile(params.height, M_PI / 2.0, kElevatorMove, kArmMove, true);
if (ShouldCancel()) return true;
// Release the totes.
- DoProfile(values.fridge.arm_zeroing_height, 0.0, false);
+ DoFridgeProfile(values.fridge.arm_zeroing_height, 0.0, kElevatorMove,
+ kArmMove, false);
if (ShouldCancel()) return true;
// Retract. Move back to our lowered position.
- DoProfile(values.fridge.elevator.lower_limit, 0.0, false);
+ DoFridgeProfile(values.fridge.elevator.lower_limit, 0.0, kElevatorMove,
+ kArmMove, false);
if (ShouldCancel()) return true;
return true;
diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
index 6a48f6d..35b30af 100644
--- a/frc971/actors/score_actor.h
+++ b/frc971/actors/score_actor.h
@@ -4,12 +4,12 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
#include "frc971/actors/score_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class ScoreActor
- : public aos::common::actions::ActorBase<ScoreActionQueueGroup> {
+class ScoreActor : public FridgeActorBase<ScoreActionQueueGroup> {
public:
explicit ScoreActor(ScoreActionQueueGroup *queues);
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index ac2d744..7267a4b 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -1,64 +1,37 @@
+#include "frc971/actors/stack_actor.h"
+
#include <math.h>
#include "aos/common/time.h"
#include "aos/common/util/phased_loop.h"
-#include "frc971/actors/stack_actor.h"
-#include "frc971/actors/fridge_profile_actor.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
namespace frc971 {
namespace actors {
namespace {
+constexpr ProfileParams kSlowArmMove{0.8, 1.4};
+constexpr ProfileParams kSlowElevatorMove{0.5, 3.0};
+constexpr ProfileParams kReallySlowElevatorMove{0.10, 1.0};
-static constexpr double kArmVelocity = 0.40;
-static constexpr double kArmAcceleration = 1.0;
-static constexpr double kElevatorVelocity = 0.5;
-static constexpr double kElevatorAcceleration = 2.2;
-
+constexpr ProfileParams kFastArmMove{0.8, 4.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
} // namespace
StackActor::StackActor(StackActionQueueGroup *queues)
- : aos::common::actions::ActorBase<StackActionQueueGroup>(queues) {}
-
-void StackActor::DoProfile(double height, double angle, bool grabbers) {
- FridgeProfileParams params;
-
- params.elevator_height = height;
- params.elevator_max_velocity = kElevatorVelocity;
- params.elevator_max_acceleration = kElevatorAcceleration;
-
- params.arm_angle = angle;
- params.arm_max_velocity = kArmVelocity;
- params.arm_max_acceleration = kArmAcceleration;
-
- params.top_front_grabber = grabbers;
- params.top_back_grabber = grabbers;
- params.bottom_front_grabber = grabbers;
- params.bottom_back_grabber = grabbers;
-
- ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
- profile->Start();
- while (!profile->CheckIteration()) {
- // wait until next Xms tick
- ::aos::time::PhasedLoopXMS(5, 2500);
- if (ShouldCancel()) {
- profile->Cancel();
- return;
- }
- }
-}
+ : FridgeActorBase<StackActionQueueGroup>(queues) {}
bool StackActor::RunAction(const StackParams ¶ms) {
const auto &values = constants::GetValues();
const double bottom = 0.020;
// Set the current stack down on top of the bottom box.
- DoProfile(0.45, 0.0, true);
+ DoFridgeProfile(0.39, 0.0, kSlowArmMove, kReallySlowElevatorMove, true);
if (ShouldCancel()) return true;
- // Move down to enclose bottom box.
- DoProfile(bottom + values.tote_height, 0.0, true);
+ // Set down on the box.
+ DoFridgeProfile(bottom + values.tote_height, 0.0, kSlowArmMove,
+ kSlowElevatorMove, true);
if (ShouldCancel()) return true;
// Clamp.
{
@@ -71,9 +44,9 @@
LOG_STRUCT(DEBUG, "Sending claw goal", *message);
message.Send();
}
- DoProfile(bottom, -0.05, false);
+ DoFridgeProfile(bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
if (ShouldCancel()) return true;
- DoProfile(bottom, 0.0, false);
+ DoFridgeProfile(bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
if (ShouldCancel()) return true;
aos::time::SleepFor(aos::time::Time::InMS(100));
diff --git a/frc971/actors/stack_actor.h b/frc971/actors/stack_actor.h
index e45bac1..0d637bc 100644
--- a/frc971/actors/stack_actor.h
+++ b/frc971/actors/stack_actor.h
@@ -8,17 +8,15 @@
#include "aos/common/actions/actions.h"
#include "aos/common/actions/actor.h"
#include "frc971/actors/stack_action.q.h"
+#include "frc971/actors/fridge_profile_lib.h"
namespace frc971 {
namespace actors {
-class StackActor
- : public aos::common::actions::ActorBase<StackActionQueueGroup> {
+class StackActor : public FridgeActorBase<StackActionQueueGroup> {
public:
explicit StackActor(StackActionQueueGroup *queues);
- void DoProfile(double height, double angle, bool grabbers);
-
bool RunAction(const StackParams ¶ms) override;
};
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index 8164b7d..bf95418 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -37,13 +37,12 @@
'<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
- '<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_action_queue',
- '<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:stack_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:lift_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:can_pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:horizontal_can_pickup_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_lib',
],
},
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index ea07798..c529054 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -17,7 +17,6 @@
#include "frc971/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
-#include "frc971/actors/fridge_profile_actor.h"
#include "frc971/actors/pickup_actor.h"
#include "frc971/actors/stack_actor.h"
#include "frc971/actors/lift_actor.h"
@@ -38,10 +37,8 @@
namespace joysticks {
// preset motion limits
-static const double kArmDebugVelocity = 0.40;
-static const double kArmDebugAcceleration = 1.0;
-static const double kElevatorDebugVelocity = 0.5;
-static const double kElevatorDebugAcceleration = 2.2;
+constexpr actors::ProfileParams kArmMove{1.00, 1.0};
+constexpr actors::ProfileParams kElevatorMove{1.00, 3.2};
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
@@ -191,21 +188,8 @@
if (data.PosEdge(kElevatorDown)) {
claw_goal_ = 0.0;
-
- actors::FridgeProfileParams fridge_params;
- fridge_params.arm_max_velocity = kArmDebugVelocity;
- fridge_params.arm_max_acceleration = kArmDebugAcceleration;
- fridge_params.elevator_max_velocity = kElevatorDebugVelocity;
- fridge_params.elevator_max_acceleration = kElevatorDebugAcceleration;
-
- fridge_params.arm_angle = 0.0;
- fridge_params.elevator_height = 0.035;
-
- fridge_params.top_front_grabber = fridge_closed_;
- fridge_params.top_back_grabber = fridge_closed_;
- fridge_params.bottom_front_grabber = fridge_closed_;
- fridge_params.bottom_back_grabber = fridge_closed_;
- action_queue_.EnqueueAction(MakeFridgeProfileAction(fridge_params));
+ arm_goal_ = 0.0;
+ elevator_goal_ = 0.035;
}
if (data.PosEdge(kClawMiddle)) {
@@ -278,10 +262,14 @@
if (!waiting_for_zero_) {
if (!action_queue_.Running()) {
auto new_fridge_goal = fridge_queue.goal.MakeMessage();
+ new_fridge_goal->max_velocity = elevator_params_.velocity;
+ new_fridge_goal->max_acceleration = elevator_params_.acceleration;
new_fridge_goal->height = elevator_goal_;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_params_.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_params_.acceleration;
new_fridge_goal->angle = arm_goal_;
new_fridge_goal->angular_velocity = 0.0;
- new_fridge_goal->velocity = 0.0;
new_fridge_goal->grabbers.top_front = fridge_closed_;
new_fridge_goal->grabbers.top_back = fridge_closed_;
new_fridge_goal->grabbers.bottom_front = fridge_closed_;
@@ -469,6 +457,8 @@
double claw_goal_ = 0.0;
bool claw_rollers_closed_ = false;
bool fridge_closed_ = false;
+ actors::ProfileParams arm_params_ = kArmMove;
+ actors::ProfileParams elevator_params_ = kElevatorMove;
// If we're waiting for the subsystems to zero.
bool waiting_for_zero_ = true;
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index e30bd22..32ada49 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -25,7 +25,6 @@
'../../aos/common/actions/actions.gyp:action_test',
'../actors/actors.gyp:drivetrain_action',
'../actors/actors.gyp:claw_action',
- '../actors/actors.gyp:fridge_profile_action',
'../actors/actors.gyp:score_action',
'../actors/actors.gyp:score_action_test',
'../actors/actors.gyp:pickup_action',
@@ -33,10 +32,7 @@
'../actors/actors.gyp:can_pickup_action',
'../actors/actors.gyp:horizontal_can_pickup_action',
'../actors/actors.gyp:lift_action',
- '../actors/actors.gyp:intake_action',
- '../actors/actors.gyp:intake_action_test',
'../actors/actors.gyp:claw_action_test',
- '../actors/actors.gyp:fridge_profile_action_test',
'../actors/actors.gyp:stack_action_test',
],
'copies': [