Shooter now unloads propperly. Need to figure out why it is overshooting when the python isn't.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index be98d31..7d85bd7 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -38,8 +38,8 @@
0.47};
const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
0.55};
-const double shooter_zeroing_off_speed=0.0;
-const double shooter_zeroing_speed=1.0;
+const double shooter_zeroing_off_speed = 0.0;
+const double shooter_zeroing_speed = 0.1;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index faae2b0..fb9df74 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -29,16 +29,20 @@
// Make sure that reality and the observer can't get too far off. There is a
// delay by one cycle between the applied voltage and X_hat(2, 0), so compare
// against last cycle's voltage.
- if (X_hat(2, 0) > last_voltage_ + 2.0) {
- voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
- } else if (X_hat(2, 0) < last_voltage_ - 2.0) {
- voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+ if (X_hat(2, 0) > last_voltage_ + 4.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
+ LOG(INFO, "Capping due to runawway\n");
+ } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
+ LOG(INFO, "Capping due to runawway\n");
}
voltage_ = std::min(limit, voltage_);
voltage_ = std::max(-limit, voltage_);
U(0, 0) = voltage_ - old_voltage;
+ LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+
last_voltage_ = voltage_;
}
@@ -107,9 +111,6 @@
const frc971::constants::Values &values = constants::GetValues();
- double relative_position = shooter_.position();
- double absolute_position = shooter_.absolute_position();
-
// Don't even let the control loops run.
bool shooter_loop_disable = false;
@@ -122,7 +123,6 @@
switch (state_) {
case STATE_INITIALIZE:
- LOG(DEBUG, "Initializing\n");
if (position) {
// Reinitialize the internal filter state.
shooter_.InitializeState(position->position);
@@ -157,8 +157,6 @@
break;
case STATE_REQUEST_LOAD:
if (position) {
- // Need to go forward a little if we are starting with the
- // back_distal_sensor triggered
if (position->plunger && position->latch) {
// The plunger is back and we are latched, get ready to shoot.
state_ = STATE_PREPARE_SHOT;
@@ -288,10 +286,11 @@
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
- absolute_position, PowerToPosition(goal->shot_power));
- // TODO(austin): Goal velocity too...
+ shooter_.absolute_position(), PowerToPosition(goal->shot_power));
if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.001) {
+ PowerToPosition(goal->shot_power)) +
+ ::std::abs(shooter_.absolute_velocity()) <
+ 0.001) {
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
@@ -307,9 +306,19 @@
break;
case STATE_READY:
LOG(DEBUG, "In ready\n");
- // Wait until the brake is set, and a shot is requested or the shot power is changed.
- if (Time::Now() > shooter_brake_set_time_) {
- // We have waited long enough for the brake to set, turn the shooter control loop off.
+ // Wait until the brake is set, and a shot is requested or the shot power
+ // is changed.
+ if (::std::abs(shooter_.absolute_position() -
+ PowerToPosition(goal->shot_power)) > 0.002) {
+ // TODO(austin): Add a state to release the brake.
+
+ // TODO(austin): Do we want to set the brake here or after shooting?
+ // Depends on air usage.
+ LOG(DEBUG, "Preparing shot again.\n");
+ state_ = STATE_PREPARE_SHOT;
+ } else if (Time::Now() > shooter_brake_set_time_) {
+ // We have waited long enough for the brake to set, turn the shooter
+ // control loop off.
shooter_loop_disable = true;
LOG(DEBUG, "Brake is now set\n");
if (goal->shot_requested && !disabled) {
@@ -320,14 +329,6 @@
apply_some_voltage = true;
state_ = STATE_PREPARE_FIRE;
}
- } else if (::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.001) {
- // TODO(austin): If the goal has changed, go back to prepare shot, not if we are off.
- // TODO(austin): Add a state to release the brake.
-
- // TODO(austin): Do we want to set the brake here or after shooting?
- LOG(DEBUG, "Preparing shot again.\n");
- state_ = STATE_PREPARE_SHOT;
} else {
LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
}
@@ -491,13 +492,13 @@
}
status->done =
- ::std::abs(absolute_position - PowerToPosition(goal->shot_power)) < 0.004;
+ ::std::abs(shooter_.absolute_position() - PowerToPosition(goal->shot_power)) < 0.004;
if (position) {
last_position_ = *position;
LOG(DEBUG,
- "pos > real: %.2f adjusted: %.2f raw: %.2f state= %d\n",
- relative_position, absolute_position, position->position,
+ "pos > absolute: %f velocity: %f state= %d\n",
+ shooter_.absolute_position(), shooter_.absolute_velocity(),
state_);
}
if (position) {
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 7e99bfc..efb4480 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -59,6 +59,7 @@
double offset() const { return offset_; }
double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1, 0); }
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
@@ -74,11 +75,11 @@
}
void SetGoalPosition(double desired_position, double desired_velocity) {
- LOG(DEBUG, "Goal position: %.2f Goal velocity: %.2f\n", desired_position, desired_velocity);
+ LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position, desired_velocity);
R << desired_position - kPositionOffset, desired_velocity,
- -A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity;
+ (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+ A(1, 1) / A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 0b1a50c..98ea5a5 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -50,7 +50,7 @@
// The difference between the position with 0 at the back, and the position
// with 0 measured where the spring has 0 force.
- constexpr static double kPositionOffset = 0.3;
+ constexpr static double kPositionOffset = 0.305054 + 0.0254;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -162,7 +162,10 @@
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << ": The test doesn't support that.";
latch_delay_count_ = -6;
- EXPECT_GE(last_voltage_, 1) << ": Must preload the gearbox when firing.";
+ if (GetAbsolutePosition() > 0.01) {
+ EXPECT_GE(last_voltage_, 1)
+ << ": Must preload the gearbox when firing.";
+ }
}
if (shooter_queue_group_.output->brake_piston && !brake_piston_state_ &&
@@ -212,8 +215,11 @@
LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
if (latch_delay_count_ == -1) {
latch_piston_state_ = false;
- EXPECT_TRUE(brake_piston_state_)
- << ": Must have the brake set when releasing the latch.";
+ if (GetAbsolutePosition() > 0.002) {
+ EXPECT_TRUE(brake_piston_state_) << ": Must have the brake set when "
+ "releasing the latch for "
+ "powerful shots..";
+ }
plunger_latched_ = false;
// TODO(austin): The brake should be set for a number of cycles after
// this as well.
@@ -343,7 +349,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
- for (int i = 0; i < 100; ++i) {
+ for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
@@ -359,7 +365,6 @@
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
- LOG(DEBUG, "MOTORSTATE = %d\n", shooter_motor_.state());
if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
hit_preparefire = true;
}
@@ -380,6 +385,98 @@
EXPECT_TRUE(hit_fire);
}
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, FireLong) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+
+ bool hit_preparefire = false;
+ bool hit_fire = false;
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
+ hit_preparefire = true;
+ }
+ if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
+ if (!hit_fire) {
+ shooter_queue_group_.goal.MakeWithBuilder()
+ .shot_requested(false)
+ .Send();
+ }
+ hit_fire = true;
+ }
+ }
+
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ EXPECT_TRUE(hit_preparefire);
+ EXPECT_TRUE(hit_fire);
+}
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, MoveGoal) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.11).Send();
+
+ for (int i = 0; i < 50; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+}
+
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(ShooterTest, Unload) {
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
+ for (int i = 0; i < 150; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ shooter_queue_group_.goal.MakeWithBuilder().unload_requested(true).Send();
+
+ for (int i = 0; i < 400; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
+ shooter_motor_plant_.GetAbsolutePosition(), 0.01);
+ EXPECT_EQ(ShooterMotor::STATE_READY_UNLOAD, shooter_motor_.state());
+}
+
+// TODO(austin): Windup.
+// TODO(austin): Slip the encoder somewhere.
+
// TODO(austin): Test all the timeouts...
} // namespace testing