Fixed race conditions in the self catch thing.
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index a0dca81..41972a9 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -15,7 +15,22 @@
],
'includes': ['../../aos/build/queues.gypi'],
},
- {
+ {
+ 'target_name': 'selfcatch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['selfcatch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'action',
'type': 'static_library',
'dependencies': [
@@ -47,6 +62,24 @@
],
},
{
+ 'target_name': 'selfcatch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'selfcatch_action.cc',
+ ],
+ 'dependencies': [
+ 'selfcatch_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(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',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ ],
+ },
+ {
'target_name': 'shoot_action',
'type': 'executable',
'sources': [
@@ -59,5 +92,18 @@
'action',
],
},
+ {
+ 'target_name': 'selfcatch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'selfcatch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'selfcatch_action_queue',
+ 'selfcatch_action_lib',
+ 'action',
+ ],
+ },
],
}
diff --git a/frc971/actions/selfcatch_action.cc b/frc971/actions/selfcatch_action.cc
index fc9a06c..6506361 100644
--- a/frc971/actions/selfcatch_action.cc
+++ b/frc971/actions/selfcatch_action.cc
@@ -124,12 +124,12 @@
bool SelfCatchAction::DoneBallIn() {
- if (!queues::othersensors.FetchLatest()) {
- queues::othersensors.FetchNextBlocking();
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
}
- if (queues::othersensors->travis_hall_effect_distance > 0.005)
+ if (sensors::othersensors->travis_hall_effect_distance > 0.005) {
LOG(INFO, "Ball in at %.2f.\n",
- queues::othersensors->travis_hall_effect_distance);
+ sensors::othersensors->travis_hall_effect_distance);
return true;
}
return false;
@@ -149,12 +149,12 @@
}
bool SelfCatchAction::DoneFoundSonar() {
- if (!queues::othersensors.FetchLatest()) {
- queues::othersensors.FetchNextBlocking();
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
}
- if (queues::othersensors->sonar_distance > 0.3 &&
- queues::othersensors->sonar_distance < kSonarTriggerDist) {
- LOG(INFO, "Hit Sonar at %.2f.\n", queues::othersensors->sonar_distance);
+ if (sensors::othersensors->sonar_distance > 0.3 &&
+ sensors::othersensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", sensors::othersensors->sonar_distance);
return true;
}
return false;
@@ -167,10 +167,30 @@
if (!control_loops::claw_queue_group.status.FetchLatest()) {
control_loops::claw_queue_group.status.FetchNextBlocking();
}
+ if (!control_loops::shooter_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch shooter goal.\n");
+ }
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch claw goal.\n");
+ }
// Make sure that both the shooter and claw have reached the necessary
// states.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool shooter_power_correct =
+ ::std::abs(control_loops::shooter_queue_group.status->hard_stop_power -
+ control_loops::shooter_queue_group.goal->shot_power) <
+ 0.005;
+
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.005;
+
if (control_loops::shooter_queue_group.status->ready &&
- control_loops::claw_queue_group.status->done_with_ball) {
+ control_loops::claw_queue_group.status->done_with_ball &&
+ shooter_power_correct && claw_angle_correct) {
LOG(INFO, "Claw and Shooter ready for shooting.\n");
// TODO(james): Get realer numbers for shooter_action.
return true;
@@ -181,7 +201,7 @@
// 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 +
+ selfcatch_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");
@@ -199,6 +219,7 @@
if (!control_loops::claw_queue_group.status.FetchLatest()) {
control_loops::claw_queue_group.status.FetchNextBlocking();
}
+
if (control_loops::claw_queue_group.status->separation >
values.shooter_action.claw_shooting_separation) {
LOG(INFO, "Opened up enough to shoot.\n");
diff --git a/frc971/actions/selfcatch_action.h b/frc971/actions/selfcatch_action.h
index 0b34e8c..dbd9541 100644
--- a/frc971/actions/selfcatch_action.h
+++ b/frc971/actions/selfcatch_action.h
@@ -21,6 +21,7 @@
static constexpr double kShotPower = 100.0;
static constexpr double kCatchSeperation = 1.0;
static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.8;
static constexpr double kCatchCentering = 12.0;
static constexpr double kFinishAngle = 0.2;
@@ -40,7 +41,6 @@
// to track when shot is complete
int previous_shots_;
- aos::Time::Time action_step_start_;
};
} // namespace actions
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
index b0254f7..9ba21ad 100644
--- a/frc971/actions/selfcatch_action.q
+++ b/frc971/actions/selfcatch_action.q
@@ -8,7 +8,8 @@
message Goal {
// If true, run this action. If false, cancel the action if it is
// currently running.
- bool run; // Shot power in joules.
+ bool run;
+ double shot_angle;
};
queue Goal goal;
diff --git a/frc971/actions/selfcatch_action_main.cc b/frc971/actions/selfcatch_action_main.cc
index b56ed50..53600ac 100644
--- a/frc971/actions/selfcatch_action_main.cc
+++ b/frc971/actions/selfcatch_action_main.cc
@@ -5,7 +5,7 @@
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
#include "frc971/actions/selfcatch_action.q.h"
-#include "frc971/actions/self_catch_action.h"
+#include "frc971/actions/selfcatch_action.h"
using ::aos::time::Time;
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index f445b64..d5ca0d1 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -14,6 +14,7 @@
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
'../actions/actions.gyp:shoot_action',
+ '../actions/actions.gyp:selfcatch_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',