rezero the shooter while unloading + test
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index b6d71c6..8e283ff 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -155,6 +155,54 @@
return power;
}
+void ShooterMotor::CheckCalibrations(
+ const control_loops::ShooterGroup::Position *position) {
+ CHECK_NOTNULL(position);
+ const frc971::constants::Values &values = constants::GetValues();
+
+ // TODO(austin): Validate that this is the right edge.
+ // If we see a posedge on any of the hall effects,
+ if (position->pusher_proximal.posedge_count != last_proximal_posedge_count_ &&
+ !last_proximal_current_) {
+ proximal_posedge_validation_cycles_left_ = 2;
+ }
+ if (proximal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_proximal.current) {
+ --proximal_posedge_validation_cycles_left_;
+ if (proximal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_proximal.posedge_value,
+ values.shooter.pusher_proximal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ proximal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+
+ if (position->pusher_distal.posedge_count != last_distal_posedge_count_ &&
+ !last_distal_current_) {
+ distal_posedge_validation_cycles_left_ = 2;
+ }
+ if (distal_posedge_validation_cycles_left_ > 0) {
+ if (position->pusher_distal.current) {
+ --distal_posedge_validation_cycles_left_;
+ if (distal_posedge_validation_cycles_left_ == 0) {
+ shooter_.SetCalibration(
+ position->pusher_distal.posedge_value,
+ values.shooter.pusher_distal.upper_angle);
+
+ LOG(DEBUG, "Setting calibration using distal sensor\n");
+ zeroed_ = true;
+ }
+ } else {
+ distal_posedge_validation_cycles_left_ = 0;
+ }
+ }
+}
+
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
@@ -330,49 +378,7 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
- // TODO(austin): Validate that this is the right edge.
- // If we see a posedge on any of the hall effects,
- if (position->pusher_proximal.posedge_count !=
- last_proximal_posedge_count_ &&
- !last_proximal_current_) {
- proximal_posedge_validation_cycles_left_ = 2;
- }
- if (proximal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_proximal.current) {
- --proximal_posedge_validation_cycles_left_;
- if (proximal_posedge_validation_cycles_left_ == 0) {
- shooter_.SetCalibration(
- position->pusher_proximal.posedge_value,
- values.shooter.pusher_proximal.upper_angle);
-
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
- zeroed_ = true;
- }
- } else {
- proximal_posedge_validation_cycles_left_ = 0;
- }
- }
-
- if (position->pusher_distal.posedge_count !=
- last_distal_posedge_count_ &&
- !last_distal_current_) {
- distal_posedge_validation_cycles_left_ = 2;
- }
- if (distal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_distal.current) {
- --distal_posedge_validation_cycles_left_;
- if (distal_posedge_validation_cycles_left_ == 0) {
- shooter_.SetCalibration(
- position->pusher_distal.posedge_value,
- values.shooter.pusher_distal.upper_angle);
-
- LOG(DEBUG, "Setting calibration using distal sensor\n");
- zeroed_ = true;
- }
- } else {
- distal_posedge_validation_cycles_left_ = 0;
- }
- }
+ CheckCalibrations(position);
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
@@ -552,6 +558,10 @@
latch_piston_ = false;
} else {
latch_piston_ = true;
+
+ if (position) {
+ CheckCalibrations(position);
+ }
}
} else {
// The plunger isn't all the way back, or it is and it is unlatched, so
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 820b15f..2cb688f 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -16,6 +16,7 @@
namespace testing {
class ShooterTest_UnloadWindupPositive_Test;
class ShooterTest_UnloadWindupNegative_Test;
+class ShooterTest_RezeroWhileUnloading_Test;
};
using ::aos::time::Time;
@@ -93,6 +94,9 @@
void CapGoal();
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ShooterTest_RezeroWhileUnloading_Test;
+
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
@@ -125,6 +129,7 @@
double PowerToPosition(double power);
double PositionToPower(double position);
+ void CheckCalibrations(const control_loops::ShooterGroup::Position *position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -156,6 +161,7 @@
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
friend class testing::ShooterTest_UnloadWindupNegative_Test;
+ friend class testing::ShooterTest_RezeroWhileUnloading_Test;
// Enter state STATE_UNLOAD
void Unload() {
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 37b186b..90f7692 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -63,12 +63,12 @@
SetPhysicalSensors(&last_position_message_);
}
- // Returns the absolute angle of the wrist.
+ // Returns the absolute angle of the shooter.
double GetAbsolutePosition() const {
return shooter_plant_->Y(0, 0) + kPositionOffset;
}
- // Returns the adjusted angle of the wrist.
+ // Returns the adjusted angle of the shooter.
double GetPosition() const {
return GetAbsolutePosition() - initial_position_;
}
@@ -351,7 +351,7 @@
EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
@@ -368,7 +368,7 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 120; ++i) {
@@ -408,7 +408,7 @@
EXPECT_TRUE(hit_fire);
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
@@ -458,7 +458,7 @@
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
@@ -485,7 +485,6 @@
}
-// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Unload) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
@@ -511,7 +510,42 @@
EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that it rezeros while unloading.
+TEST_F(ShooterTest, RezeroWhileUnloading) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+
+ shooter_motor_.shooter_.offset_ += 0.01;
+ for (int i = 0; i < 50; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ for (int i = 0;
+ i < 800 && shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD;
+ ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.015);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+}
+
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
@@ -551,7 +585,7 @@
EXPECT_GE(3, capped_goal_count);
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
@@ -595,7 +629,7 @@
return (pair.lower_angle + pair.upper_angle) / 2.0;
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
@@ -614,7 +648,7 @@
}
-// Tests that the wrist zeros correctly and goes to a position.
+// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnProximal) {
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));