Fixed a couple bugs with the shoot action.
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 92b5de6..54cdfef 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -37,6 +37,7 @@
LOG(ERROR, "Failed to send the status.\n");
}
RunAction();
+ LOG(INFO, "Done with action\n");
if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 473f1af..3d33076 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -18,27 +18,35 @@
double ShootAction::SpeedToAngleOffset(double speed) {
const frc971::constants::Values& values = frc971::constants::GetValues();
// scale speed to a [0.0-1.0] on something close to the max
- return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
+ return -(speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
}
void ShootAction::RunAction() {
const frc971::constants::Values& values = frc971::constants::GetValues();
+ LOG(INFO, "Shooting at angle %f, power %f\n", shoot_action.goal->shot_angle,
+ shoot_action.goal->shot_power);
// Set shot power
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action.goal->shot_power)
- .shot_requested(false).unload_requested(false).load_requested(false)
- .Send()) {
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(shoot_action.goal->shot_power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
LOG(ERROR, "Failed to send the shoot action\n");
return;
}
// Set claw angle
control_loops::drivetrain.status.FetchLatest();
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle +
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
- .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(
+ control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0)
+ .intake(2.0)
+ .centering(1.0)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
@@ -47,11 +55,14 @@
if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
// Open up the claw in preparation for shooting.
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle +
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
- .separation_angle(values.shooter_action.claw_separation_goal)
- .intake(2.0).centering(1.0).Send()) {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(
+ control_loops::drivetrain.status->robot_speed))
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0)
+ .centering(1.0)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
@@ -77,7 +88,6 @@
if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
// done with action
- return;
}
bool ShootAction::DoneSetupShot() {
@@ -90,7 +100,7 @@
// Make sure that both the shooter and claw have reached the necessary
// states.
if (control_loops::shooter_queue_group.status->ready &&
- control_loops::claw_queue_group.status->done) {
+ control_loops::claw_queue_group.status->done_with_ball) {
LOG(INFO, "Claw and Shooter ready for shooting.\n");
// TODO(james): Get realer numbers for shooter_action.
return true;
@@ -100,16 +110,21 @@
// TODO(ben): the claw may never reach the goal if the velocity is
// continually changing, we will need testing to see
control_loops::drivetrain.status.FetchLatest();
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle +
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
- .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ if (ShouldCancel()) return false;
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(
+ control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0)
+ .intake(2.0)
+ .centering(1.0)
+ .Send()) {
LOG(WARNING, "sending claw goal failed\n");
abort_ = true;
return true;
- } else {
- LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ //} else {
+ //LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ //SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
}
return false;
}
diff --git a/frc971/actions/shoot_action.q b/frc971/actions/shoot_action.q
index 507b12a..d080071 100644
--- a/frc971/actions/shoot_action.q
+++ b/frc971/actions/shoot_action.q
@@ -11,7 +11,7 @@
bool run; // Shot power in joules.
double shot_power;
// Claw angle when shooting.
- bool shot_angle;
+ double shot_angle;
};
queue Goal goal;
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 9e57e6f..e55a827 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -86,24 +86,35 @@
TypedAction(T *queue_group)
: queue_group_(queue_group),
goal_(queue_group_->goal.MakeMessage()),
- has_seen_response_(false) {}
+ has_started_(false) {}
// Returns the current goal that will be sent when the action is sent.
GoalType *GetGoal() { return goal_.get(); }
+ ~TypedAction() {
+ LOG(INFO, "Calling destructor\n");
+ DoCancel();
+ }
+
private:
// Cancels the action.
- virtual void DoCancel() { queue_group_->goal.MakeWithBuilder().run(false).Send(); }
+ virtual void DoCancel() {
+ LOG(INFO, "Canceling action\n");
+ queue_group_->goal.MakeWithBuilder().run(false).Send();
+ }
// Returns true if the action is running or we don't have an initial response
// back from it to signal whether or not it is running.
virtual bool DoRunning() {
- if (has_seen_response_) {
+ if (has_started_) {
queue_group_->status.FetchLatest();
} else if (queue_group_->status.FetchLatest()) {
- has_seen_response_ = true;
+ if (queue_group_->status->running) {
+ // Wait until it reports that it is running to start.
+ has_started_ = true;
+ }
}
- return !has_seen_response_ ||
+ return !has_started_ ||
(queue_group_->status.get() && queue_group_->status->running);
}
@@ -112,9 +123,10 @@
if (goal_) {
goal_->run = true;
goal_.Send();
- has_seen_response_ = false;
+ has_started_ = false;
+ LOG(INFO, "Starting action\n");
} else {
- has_seen_response_ = true;
+ has_started_ = true;
}
}
@@ -122,7 +134,7 @@
::aos::ScopedMessagePtr<GoalType> goal_;
// Track if we have seen a response to the start message.
// If we haven't, we are considered running regardless.
- bool has_seen_response_;
+ bool has_started_;
};
// Makes a new ShootAction action.
@@ -140,9 +152,11 @@
// Queues up an action for sending.
void QueueAction(::std::unique_ptr<Action> action) {
if (current_action_) {
+ LOG(INFO, "Queueing action, canceling prior\n");
current_action_->Cancel();
next_action_ = ::std::move(action);
} else {
+ LOG(INFO, "Queueing action\n");
current_action_ = ::std::move(action);
current_action_->Start();
}
@@ -151,6 +165,7 @@
// Cancels the current action, and runs the next one when the current one has
// finished.
void CancelCurrentAction() {
+ LOG(INFO, "Canceling current action\n");
if (current_action_) {
current_action_->Cancel();
}
@@ -158,6 +173,7 @@
// Cancels all running actions.
void CancelAllActions() {
+ LOG(INFO, "Canceling all actions\n");
if (current_action_) {
current_action_->Cancel();
}
@@ -168,8 +184,10 @@
void Tick() {
if (current_action_) {
if (!current_action_->Running()) {
- if (next_action_) {
- current_action_ = ::std::move(next_action_);
+ LOG(INFO, "Action is done.\n");
+ current_action_ = ::std::move(next_action_);
+ if (current_action_) {
+ LOG(INFO, "Running next action\n");
current_action_->Start();
}
}
@@ -280,6 +298,9 @@
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
HandleDrivetrain(data);
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ }
if (data.IsPressed(kIntakeOpenPosition)) {
action_queue_.CancelAllActions();
@@ -313,7 +334,7 @@
} else if (data.PosEdge(kShortShot)) {
auto shoot_action = MakeShootAction();
- shot_power_ = 70.0;
+ shot_power_ = 20.0;
shoot_action->GetGoal()->shot_power = shot_power_;
shoot_action->GetGoal()->shot_angle = kShortShotGoal.angle;
SetGoal(kShortShotGoal);
@@ -322,6 +343,9 @@
}
action_queue_.Tick();
+ if (data.IsPressed(kUnload) || data.IsPressed(kReload)) {
+ action_queue_.CancelAllActions();
+ }
// Send out the claw and shooter goals if no actions are running.
if (!action_queue_.Running()) {
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/scripts/start_list.txt
index 179c122..043f616 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/scripts/start_list.txt
@@ -6,3 +6,4 @@
sensor_receiver
claw
shooter
+shoot_action