Add score action.
It's pretty basic right now...
Change-Id: I1fd82e9b0f05d2219cff5e947a0f4f2750956726
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 239ba73..77ffe8f 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -122,5 +122,50 @@
'fridge_profile_action_lib',
],
},
+ {
+ 'target_name': 'score_action_queue',
+ 'type': 'static_library',
+ 'sources': ['score_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': 'score_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'score_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_action_lib',
+ 'score_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'score_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'score_action',
+ 'type': 'executable',
+ 'sources': [
+ 'score_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'score_action_queue',
+ 'score_action_lib',
+ ],
+ },
],
}
diff --git a/frc971/actors/score_action.q b/frc971/actors/score_action.q
new file mode 100644
index 0000000..3e99a75
--- /dev/null
+++ b/frc971/actors/score_action.q
@@ -0,0 +1,23 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct ScoreParams {
+ // Height for the elevator to be when we're scoring.
+ double height;
+};
+
+queue_group ScoreActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ ScoreParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group ScoreActionQueueGroup score_action;
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
new file mode 100644
index 0000000..44d829b
--- /dev/null
+++ b/frc971/actors/score_actor.cc
@@ -0,0 +1,71 @@
+#include "frc971/actors/score_actor.h"
+
+#include <math.h>
+
+#include "aos/common/logging/logging.h"
+#include "frc971/actors/fridge_profile_actor.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;
+
+} // namespace
+
+ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
+ : aos::common::actions::ActorBase<ScoreActionQueueGroup>(queues) {}
+
+namespace {
+
+void 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();
+ profile->WaitUntilDone();
+}
+
+} // namespace
+
+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, M_PI / 2.0, true);
+ // Now move them both together.
+ DoProfile(params.height, M_PI, true);
+ // Release the totes and retract.
+ DoProfile(values.fridge.arm_zeroing_height, M_PI / 2.0, false);
+ DoProfile(values.fridge.normal_height, M_PI / 2.0, false);
+
+ return true;
+}
+
+::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams& params) {
+ return ::std::unique_ptr<ScoreAction>(
+ new ScoreAction(&::frc971::actors::score_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
new file mode 100644
index 0000000..0d43636
--- /dev/null
+++ b/frc971/actors/score_actor.h
@@ -0,0 +1,27 @@
+#ifndef FRC971_ACTORS_SCORE_ACTOR_H_
+#define FRC971_ACTORS_SCORE_ACTOR_H_
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/score_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class ScoreActor
+ : public aos::common::actions::ActorBase<ScoreActionQueueGroup> {
+ public:
+ explicit ScoreActor(ScoreActionQueueGroup *queues);
+
+ bool RunAction(const ScoreParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<ScoreActionQueueGroup> ScoreAction;
+
+// Makes a new ScoreActor action.
+::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/frc971/actors/score_actor_main.cc b/frc971/actors/score_actor_main.cc
new file mode 100644
index 0000000..ce4548c
--- /dev/null
+++ b/frc971/actors/score_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/score_action.q.h"
+#include "frc971/actors/score_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actors::ScoreActor score(&::frc971::actors::score_action);
+ score.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 56798a2..a8898a8 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -39,8 +39,10 @@
const ShifterHallEffect kCompRightDriveShifter{555, 657, 660, 560, 0.2, 0.7};
const ShifterHallEffect kCompLeftDriveShifter{555, 660, 644, 552, 0.2, 0.7};
-const ShifterHallEffect kPracticeRightDriveShifter{2.95, 3.95, 3.95, 2.95, 0.2, 0.7};
-const ShifterHallEffect kPracticeLeftDriveShifter{2.95, 4.2, 3.95, 3.0, 0.2, 0.7};
+const ShifterHallEffect kPracticeRightDriveShifter{2.95, 3.95, 3.95,
+ 2.95, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{2.95, 4.2, 3.95,
+ 3.0, 0.2, 0.7};
// Set by Daniel on 2/13/15.
// Distance from the center of the left wheel to the center of the right wheel.
@@ -56,8 +58,9 @@
(2.0 * M_PI);
const double kArmZeroingHeight = 0.2;
+const double kElevatorNormalHeight = 0.1;
-const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
+const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
// Gearing ratios of the pots and encoders for the elevator and arm.
@@ -71,11 +74,10 @@
const double kClawPotRatio = 18.0 / 72.0;
// Number of radians between each index pulse on the arm.
-const double kArmEncoderIndexDifference = 2.0 * M_PI * kArmEncoderRatio;
+const double kArmEncoderIndexDifference = 2.0 * M_PI * kArmEncoderRatio;
// Number of meters between each index pulse on the elevator.
const double kElevatorEncoderIndexDifference =
- kElevatorEncoderRatio *
- 2.0 * M_PI * // radians
+ kElevatorEncoderRatio * 2.0 * M_PI * // radians
kElevatorGearboxOutputRadianDistance;
// Number of radians between index pulses on the claw.
const double kClawEncoderIndexDifference = 2.0 * M_PI * kClawEncoderRatio;
@@ -112,45 +114,43 @@
// Motion ranges: hard_lower_limit, hard_upper_limit,
// soft_lower_limit, soft_upper_limit
// TODO(sensors): Get actual bounds before turning on robot.
- {
- // Claw values, in radians.
- // 0 is level with the ground.
- // Positive moves in the direction of positive encoder values.
- {0.0000000000, 1.5700000000,
- 0.1000000000, 1.2000000000},
+ {// Claw values, in radians.
+ // 0 is level with the ground.
+ // Positive moves in the direction of positive encoder values.
+ {0.0000000000, 1.5700000000, 0.1000000000, 1.2000000000},
- // Zeroing constants for wrist.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.35},
+ // Zeroing constants for wrist.
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.35},
- 0.0,
- kClawPistonSwitchTime,
- kClawZeroingRange
- },
+ 0.0,
+ kClawPistonSwitchTime,
+ kClawZeroingRange},
- {
- // Elevator values, in meters.
- // TODO(austin): Fix this. Positive is up.
- // 0 is at the top of the elevator frame.
- // Positive is down towards the drivebase.
- {0.0000000000, 0.6790000000,
- 0.2000000000, 0.6000000000},
+ {// Elevator values, in meters.
+ // TODO(austin): Fix this. Positive is up.
+ // 0 is at the top of the elevator frame.
+ // Positive is down towards the drivebase.
+ {0.0000000000, 0.6790000000, 0.2000000000, 0.6000000000},
- // Arm values, in radians.
- // 0 is sticking straight out horizontally over the intake/front.
- // Positive is rotating up and into the robot (towards the back).
- {-1.570000000, 1.5700000000,
- -1.200000000, 1.2000000000},
+ // Arm values, in radians.
+ // 0 is sticking straight out horizontally over the intake/front.
+ // Positive is rotating up and into the robot (towards the back).
+ {-1.570000000, 1.5700000000, -1.200000000, 1.2000000000},
- // Elevator zeroing constants: left, right.
- // TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
- // Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
- 0.0, 0.0, 0.0, 0.0,
+ // Elevator zeroing constants: left, right.
+ // TODO(sensors): Get actual offsets for these.
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
+ // Arm zeroing constants: left, right.
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
- kArmZeroingHeight,
+ kArmZeroingHeight,
+ kElevatorNormalHeight,
},
// End "sensor" values.
@@ -217,7 +217,9 @@
-0.081354,
-3.509611 - 0.007415 - -0.019081,
3.506927 - 0.170017 - -0.147970,
- kArmZeroingHeight,
+
+ kArmZeroingHeight,
+ kElevatorNormalHeight,
},
// End "sensor" values.
@@ -260,8 +262,7 @@
6.1663463999999992,
kClawPistonSwitchTime,
- kClawZeroingRange
- },
+ kClawZeroingRange},
{// Elevator values, in meters.
// 0 is at the top of the elevator frame.
@@ -275,8 +276,10 @@
// Elevator zeroing constants: left, right.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.016041 + 0.001290},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.011367 + 0.003216},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference,
+ 0.016041 + 0.001290},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference,
+ 0.011367 + 0.003216},
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, -0.312677},
{kZeroingSampleSize, kArmEncoderIndexDifference, -0.40855},
@@ -286,6 +289,7 @@
3.5263507647058816 - 0.018921 + 0.006545,
kArmZeroingHeight,
+ kElevatorNormalHeight,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index e12a27f..ac6f315 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -105,6 +105,8 @@
// How high the elevator has to be before we start zeroing the arm.
double arm_zeroing_height;
+ // The height at which the fridge should hang out by default.
+ double normal_height;
};
Fridge fridge;
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 94aa40b..699d772 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -21,6 +21,7 @@
'../../aos/common/actions/actions.gyp:action_test',
'../actors/actors.gyp:drivetrain_action',
'../actors/actors.gyp:fridge_profile_action',
+ '../actors/actors.gyp:score_action',
'../actors/actors.gyp:fridge_profile_action_test',
],
'copies': [