Got shoot action stuff building.
Still need to clean some stuff up and get it working and test it and all that fancy stuff.
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 91b1b2f..fe2abe1 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -16,6 +16,21 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'shoot_action_queue',
+ 'type': 'static_library',
+ 'sources': ['shoot_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/autonomous',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'auto_lib',
'type': 'static_library',
'sources': [
@@ -23,6 +38,7 @@
],
'dependencies': [
'auto_queue',
+ 'shoot_action_lib',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
@@ -36,6 +52,22 @@
],
},
{
+ 'target_name': 'shoot_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shoot_action.cc',
+ ],
+ 'dependencies': [
+ 'shoot_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ ],
+ },
+ {
'target_name': 'auto',
'type': 'executable',
'sources': [
diff --git a/frc971/autonomous/shoot_action.cc b/frc971/autonomous/shoot_action.cc
index 118736b..3fe3b76 100644
--- a/frc971/autonomous/shoot_action.cc
+++ b/frc971/autonomous/shoot_action.cc
@@ -1,10 +1,9 @@
-#include "stdio.h"
-
#include "aos/common/control_loop/Timing.h"
#include "aos/common/logging/logging.h"
-#include "aos/common/network/team_number.h"
-#include "frc971/actions/shoot_action.q.h"
+#include "frc971/autonomous/shoot_action.q.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/constants.h"
namespace frc971 {
@@ -13,33 +12,47 @@
class ShootAction {
public:
void Run() {
- ::frc971::actions::shoot_action.FetchLatest();
- while (!::frc971::actions::shoot_action.get()) {
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::frc971::actions::shoot_action.goal.FetchLatest();
+ while (!::frc971::actions::shoot_action.goal.get()) {
+ ::frc971::actions::shoot_action.goal.FetchNextBlocking();
}
+ if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
while (true) {
- while (!::frc971::actions::shoot_action->run) {
+ while (!::frc971::actions::shoot_action.goal->run) {
LOG(INFO, "Waiting for an action request.\n");
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::frc971::actions::shoot_action.goal.FetchNextBlocking();
}
LOG(INFO, "Starting action\n");
+ if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(
+ true).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
RunAction();
+ if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(
+ false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
- while (::frc971::actions::shoot_action->run) {
+ while (::frc971::actions::shoot_action.goal->run) {
LOG(INFO, "Waiting for the action to be stopped.\n");
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::frc971::actions::shoot_action.goal.FetchNextBlocking();
}
}
}
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
void RunAction();
protected:
// Returns true if the action should be canceled.
bool ShouldCancel() {
- shoot_action.FetchLatest();
- bool ans = !::frc971::actions::shoot_action->run;
+ shoot_action.goal.FetchLatest();
+ bool ans = !::frc971::actions::shoot_action.goal->run;
if (ans) {
LOG(INFO, "Time to exit auto mode\n");
}
@@ -49,14 +62,14 @@
void ShootAction::RunAction() {
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action->shot_power).shot_requested(false)
+ 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;
}
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action->shot_angle).separation_angle(0.0).intake(2.0)
+ shoot_action.goal->shot_angle).separation_angle(0.0).intake(2.0)
.centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
@@ -66,9 +79,12 @@
control_loops::shooter_queue_group.status.FetchLatest();
control_loops::claw_queue_group.status.FetchLatest();
while (true) {
+ // 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) {
LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ // TODO(james): Get realer numbers for shooter_action.
break;
}
@@ -79,20 +95,23 @@
if (ShouldCancel()) return;
}
+ const frc971::constants::Values &values = frc971::constants::GetValues();
+
// Open up the claw in preparation for shooting.
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action->shot_angle).separation_angle(k2).intake(2.0)
- .centering(1.0).Send()) {
+ shoot_action.goal->shot_angle)
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0).centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
}
if (ShouldCancel()) return;
- // Make sure we have the latest statuses.
+ // Make sure we have the latest status.
control_loops::claw_queue_group.status.FetchLatest();
while (true) {
- if (control_loops::claw_queue_group.status->separation > k1) {
+ if (control_loops::claw_queue_group.status->separation > values.shooter_action.claw_shooting_separation) {
LOG(INFO, "Opened up enough to shoot.\n");
break;
}
@@ -102,6 +121,35 @@
if (ShouldCancel()) return;
}
+
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ shoot_action.goal->shot_power).shot_requested(true)
+ .unload_requested(false).load_requested(false).Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ if (ShouldCancel()) return;
+
+ // Make sure that we have the latest shooter status.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ // Get the number of shots fired up to this point. This should not be updated
+ // again for another few cycles.
+ int previous_shots = control_loops::shooter_queue_group.status->shots;
+ while (true) {
+ if (control_loops::shooter_queue_group.status->shots > previous_shots) {
+ LOG(INFO, "Shot succeeded!\n");
+ break;
+ }
+
+ // Wait until we have a new status.
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+
+ if (ShouldCancel()) return;
+ }
+
+ return;
}
} // namespace actions
diff --git a/frc971/autonomous/shoot_action.q b/frc971/autonomous/shoot_action.q
index 15073cb..507b12a 100644
--- a/frc971/autonomous/shoot_action.q
+++ b/frc971/autonomous/shoot_action.q
@@ -1,6 +1,6 @@
package frc971.actions;
-queue_group ShootAction {
+queue_group ShootActionQueueGroup {
message Status {
bool running;
};
@@ -18,4 +18,4 @@
queue Status status;
};
-queue_group ShootAction shoot_action;
+queue_group ShootActionQueueGroup shoot_action;
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 1e5e1d0..9a6828f 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -70,7 +70,8 @@
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
- }
+ },
+ {0.01, 0.1} // shooter_action
};
break;
case kCompTeamNumber:
@@ -102,7 +103,9 @@
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
4.0,
- }
+ },
+ //TODO(james): Get realer numbers for shooter_action.
+ {0.01, 0.1} // shooter_action
};
break;
case kPracticeTeamNumber:
@@ -136,7 +139,9 @@
0.020000 * 2.0, // claw_unimportant_epsilon
-0.200000 * 2.0, // start_fine_tune_pos
4.000000,
- }
+ },
+ //TODO(james): Get realer numbers for shooter_action.
+ {0.01, 0.1} // shooter_action
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 8222bbe..7d06a70 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -99,6 +99,19 @@
double max_zeroing_voltage;
};
Claws claw;
+
+ // Has all the constants for the ShootAction class.
+ struct ShooterAction {
+ // Minimum separation required between the claws in order to be able to
+ // shoot.
+ double claw_shooting_separation;
+
+ // Goal to send to the claw when opening it up in preparation for shooting;
+ // should be larger than claw_shooting_separation so that we can shoot
+ // promptly.
+ double claw_separation_goal;
+ };
+ ShooterAction shooter_action;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 815210f..85751d9 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -399,7 +399,7 @@
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status) {
+ control_loops::ClawGroup::Status *status) {
constexpr double dt = 0.01;
// Disable the motors now so that all early returns will return with the
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 8a4678e..37a1787 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -205,7 +205,7 @@
virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
const control_loops::ClawGroup::Position *position,
control_loops::ClawGroup::Output *output,
- ::aos::control_loops::Status *status);
+ control_loops::ClawGroup::Status *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index c67ed20..5d6633b 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -64,7 +64,7 @@
queue Goal goal;
queue Position position;
queue Output output;
- queue aos.control_loops.Status status;
+ queue Status status;
};
queue_group ClawGroup claw_queue_group;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 036b466..158e3cd 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -460,6 +460,7 @@
cycles_not_moved_ > 3) ||
Time::Now() > shot_end_time_) {
state_ = STATE_REQUEST_LOAD;
+ status->shots += 1;
}
latch_piston_ = false;
brake_piston_ = true;