Made hanging work.
Change-Id: I02f7601c530387a1d9c03c0995544a8541466231
diff --git a/y2016/actors/superstructure_action.q b/y2016/actors/superstructure_action.q
index 2da9202..231f5e7 100644
--- a/y2016/actors/superstructure_action.q
+++ b/y2016/actors/superstructure_action.q
@@ -7,6 +7,7 @@
double partial_angle;
double delay_time;
double full_angle;
+ double shooter_angle;
};
queue_group SuperstructureActionQueueGroup {
diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index 3b0c258..c2d7c4d 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -19,27 +19,27 @@
const actors::SuperstructureActionParams ¶ms) {
LOG(INFO, "Starting superstructure action\n");
- MoveSuperstructure(params.partial_angle, false);
+ MoveSuperstructure(params.partial_angle, params.shooter_angle, false);
WaitForSuperstructure();
if (ShouldCancel()) return true;
- MoveSuperstructure(params.partial_angle, true);
+ MoveSuperstructure(params.partial_angle, params.shooter_angle, true);
if (!WaitOrCancel(::aos::time::Time::InSeconds(params.delay_time))) return true;
- MoveSuperstructure(params.full_angle, true);
+ MoveSuperstructure(params.full_angle, params.shooter_angle, true);
WaitForSuperstructure();
if (ShouldCancel()) return true;
return true;
}
-void SuperstructureActor::MoveSuperstructure(double shoulder,
+void SuperstructureActor::MoveSuperstructure(double shoulder, double shooter,
bool unfold_climber) {
- superstructure_goal_ = {0, shoulder, 0};
+ superstructure_goal_ = {0, shoulder, shooter};
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->angle_wrist = shooter;
new_superstructure_goal->max_angular_velocity_intake = 7.0;
new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
@@ -70,14 +70,9 @@
::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;
}
diff --git a/y2016/actors/superstructure_actor.h b/y2016/actors/superstructure_actor.h
index a8a0cc4..4d5e372 100644
--- a/y2016/actors/superstructure_actor.h
+++ b/y2016/actors/superstructure_actor.h
@@ -24,7 +24,7 @@
};
SuperstructureGoal superstructure_goal_;
bool RunAction(const actors::SuperstructureActionParams ¶ms) override;
- void MoveSuperstructure(double shoulder, bool unfold_climber);
+ void MoveSuperstructure(double shoulder, double shooter, bool unfold_climber);
void WaitForSuperstructure();
bool SuperstructureProfileDone();
bool SuperstructureDone();
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index d90ec68..49ea43f 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -48,7 +48,7 @@
// incorporating a small safety margin makes writing test cases much easier
// since you can directly compare statuses against the constants in the
// CollisionAvoidance class.
- constexpr double kSafetyMargin = 0.02; // radians
+ constexpr double kSafetyMargin = 0.03; // radians
// Avoid colliding the shooter with the frame.
// If the shoulder is below a certain angle or we want to move it below
@@ -624,7 +624,8 @@
kill_shoulder_ = true;
}
}
- arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage, max_voltage);
+ arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? kShooterHangingVoltage : max_voltage);
intake_.set_max_voltage(max_voltage);
if (IsRunning() && !kill_shoulder_) {
@@ -671,7 +672,7 @@
output->unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
- ::y2016::sensors::ball_detector.FetchAnother();
+ ::y2016::sensors::ball_detector.FetchLatest();
bool ball_detected = false;
if (::y2016::sensors::ball_detector.get()) {
ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 4e76a2b..0fb851a 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -111,6 +111,7 @@
&control_loops::superstructure_queue);
static constexpr double kZeroingVoltage = 6.0;
+ static constexpr double kShooterHangingVoltage = 6.0;
static constexpr double kOperatingVoltage = 12.0;
static constexpr double kLandingShoulderDownVoltage = -1.5;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index f1ae333..e1e0c9b 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -148,7 +148,7 @@
}
// Don't do any normal drivetrain stuff if vision is in charge.
- if (was_running_) {
+ if (vision_action_running_) {
return;
}
@@ -253,7 +253,7 @@
// 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;
+ wrist_goal_ = 1.0;
intake_goal_ = 0.0;
if (data.PosEdge(kExpand)) {
is_expanding_ = true;
@@ -261,6 +261,7 @@
params.partial_angle = 0.3;
params.delay_time = 0.7;
params.full_angle = shoulder_goal_;
+ params.shooter_angle = wrist_goal_;
action_queue_.EnqueueAction(actors::MakeSuperstructureAction(params));
}
if (data.IsPressed(kWinch)) {