made it move the wrist once the hopper is clear
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index b5c3a52..1261115 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -57,6 +57,7 @@
is_shooting_(false),
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
+ hopper_clear_(true),
no_prior_position_(true),
missing_position_count_(0) {
}
@@ -535,6 +536,7 @@
break;
case Goal::READY_LOWER:
case Goal::INTAKE:
+ hopper_clear_ = false;
{
if (position) {
// Posedge of the disc entering the beam break.
@@ -760,6 +762,7 @@
"Emptied the hopper out but there are still discs there\n");
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
}
@@ -792,6 +795,7 @@
hopper_disc_count_);
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
@@ -1036,6 +1040,7 @@
status->shot_disc_count = shot_disc_count_;
status->preloaded = (loader_state_ != LoaderState::READY);
status->is_shooting = is_shooting_;
+ status->hopper_clear = hopper_clear_;
if (output) {
output->intake_voltage = intake_voltage;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 667aa79..0be4801 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -334,6 +334,7 @@
// Bottom disc detect from the last valid packet for detecting edges.
bool last_bottom_disc_detect_;
bool last_top_disc_detect_;
+ bool hopper_clear_;
int32_t last_bottom_disc_posedge_count_;
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index b12e31c..b299b42 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -80,6 +80,9 @@
// True from when we're committed to shooting util after the disk is clear
// of the robot.
bool is_shooting;
+ // Goes false when we first get a disk and back true after we finish
+ // clearing.
+ bool hopper_clear;
};
queue Goal goal;
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 3943350..05c5e69 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -136,6 +136,10 @@
is_high_gear = true;
}
+ // Whether we should change wrist positions to indicate that the hopper is
+ // clear.
+ bool hopper_clear = false;
+
// Where the wrist should be to pick up a frisbee.
// TODO(brians): Make these globally accessible and clean up auto.
static const double kWristPickup = -0.580;
@@ -143,8 +147,19 @@
// Where the wrist gets stored when up.
// All the way up is 1.5.
static const double kWristUp = 1.43;
+ static const double kWristCleared = kWristUp - 0.2;
static double wrist_down_position = kWristPickup;
double wrist_up_position = kWristUp;
+ double wrist_pickup_position = data.IsPressed(kIntake) ?
+ kWristPickup : kWristNearGround;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->hopper_disc_count >= 4) {
+ wrist_down_position = kWristNearGround;
+ } else {
+ wrist_down_position = wrist_pickup_position;
+ }
+ hopper_clear = index_loop.status->hopper_clear;
+ }
::aos::ScopedMessagePtr<control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
@@ -152,6 +167,7 @@
static double angle_adjust_goal = 0.42;
if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
shooter_goal->velocity = 131;
+ if (hopper_clear) wrist_up_position = kWristCleared;
angle_adjust_goal = 0.70;
} else if (data.IsPressed(kLongShot)) {
#if 0
@@ -167,7 +183,7 @@
}
#endif
shooter_goal->velocity = 360;
- wrist_up_position = 1.23 - 0.4;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.596;
} else if (data.IsPressed(kMediumShot)) {
#if 0
@@ -177,24 +193,15 @@
#endif
// middle wheel on the back line (same as auto)
shooter_goal->velocity = 395;
- wrist_up_position = 1.23 - 0.4;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.520;
} else if (data.IsPressed(kShortShot)) {
shooter_goal->velocity = 375;
+ if (hopper_clear) wrist_up_position = kWristCleared;
angle_adjust_goal = 0.671;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
- double wrist_pickup_position = data.IsPressed(kIntake) ?
- kWristPickup : kWristNearGround;
- index_loop.status.FetchLatest();
- if (index_loop.status.get()) {
- if (index_loop.status->hopper_disc_count >= 4) {
- wrist_down_position = kWristNearGround;
- } else {
- wrist_down_position = wrist_pickup_position;
- }
- }
wrist.goal.MakeWithBuilder()
.goal(data.IsPressed(kWristDown) ?
wrist_down_position :