Fridge Profile:
- Added profile action for basic fridge motion.
Change-Id: I170959e349b1048ce975b5348d8f8c474464e1e2
diff --git a/aos/common/actions/action_test.cc b/aos/common/actions/action_test.cc
index df204bc..0db4b29 100644
--- a/aos/common/actions/action_test.cc
+++ b/aos/common/actions/action_test.cc
@@ -23,7 +23,7 @@
explicit TestActorNOP(actions::TestActionQueueGroup* s)
: actions::ActorBase<actions::TestActionQueueGroup>(s) {}
- void RunAction() { return; }
+ bool RunAction() { return true; }
};
::std::unique_ptr<
@@ -41,11 +41,11 @@
explicit TestActorShouldCancel(actions::TestActionQueueGroup* s)
: aos::common::actions::ActorBase<actions::TestActionQueueGroup>(s) {}
- void RunAction() {
+ bool RunAction() {
while (!ShouldCancel()) {
LOG(FATAL, "NOT CANCELED!!\n");
}
- return;
+ return true;
}
};
diff --git a/aos/common/actions/actions.q b/aos/common/actions/actions.q
index fff6326..8950dd1 100644
--- a/aos/common/actions/actions.q
+++ b/aos/common/actions/actions.q
@@ -15,6 +15,9 @@
uint32_t running;
// A run value we were previously running or 0.
uint32_t last_running;
+ // If false the action failed to complete and may be in a bad state,
+ // this is a critical problem not a cancellation.
+ bool success;
};
message Goal {
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
index 63cdb08..b64051a 100644
--- a/aos/common/actions/actor.h
+++ b/aos/common/actions/actor.h
@@ -19,7 +19,10 @@
public:
ActorBase(T* acq) : action_q_(acq) {}
- virtual void RunAction() = 0;
+ // Will return true if finished or asked to cancel.
+ // Will return false if it failed accomplish its goal
+ // due to a problem with the system.
+ virtual bool RunAction() = 0;
// Runs action while enabled.
void Run();
@@ -87,6 +90,7 @@
if (!action_q_->status.MakeWithBuilder()
.running(0)
.last_running(0)
+ .success(!abort_)
.Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
@@ -102,10 +106,11 @@
if (!action_q_->status.MakeWithBuilder()
.running(running_id)
.last_running(0)
+ .success(!abort_)
.Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
- RunAction();
+ abort_ = !RunAction();
LOG(INFO, "Done with action %" PRIx32 "\n", running_id);
// If we have a new one to run, we shouldn't say we're stopped in between.
@@ -113,6 +118,7 @@
if (!action_q_->status.MakeWithBuilder()
.running(0)
.last_running(running_id)
+ .success(!abort_)
.Send()) {
LOG(ERROR, "Failed to send the status.\n");
} else {
@@ -142,7 +148,7 @@
// Make sure the last job is done and we have a signal.
CheckInitialRunning();
- if (!action_q_->status.MakeWithBuilder().running(0).last_running(0).Send()) {
+ if (!action_q_->status.MakeWithBuilder().running(0).last_running(0).success(!abort_).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
deleted file mode 100644
index 4ee8119..0000000
--- a/frc971/actions/actions.gyp
+++ /dev/null
@@ -1,53 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'drivetrain_action_queue',
- 'type': 'static_library',
- 'sources': ['drivetrain_action.q'],
- 'variables': {
- 'header_path': 'frc971/actions',
- },
- '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': 'drivetrain_action_lib',
- 'type': 'static_library',
- 'sources': [
- 'drivetrain_actor.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):eigen',
- '<(AOS)/common/util/util.gyp:phased_loop',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/util/util.gyp:trapezoid_profile',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/actions/actions.gyp:action_lib',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- 'drivetrain_action_queue',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/actions/actions.gyp:action_lib',
- 'drivetrain_action_queue',
- ],
- },
- {
- 'target_name': 'drivetrain_action',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_actor_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'drivetrain_action_queue',
- 'drivetrain_action_lib',
- ],
- },
- ],
-}
diff --git a/frc971/actions/drivetrain_actor_main.cc b/frc971/actions/drivetrain_actor_main.cc
deleted file mode 100644
index 9711065..0000000
--- a/frc971/actions/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "frc971/actions/drivetrain_action.q.h"
-#include "frc971/actions/drivetrain_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
-
- frc971::actions::DrivetrainActor drivetrain(
- &::frc971::actions::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
new file mode 100644
index 0000000..6593d80
--- /dev/null
+++ b/frc971/actors/actors.gyp
@@ -0,0 +1,125 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': ['drivetrain_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': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_actor.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ ],
+ },
+ {
+ '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',
+ ],
+ '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',
+ '<(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',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
+ 'fridge_profile_action_queue',
+ 'fridge_profile_action_lib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actors/drivetrain_action.q
similarity index 94%
rename from frc971/actions/drivetrain_action.q
rename to frc971/actors/drivetrain_action.q
index 5791318..826900a 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actors/drivetrain_action.q
@@ -1,4 +1,4 @@
-package frc971.actions;
+package frc971.actors;
import "aos/common/actions/actions.q";
diff --git a/frc971/actions/drivetrain_actor.cc b/frc971/actors/drivetrain_actor.cc
similarity index 88%
rename from frc971/actions/drivetrain_actor.cc
rename to frc971/actors/drivetrain_actor.cc
index a453502..165e6cb 100644
--- a/frc971/actions/drivetrain_actor.cc
+++ b/frc971/actors/drivetrain_actor.cc
@@ -1,4 +1,4 @@
-#include "frc971/actions/drivetrain_actor.h"
+#include "frc971/actors/drivetrain_actor.h"
#include <functional>
#include <numeric>
@@ -11,17 +11,17 @@
#include "aos/common/commonmath.h"
#include "aos/common/time.h"
-#include "frc971/actions/drivetrain_actor.h"
+#include "frc971/actors/drivetrain_actor.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace frc971 {
-namespace actions {
+namespace actors {
-DrivetrainActor::DrivetrainActor(actions::DrivetrainActionQueueGroup* s)
- : aos::common::actions::ActorBase<actions::DrivetrainActionQueueGroup>(s) {}
+DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
-void DrivetrainActor::RunAction() {
+bool DrivetrainActor::RunAction() {
static const auto K = constants::GetValues().make_drivetrain_loop().K();
const double yoffset = action_q_->goal->y_offset;
@@ -94,7 +94,8 @@
}
} else {
// If we ever get here, that's bad and we should just give up
- LOG(FATAL, "no drivetrain status!\n");
+ LOG(ERROR, "no drivetrain status!\n");
+ return false;
}
const auto drive_profile_goal_state =
@@ -107,7 +108,8 @@
::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
break;
}
- if (ShouldCancel()) return;
+
+ if (ShouldCancel()) return true;
LOG(DEBUG, "Driving left to %f, right to %f\n",
left_goal_state(0, 0) + action_q_->goal->left_initial_position,
@@ -121,16 +123,16 @@
.right_velocity_goal(right_goal_state(1, 0))
.Send();
}
- if (ShouldCancel()) return;
+ if (ShouldCancel()) return true;
control_loops::drivetrain_queue.status.FetchLatest();
while (!control_loops::drivetrain_queue.status.get()) {
LOG(WARNING,
"No previous drivetrain status packet, trying to fetch again\n");
control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return;
+ if (ShouldCancel()) return true;
}
while (true) {
- if (ShouldCancel()) return;
+ if (ShouldCancel()) return true;
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
@@ -151,17 +153,18 @@
control_loops::drivetrain_queue.status.FetchNextBlocking();
}
LOG(INFO, "Done moving\n");
+ return true;
}
::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actions::DrivetrainActionQueueGroup>>
+ ::frc971::actors::DrivetrainActionQueueGroup>>
MakeDrivetrainAction() {
return ::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actions::DrivetrainActionQueueGroup>>(
+ ::frc971::actors::DrivetrainActionQueueGroup>>(
new aos::common::actions::TypedAction<
- ::frc971::actions::DrivetrainActionQueueGroup>(
- &::frc971::actions::drivetrain_action));
+ ::frc971::actors::DrivetrainActionQueueGroup>(
+ &::frc971::actors::drivetrain_action));
}
-} // namespace actions
+} // namespace actors
} // namespace frc971
diff --git a/frc971/actions/drivetrain_actor.h b/frc971/actors/drivetrain_actor.h
similarity index 82%
rename from frc971/actions/drivetrain_actor.h
rename to frc971/actors/drivetrain_actor.h
index f8e2a6e..2ded4ec 100644
--- a/frc971/actions/drivetrain_actor.h
+++ b/frc971/actors/drivetrain_actor.h
@@ -3,26 +3,26 @@
#include <memory>
-#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actors/drivetrain_action.q.h"
#include "aos/common/actions/actor.h"
#include "aos/common/actions/actions.h"
namespace frc971 {
-namespace actions {
+namespace actors {
class DrivetrainActor
: public aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
public:
explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
- void RunAction() override;
+ bool RunAction() override;
};
// Makes a new DrivetrainActor action.
::std::unique_ptr<aos::common::actions::TypedAction<DrivetrainActionQueueGroup>>
MakeDrivetrainAction();
-} // namespace actions
+} // namespace actors
} // namespace frc971
#endif
diff --git a/frc971/actors/drivetrain_actor_main.cc b/frc971/actors/drivetrain_actor_main.cc
new file mode 100644
index 0000000..5337745
--- /dev/null
+++ b/frc971/actors/drivetrain_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/drivetrain_action.q.h"
+#include "frc971/actors/drivetrain_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actors::DrivetrainActor drivetrain(
+ &::frc971::actors::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/actors/fridge_profile_action.q b/frc971/actors/fridge_profile_action.q
new file mode 100644
index 0000000..7e93a5e
--- /dev/null
+++ b/frc971/actors/fridge_profile_action.q
@@ -0,0 +1,26 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+queue_group FridgeProfileActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ 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;
+ };
+
+ 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
new file mode 100644
index 0000000..794fb7f
--- /dev/null
+++ b/frc971/actors/fridge_profile_actor.cc
@@ -0,0 +1,164 @@
+#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 (arm_profile_ != nullptr || elevator_profile_ != nullptr) {
+ return false;
+ }
+ // 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);
+
+ // 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);
+ 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) {
+ ::Eigen::Matrix<double, 2, 1> goal_state;
+
+ goal_state = arm_profile_->Update(goal_angle, 0.0);
+ *next_angle = goal_state(0, 0);
+ *next_angle_velocity = goal_state(1, 0);
+ 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() {
+ double goal_angle = action_q_->goal->arm_angle;
+ double goal_height = action_q_->goal->elevator_height;
+ bool top_front = action_q_->goal->top_front_grabber;
+ bool top_back = action_q_->goal->top_back_grabber;
+ bool bottom_front = action_q_->goal->bottom_front_grabber;
+ bool bottom_back = action_q_->goal->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
+ const double angle_epsilon = 0.01, height_epsilon = 0.01;
+
+ // Initialize arm profile.
+ if(!InitializeProfile(action_q_->goal->arm_max_velocity,
+ action_q_->goal->arm_max_acceleration,
+ action_q_->goal->elevator_max_velocity,
+ action_q_->goal->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->angle;
+ elev_start_height_ = control_loops::fridge_queue.status->height;
+ } else {
+ LOG(ERROR, "No fridge status!\n");
+ return false;
+ }
+
+ while (true) {
+ // wait until next Xms tick
+ ::aos::time::PhasedLoopXMS(5, 2500);
+
+ double delta_angle, delta_height;
+ double angle_vel, height_vel;
+ if (!IterateProfile(goal_angle, goal_height, &delta_angle, &delta_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 = arm_start_angle_ + delta_angle;
+ message->angular_velocity = angle_vel;
+ message->height = elev_start_height_ + delta_height;
+ message->velocity = height_vel;
+ message->grabbers.top_front = top_front;
+ message->grabbers.top_back = top_back;
+ message->grabbers.bottom_front = top_front;
+ message->grabbers.top_front = top_front;
+
+ LOG_STRUCT(DEBUG, "Sending fridge goal", *message);
+ message.Send();
+
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+ const double current_height = control_loops::fridge_queue.status->height;
+ const double current_angle = control_loops::fridge_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got fridge status",
+ *control_loops::fridge_queue.status);
+
+ if (::std::abs(arm_start_angle_ + delta_angle - goal_angle) <
+ angle_epsilon &&
+ ::std::abs(arm_start_angle_ + delta_angle - current_angle) <
+ angle_epsilon &&
+ ::std::abs(elev_start_height_ + delta_height - goal_height) <
+ height_epsilon &&
+ ::std::abs(elev_start_height_ + delta_height - current_height) <
+ height_epsilon) {
+ break;
+ }
+ }
+
+ 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<aos::common::actions::TypedAction<
+ ::frc971::actors::FridgeProfileActionQueueGroup>>
+MakeFridgeProfileAction() {
+ return ::std::unique_ptr<aos::common::actions::TypedAction<
+ ::frc971::actors::FridgeProfileActionQueueGroup>>(
+ new aos::common::actions::TypedAction<
+ ::frc971::actors::FridgeProfileActionQueueGroup>(
+ &::frc971::actors::fridge_profile_action));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/fridge_profile_actor.h b/frc971/actors/fridge_profile_actor.h
new file mode 100644
index 0000000..7da3cbd
--- /dev/null
+++ b/frc971/actors/fridge_profile_actor.h
@@ -0,0 +1,46 @@
+#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() override;
+
+ 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;
+};
+
+// Makes a new FridgeProfileActor action.
+::std::unique_ptr<aos::common::actions::TypedAction<FridgeProfileActionQueueGroup>>
+ MakeFridgeProfileAction();
+
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/frc971/actors/fridge_profile_actor_main.cc b/frc971/actors/fridge_profile_actor_main.cc
new file mode 100644
index 0000000..24cf75e
--- /dev/null
+++ b/frc971/actors/fridge_profile_actor_main.cc
@@ -0,0 +1,19 @@
+#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
new file mode 100644
index 0000000..cbebec2
--- /dev/null
+++ b/frc971/actors/fridge_profile_actor_test.cc
@@ -0,0 +1,315 @@
+#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);
+
+ frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
+ .run(true)
+ .arm_angle(5.0)
+ .arm_max_velocity(200.0)
+ .arm_max_acceleration(20000.0)
+ .elevator_height(5.0)
+ .elevator_max_velocity(200.0)
+ .elevator_max_acceleration(20000.0)
+ .top_front_grabber(true)
+ .top_back_grabber(false)
+ .bottom_front_grabber(true)
+ .bottom_back_grabber(false)
+ .Send();
+
+ // tell it the fridge is zeroed
+ control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send();
+
+ // do the action and it will post to the goal queue
+ fridge_profile.WaitForActionRequest();
+ fridge_profile.RunAction();
+
+ // 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());
+}
+
+} // namespace testing.
+} // namespace actors.
+} // namespace frc971.
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index a5cfd22..d2353fe 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -11,7 +11,7 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/actions/drivetrain_actor.h"
+#include "frc971/actors/drivetrain_actor.h"
using ::aos::time::Time;
@@ -108,11 +108,11 @@
}
::std::unique_ptr<aos::common::actions::TypedAction<
- ::frc971::actions::DrivetrainActionQueueGroup>>
+ ::frc971::actors::DrivetrainActionQueueGroup>>
SetDriveGoal(double distance, bool slow_acceleration,
double maximum_velocity = 1.7, double theta = 0) {
LOG(INFO, "Driving to %f\n", distance);
- auto drivetrain_action = actions::MakeDrivetrainAction();
+ auto drivetrain_action = actors::MakeDrivetrainAction();
drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
drivetrain_action->GetGoal()->y_offset = distance;
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 966884f..6ab2f87 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -24,7 +24,7 @@
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:drivetrain_action_lib',
'<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index e8b0eb5..94aa40b 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -15,11 +15,13 @@
'../control_loops/claw/claw.gyp:claw',
'../control_loops/claw/claw.gyp:claw_lib_test',
'../autonomous/autonomous.gyp:auto',
- '../actions/actions.gyp:drivetrain_action',
'../frc971.gyp:joystick_reader',
'../zeroing/zeroing.gyp:zeroing_test',
'../control_loops/voltage_cap/voltage_cap.gyp:voltage_cap_test',
'../../aos/common/actions/actions.gyp:action_test',
+ '../actors/actors.gyp:drivetrain_action',
+ '../actors/actors.gyp:fridge_profile_action',
+ '../actors/actors.gyp:fridge_profile_action_test',
],
'copies': [
{