Fire is not quite working, I believe the problem is in the simulation. the plunger effect is not set in STATE_LOADING_PROBLEM.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index f951e06..831ef19 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -183,7 +183,7 @@
if (position->plunger_back_hall_effect && position->latch_hall_effect) {
state_ = STATE_PREPARE_SHOT;
} else if (position->plunger_back_hall_effect &&
- abs(adjusted_position - PowerToPosition(goal->shot_power)) <
+ fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
0.05) {
state_ = STATE_LOADING_PROBLEM;
loading_problem_end_time_ =
@@ -207,12 +207,14 @@
break;
case STATE_PREPARE_SHOT:
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
- if (abs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.05) {
+ LOG(DEBUG, "PDIFF: adjusted_position: %.2f, pow: %.2f\n",
+ adjusted_position, PowerToPosition(goal->shot_power));
+ if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.05) {
state_ = STATE_READY;
output->latch_piston = true;
output->brake_piston = true;
shooter_brake_set_time_ =
- Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
+ Time::Now(Time::kDefaultClock) + Time::InSeconds(0.03);
} else {
output->latch_piston = true;
output->brake_piston = false;
@@ -222,15 +224,19 @@
if (Time::Now() > shooter_brake_set_time_) {
shooter_loop_disable = true;
if (goal->unload_requested) {
+ printf("GHA\n");
state_ = STATE_UNLOAD;
- } else if (abs(adjusted_position - PowerToPosition(goal->shot_power)) >
+ } else if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) >
0.05) {
+ printf("GHB\n");
state_ = STATE_PREPARE_SHOT;
} else if (goal->shot_requested) {
+ printf("GHC\n");
state_ = STATE_REQUEST_FIRE;
}
-
}
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+
output->latch_piston = true;
output->brake_piston = true;
break;
@@ -262,7 +268,7 @@
case STATE_FIRE:
shooter_loop_disable = true;
//TODO_ben: need approamately equal
- if (abs(last_position_.position - adjusted_position) < 0.07) {
+ if (fabs(last_position_.position - adjusted_position) < 0.07) {
cycles_not_moved_++;
} else {
cycles_not_moved_ = 0;
@@ -323,7 +329,7 @@
}
status->done =
- ::std::abs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
+ ::std::fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
}
} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 953bb27..9d134af 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -166,6 +166,10 @@
STATE_READY_UNLOAD = 12
} State;
+ State GetState() {return state_;}
+
+ double GetPosition() { return shooter_.position() - calibration_position_; }
+
protected:
virtual void RunIteration(
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 5a4fd86..4371f17 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -172,6 +172,8 @@
LOG(INFO, "seteffect: pusher proximal: %d\n",
position->plunger_back_hall_effect);
+
+
last_position_message_ = *position;
position.Send();
}
@@ -182,7 +184,6 @@
EXPECT_TRUE(shooter_queue_group_.output.FetchLatest());
shooter_plant_->U << last_voltage_;
shooter_plant_->Update();
-
if (shooter_queue_group_.output->latch_piston && !latch_piston_state_ &&
latch_delay_count_ == 0) {
latch_delay_count_ = 6;
@@ -205,6 +206,7 @@
EXPECT_LE(constants::GetValues().shooter.lower_limit - 1.0,
shooter_plant_->Y(0, 0));
last_voltage_ = shooter_queue_group_.output->voltage;
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// pointer to plant
@@ -256,6 +258,7 @@
// test.
::aos::robot_state.Clear();
SendDSPacket(true);
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
}
void SendDSPacket(bool enabled) {
@@ -271,7 +274,10 @@
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 1e-4);
}
- virtual ~ShooterTest() { ::aos::robot_state.Clear(); }
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ ::aos::time::Time::DisableMockTime();
+ }
};
TEST_F(ShooterTest, PowerConversion) {
@@ -289,18 +295,21 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
- VerifyNearGoaliand state();
+ //EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
}
// Tests that the wrist zeros correctly and goes to a position.
-TEST_F(ShooterTest, ) {
+TEST_F(ShooterTest, Fire) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
@@ -308,7 +317,35 @@
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
- VerifyNearGoal();
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+ shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
+
+ bool hit_requestfire = false;
+ bool hit_preparefire = false;
+ bool hit_fire = false;
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ printf("MOTORSTATE = %d\n", shooter_motor_.GetState());
+ if (shooter_motor_.GetState() == ShooterMotor::STATE_REQUEST_FIRE){
+ hit_requestfire = true;
+ }
+ if (shooter_motor_.GetState() == ShooterMotor::STATE_PREPARE_FIRE){
+ hit_preparefire = true;
+ }
+ if (shooter_motor_.GetState() == ShooterMotor::STATE_FIRE){
+ 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_.GetState());
+ EXPECT_TRUE(hit_requestfire);
+ EXPECT_TRUE(hit_preparefire);
+ EXPECT_TRUE(hit_fire);
}
} // namespace testing