Initial stab at hanging actor.
Change-Id: I7d225176962759f8cbcdd42e72fe21eb003349a2
diff --git a/y2016/BUILD b/y2016/BUILD
index 0239d96..4a2e768 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -39,6 +39,7 @@
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/queues:gyro',
'//y2016/actors:autonomous_action_lib',
+ '//y2016/actors:superstructure_action_lib',
'//y2016/actors:vision_align_action_lib',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_lib',
@@ -57,6 +58,7 @@
'//y2016/control_loops/shooter:shooter',
'//y2016/dashboard:dashboard',
'//y2016/actors:autonomous_action',
+ '//y2016/actors:superstructure_action',
'//y2016/actors:vision_align_action',
'//y2016/wpilib:wpilib_interface',
'//y2016/vision:target_receiver',
@@ -79,6 +81,7 @@
'//y2016/control_loops/shooter:shooter.stripped',
'//y2016/dashboard:dashboard.stripped',
'//y2016/actors:autonomous_action.stripped',
+ '//y2016/actors:superstructure_action.stripped',
'//y2016/actors:vision_align_action.stripped',
'//y2016/wpilib:wpilib_interface.stripped',
'//y2016/vision:target_receiver.stripped',
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index d547c7b..4d0f92c 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -322,6 +322,8 @@
new_superstructure_goal->traverse_unlatched = true;
new_superstructure_goal->traverse_down = !traverse_up;
+ new_superstructure_goal->voltage_climber = 0.0;
+ new_superstructure_goal->unfold_climber = false;
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
diff --git a/y2016/actors/superstructure_action.q b/y2016/actors/superstructure_action.q
index 72bd310..2da9202 100644
--- a/y2016/actors/superstructure_action.q
+++ b/y2016/actors/superstructure_action.q
@@ -4,7 +4,9 @@
// Parameters to send with start.
struct SuperstructureActionParams {
- double value;
+ double partial_angle;
+ double delay_time;
+ double full_angle;
};
queue_group SuperstructureActionQueueGroup {
diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index b8ad33a..3b0c258 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -1,5 +1,7 @@
#include "y2016/actors/superstructure_actor.h"
+#include <cmath>
+
#include "aos/common/util/phased_loop.h"
#include "aos/common/logging/logging.h"
#include "y2016/actors/superstructure_actor.h"
@@ -9,22 +11,99 @@
namespace actors {
SuperstructureActor::SuperstructureActor(
- actors::SuperstructureActionQueueGroup* s)
+ actors::SuperstructureActionQueueGroup *s)
: aos::common::actions::ActorBase<actors::SuperstructureActionQueueGroup>(
s) {}
bool SuperstructureActor::RunAction(
- const actors::SuperstructureActionParams& params) {
- LOG(INFO, "Starting superstructure action with value %f", params.value);
+ const actors::SuperstructureActionParams ¶ms) {
+ LOG(INFO, "Starting superstructure action\n");
- while (true) {
- control_loops::superstructure_queue.status.FetchLatest();
- ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
- ::aos::time::Time::InMS(5) / 2);
- break;
+ MoveSuperstructure(params.partial_angle, false);
+ WaitForSuperstructure();
+ if (ShouldCancel()) return true;
+ MoveSuperstructure(params.partial_angle, true);
+ if (!WaitOrCancel(::aos::time::Time::InSeconds(params.delay_time))) return true;
+ MoveSuperstructure(params.full_angle, true);
+ WaitForSuperstructure();
+ if (ShouldCancel()) return true;
+ return true;
+}
+
+void SuperstructureActor::MoveSuperstructure(double shoulder,
+ bool unfold_climber) {
+ superstructure_goal_ = {0, shoulder, 0};
+
+ auto new_superstructure_goal =
+ ::y2016::control_loops::superstructure_queue.goal.MakeMessage();
+
+ new_superstructure_goal->angle_intake = 0;
+ new_superstructure_goal->angle_shoulder = shoulder;
+ new_superstructure_goal->angle_wrist = 0;
+
+ new_superstructure_goal->max_angular_velocity_intake = 7.0;
+ new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
+ new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+
+ new_superstructure_goal->max_angular_acceleration_intake = 40.0;
+ new_superstructure_goal->max_angular_acceleration_shoulder = 5.0;
+ new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+
+ new_superstructure_goal->voltage_top_rollers = 0;
+ new_superstructure_goal->voltage_bottom_rollers = 0;
+
+ new_superstructure_goal->traverse_unlatched = true;
+ new_superstructure_goal->traverse_down = false;
+ new_superstructure_goal->voltage_climber = 0.0;
+ new_superstructure_goal->unfold_climber = unfold_climber;
+
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure move failed.\n");
+ }
+}
+
+bool SuperstructureActor::SuperstructureProfileDone() {
+ constexpr double kProfileError = 1e-2;
+ return ::std::abs(
+ control_loops::superstructure_queue.status->intake.goal_angle -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(
+ control_loops::superstructure_queue.status->shoulder.goal_angle -
+ superstructure_goal_.shoulder) < kProfileError &&
+ ::std::abs(
+ control_loops::superstructure_queue.status->wrist.goal_angle -
+ superstructure_goal_.wrist) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->intake
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->shoulder
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(control_loops::superstructure_queue.status->wrist
+ .goal_angular_velocity) < kProfileError;
+}
+
+bool SuperstructureActor::SuperstructureDone() {
+ control_loops::superstructure_queue.status.FetchAnother();
+
+ // We are no longer running if we are in the zeroing states (below 12), or
+ // estopped.
+ if (control_loops::superstructure_queue.status->state < 12 ||
+ control_loops::superstructure_queue.status->state == 16) {
+ LOG(ERROR, "Superstructure no longer running, aborting action\n");
+ return true;
}
- return true;
+ if (SuperstructureProfileDone()) {
+ LOG(DEBUG, "Profile done.\n");
+ return true;
+ }
+ return false;
+}
+
+void SuperstructureActor::WaitForSuperstructure() {
+ while (true) {
+ if (ShouldCancel()) return;
+ if (SuperstructureDone()) return;
+ }
}
::std::unique_ptr<SuperstructureAction> MakeSuperstructureAction(
diff --git a/y2016/actors/superstructure_actor.h b/y2016/actors/superstructure_actor.h
index b882585..a8a0cc4 100644
--- a/y2016/actors/superstructure_actor.h
+++ b/y2016/actors/superstructure_actor.h
@@ -13,9 +13,21 @@
class SuperstructureActor
: public ::aos::common::actions::ActorBase<SuperstructureActionQueueGroup> {
public:
- explicit SuperstructureActor(SuperstructureActionQueueGroup* s);
+ explicit SuperstructureActor(SuperstructureActionQueueGroup *s);
- bool RunAction(const actors::SuperstructureActionParams& params) override;
+ // Internal struct holding superstructure goals sent by autonomous to the
+ // loop.
+ struct SuperstructureGoal {
+ double intake;
+ double shoulder;
+ double wrist;
+ };
+ SuperstructureGoal superstructure_goal_;
+ bool RunAction(const actors::SuperstructureActionParams ¶ms) override;
+ void MoveSuperstructure(double shoulder, bool unfold_climber);
+ void WaitForSuperstructure();
+ bool SuperstructureProfileDone();
+ bool SuperstructureDone();
};
using SuperstructureAction =
@@ -23,7 +35,7 @@
// Makes a new SuperstructureActor action.
::std::unique_ptr<SuperstructureAction> MakeSuperstructureAction(
- const ::y2016::actors::SuperstructureActionParams& params);
+ const ::y2016::actors::SuperstructureActionParams ¶ms);
} // namespace actors
} // namespace y2016
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 75f0c59..4d5bbb9 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -379,7 +379,7 @@
scenario_plotter = ScenarioPlotter()
- J_accelerating = 12
+ J_accelerating = 15
J_decelerating = 5
arm = Arm(name='AcceleratingArm', J=J_accelerating)
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index cc2dfaf..d99160d 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -42,7 +42,7 @@
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
- self.J = 0.34 + 0.65
+ self.J = 0.34 + 0.40
# Control loop time step
self.dt = 0.005
@@ -73,7 +73,7 @@
glog.debug("Free speed is %f", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.20
- q_vel = 5.5
+ q_vel = 5.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index fb54a95..d90ec68 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -627,7 +627,7 @@
arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage, max_voltage);
intake_.set_max_voltage(max_voltage);
- if (IsRunning()) {
+ if (IsRunning() && !kill_shoulder_) {
// We don't want lots of negative voltage when we are near the bellypan on
// the shoulder...
// TODO(austin): Do I want to push negative power into the belly pan at this
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 859a77b..f1ae333 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -23,6 +23,7 @@
#include "frc971/autonomous/auto.q.h"
#include "y2016/actors/autonomous_actor.h"
#include "y2016/actors/vision_align_actor.h"
+#include "y2016/actors/superstructure_actor.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain_queue;
@@ -55,7 +56,6 @@
// Buttons on the lexan driver station to get things running on bring-up day.
const ButtonLocation kIntakeDown(3, 11);
const POVLocation kFrontLong(3, 180);
-const ButtonLocation kHigherFrontLong(3, 6);
const POVLocation kBackLong(3, 0);
const POVLocation kBackFender(3, 90);
const POVLocation kFrontFender(3, 270);
@@ -67,6 +67,9 @@
const ButtonLocation kVisionAlign(3, 4);
+const ButtonLocation kExpand(3, 6);
+const ButtonLocation kWinch(3, 5);
+
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
@@ -182,6 +185,7 @@
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ float voltage_climber = 0.0;
// Default the intake to up.
intake_goal_ = constants::Values::kIntakeRange.upper - 0.04;
@@ -215,16 +219,7 @@
}
}
- if (data.IsPressed(kHigherFrontLong)) {
- // Forwards shot
- shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.43 + 0.02;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
- }
- shooter_velocity_ = 640.0;
- intake_goal_ = intake_when_shooting;
- } else if (data.IsPressed(kFrontLong)) {
+ if (data.IsPressed(kFrontLong)) {
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
wrist_goal_ = M_PI + 0.41 + 0.02;
@@ -254,11 +249,32 @@
wrist_goal_ = 2.5 + 1.7;
shooter_velocity_ = 550.0;
intake_goal_ = intake_when_shooting;
+ } else if (data.IsPressed(kExpand) || data.IsPressed(kWinch)) {
+ // Set the goals to the hanging position so when the actor finishes, we
+ // will still be at the right spot.
+ shoulder_goal_ = 1.2;
+ wrist_goal_ = 0.0;
+ intake_goal_ = 0.0;
+ if (data.PosEdge(kExpand)) {
+ is_expanding_ = true;
+ actors::SuperstructureActionParams params;
+ params.partial_angle = 0.3;
+ params.delay_time = 0.7;
+ params.full_angle = shoulder_goal_;
+ action_queue_.EnqueueAction(actors::MakeSuperstructureAction(params));
+ }
+ if (data.IsPressed(kWinch)) {
+ voltage_climber = 12.0;
+ }
} else {
wrist_goal_ = 0.0;
shoulder_goal_ = -0.010;
shooter_velocity_ = 0.0;
}
+ if (data.NegEdge(kExpand) || voltage_climber > 1.0) {
+ is_expanding_ = false;
+ action_queue_.CancelAllActions();
+ }
bool ball_detected = false;
::y2016::sensors::ball_detector.FetchLatest();
@@ -344,55 +360,59 @@
}
if (!waiting_for_zero_) {
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
- new_superstructure_goal->angle_intake = intake_goal_;
- new_superstructure_goal->angle_shoulder = shoulder_goal_;
- new_superstructure_goal->angle_wrist = wrist_goal_;
+ if (!is_expanding_) {
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->angle_intake = intake_goal_;
+ new_superstructure_goal->angle_shoulder = shoulder_goal_;
+ new_superstructure_goal->angle_wrist = wrist_goal_;
- new_superstructure_goal->max_angular_velocity_intake = 7.0;
- new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
- new_superstructure_goal->max_angular_velocity_wrist = 10.0;
- if (use_slow_profile) {
- new_superstructure_goal->max_angular_acceleration_intake = 10.0;
- } else {
- new_superstructure_goal->max_angular_acceleration_intake = 40.0;
- }
- new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
- if (shoulder_goal_ > 1.0) {
- new_superstructure_goal->max_angular_acceleration_wrist = 45.0;
- } else {
- new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
- }
+ new_superstructure_goal->max_angular_velocity_intake = 7.0;
+ new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
+ new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+ if (use_slow_profile) {
+ new_superstructure_goal->max_angular_acceleration_intake = 10.0;
+ } else {
+ new_superstructure_goal->max_angular_acceleration_intake = 40.0;
+ }
+ new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
+ if (shoulder_goal_ > 1.0) {
+ new_superstructure_goal->max_angular_acceleration_wrist = 45.0;
+ } else {
+ new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+ }
- // Granny mode
- /*
- new_superstructure_goal->max_angular_velocity_intake = 0.2;
- new_superstructure_goal->max_angular_velocity_shoulder = 0.2;
- new_superstructure_goal->max_angular_velocity_wrist = 0.2;
- new_superstructure_goal->max_angular_acceleration_intake = 1.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 1.0;
- new_superstructure_goal->max_angular_acceleration_wrist = 1.0;
- */
- if (is_intaking_) {
- new_superstructure_goal->voltage_top_rollers = 12.0;
- new_superstructure_goal->voltage_bottom_rollers = 12.0;
- } else if (is_outtaking_) {
- new_superstructure_goal->voltage_top_rollers = -12.0;
- new_superstructure_goal->voltage_bottom_rollers = -7.0;
- } else {
- new_superstructure_goal->voltage_top_rollers = 0.0;
- new_superstructure_goal->voltage_bottom_rollers = 0.0;
- }
+ // Granny mode
+ /*
+ new_superstructure_goal->max_angular_velocity_intake = 0.2;
+ new_superstructure_goal->max_angular_velocity_shoulder = 0.2;
+ new_superstructure_goal->max_angular_velocity_wrist = 0.2;
+ new_superstructure_goal->max_angular_acceleration_intake = 1.0;
+ new_superstructure_goal->max_angular_acceleration_shoulder = 1.0;
+ new_superstructure_goal->max_angular_acceleration_wrist = 1.0;
+ */
+ if (is_intaking_) {
+ new_superstructure_goal->voltage_top_rollers = 12.0;
+ new_superstructure_goal->voltage_bottom_rollers = 12.0;
+ } else if (is_outtaking_) {
+ new_superstructure_goal->voltage_top_rollers = -12.0;
+ new_superstructure_goal->voltage_bottom_rollers = -7.0;
+ } else {
+ new_superstructure_goal->voltage_top_rollers = 0.0;
+ new_superstructure_goal->voltage_bottom_rollers = 0.0;
+ }
- new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
- new_superstructure_goal->traverse_down = traverse_down_;
- new_superstructure_goal->force_intake = true;
+ new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
+ new_superstructure_goal->unfold_climber = false;
+ new_superstructure_goal->voltage_climber = voltage_climber;
+ new_superstructure_goal->traverse_down = traverse_down_;
+ new_superstructure_goal->force_intake = true;
- if (!new_superstructure_goal.Send()) {
- LOG(ERROR, "Sending superstructure goal failed.\n");
- } else {
- LOG(DEBUG, "sending goals: intake: %f, shoulder: %f, wrist: %f\n",
- intake_goal_, shoulder_goal_, wrist_goal_);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ } else {
+ LOG(DEBUG, "sending goals: intake: %f, shoulder: %f, wrist: %f\n",
+ intake_goal_, shoulder_goal_, wrist_goal_);
+ }
}
if (!shooter_queue.goal.MakeWithBuilder()
@@ -462,6 +482,8 @@
const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;
+ bool is_expanding_ = false;
+
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
"no drivetrain status");