Reorganized sensors in the shooter.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index c6949ad..a3a27cc 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -38,13 +38,8 @@
0.47};
const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
0.55};
-const double shooter_lower_physical_limit=0.0;
-const double shooter_upper_physical_limit=0.0;
-const double shooter_voltage=0.0;
-const double shooter_hall_effect_start_position=0.0;
const double shooter_zeroing_off_speed=0.0;
const double shooter_zeroing_speed=1.0;
-const double position=0.0;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
@@ -62,14 +57,10 @@
control_loops::MakeClutchDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07}},
- shooter_voltage,
- // shooter_total_length
- 1.0,
- shooter_hall_effect_start_position,
+ {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07},
shooter_zeroing_off_speed,
- shooter_zeroing_speed,
- position,
+ shooter_zeroing_speed
+ },
{0.5,
0.1,
0.1,
@@ -95,14 +86,10 @@
control_loops::MakeDogDrivetrainLoop,
// ShooterLimits
// TODO(ben): make these real numbers
- {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07}},
- shooter_voltage,
- // shooter_total_length
- 1.0,
- shooter_hall_effect_start_position,
+ {0.0, 1.0, {-0.03, 0.02}, {-0.01, 0.04}, {0.02, 0.07},
shooter_zeroing_off_speed,
shooter_zeroing_speed,
- position,
+ },
{0.5,
0.2,
0.1,
diff --git a/frc971/constants.h b/frc971/constants.h
index e95ce88..ab4bd35 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -46,22 +46,17 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
- struct ShooterLimits {
+ struct Shooter {
double lower_limit;
double upper_limit;
AnglePair plunger_back;
AnglePair pusher_distal;
AnglePair pusher_proximal;
+ double zeroing_off_speed;
+ double zeroing_speed;
};
- ShooterLimits shooter;
-
- double shooter_voltage;
- double shooter_total_length;
- double shooter_hall_effect_start_position;
- double shooter_zeroing_off_speed;
- double shooter_zeroing_speed;
- double position;
+ Shooter shooter;
struct Claws {
double claw_zeroing_off_speed;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 631fd59..ffadb94 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -13,3 +13,11 @@
double posedge;
double negedge;
};
+
+// Records edges captured on a single hall effect sensor.
+struct PosedgeOnlyCountedHallEffectStruct {
+ bool current;
+ int32_t posedge_count;
+ int32_t negedge_count;
+ double posedge_value;
+};
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 02766ba..08138ff 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -74,6 +74,7 @@
}
if (initial_loop_) {
+ // TODO(austin): If 'reset()', we are lost, start over.
initial_loop_ = false;
shooter_.SetPositionDirectly(position->position);
} else {
@@ -108,11 +109,11 @@
// Start off with the assumption that we are at the value
// futhest back given our sensors.
if (position && position->pusher_distal.current) {
- //TODO_ben: use posedge
+ //TODO(ben): use posedge
calibration_position_ =
position->position - values.shooter.pusher_distal.lower_angle;
} else if (position && position->pusher_proximal.current) {
- //TODO_ben: use posedge
+ //TODO(ben): use posedge
calibration_position_ =
position->position - values.shooter.pusher_proximal.lower_angle;
}
@@ -122,7 +123,7 @@
// Zero out initial goal.
shooter_.SetGoalPosition(real_position, 0.0);
if (position) {
- output->latch_piston = position->plunger.current;
+ output->latch_piston = position->plunger;
} else {
// We don't know what is going on so just close the latch to be safe.
output->latch_piston = true;
@@ -130,13 +131,12 @@
output->brake_piston = false;
break;
case STATE_REQUEST_LOAD:
- if (position->plunger.current && position->latch.current) {
+ if (position->plunger && position->latch) {
// Already latched.
state_ = STATE_PREPARE_SHOT;
- } else if (position->pusher_distal.current ||
- (adjusted_position) < 0) {
+ } else if (position->pusher_distal.current || (adjusted_position) < 0) {
state_ = STATE_LOAD_BACKTRACK;
- //TODO_ben: double check that rezero is the right thing to do here
+ // TODO(ben): double check that rezero is the right thing to do here
if (position) {
calibration_position_ = position->position;
}
@@ -145,15 +145,18 @@
}
shooter_.SetGoalPosition(0.0, 0.0);
- if (position && output)
- output->latch_piston = position->plunger.current;
- if (output) output->brake_piston = false;
+ if (position && output) {
+ output->latch_piston = position->plunger;
+ }
+ if (output) {
+ output->brake_piston = false;
+ }
break;
case STATE_LOAD_BACKTRACK:
if (adjusted_position > values.shooter.pusher_distal.upper_angle + 0.01) {
shooter_.SetGoalPosition(
- real_position - values.shooter_zeroing_speed * dt,
- values.shooter_zeroing_speed);
+ real_position - values.shooter.zeroing_speed * dt,
+ values.shooter.zeroing_speed);
} else {
state_ = STATE_LOAD;
}
@@ -164,27 +167,27 @@
case STATE_LOAD:
if (position && position->pusher_proximal.current &&
!last_position_.pusher_proximal.current) {
- //TODO_ben: use posedge
+ //TODO(ben): use posedge
calibration_position_ =
position->position - values.shooter.pusher_proximal.upper_angle;
}
if (position && position->pusher_distal.current &&
!last_position_.pusher_distal.current) {
- //TODO_ben: use posedge
+ //TODO(ben): use posedge
calibration_position_ =
position->position - values.shooter.pusher_distal.lower_angle;
}
shooter_.SetGoalPosition(calibration_position_, 0.0);
if (position && output) {
- output->latch_piston = position->plunger.current;
+ output->latch_piston = position->plunger;
}
- if (position->plunger.current && position->latch.current) {
+ if (position->plunger && position->latch) {
state_ = STATE_PREPARE_SHOT;
- } else if (position->plunger.current &&
+ } else if (position->plunger &&
fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
- 0.05) {
+ 0.05) {
state_ = STATE_LOADING_PROBLEM;
loading_problem_end_time_ =
Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
@@ -194,13 +197,12 @@
case STATE_LOADING_PROBLEM:
if (Time::Now() > loading_problem_end_time_) {
state_ = STATE_UNLOAD;
- } else if (position->plunger.current &&
- position->latch.current) {
+ } else if (position->plunger && position->latch) {
state_ = STATE_PREPARE_SHOT;
}
shooter_.SetGoalPosition(calibration_position_, 0.0);
LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger.current, position->latch.current);
+ position->plunger, position->latch);
if (output) output->latch_piston = true;
if (output) output->brake_piston = false;
@@ -242,7 +244,7 @@
break;
case STATE_REQUEST_FIRE:
shooter_loop_disable = true;
- if (position->plunger.current) {
+ if (position->plunger) {
prepare_fire_end_time_ =
Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
apply_some_voltage = true;
@@ -267,7 +269,7 @@
break;
case STATE_FIRE:
shooter_loop_disable = true;
- //TODO_ben: need approamately equal
+ //TODO(ben): need approamately equal
if (fabs(last_position_.position - adjusted_position) < 0.07) {
cycles_not_moved_++;
} else {
@@ -281,7 +283,7 @@
output->brake_piston = true;
break;
case STATE_UNLOAD:
- if (position->plunger.current && position->latch.current) {
+ if (position->plunger && position->latch) {
shooter_.SetGoalPosition(0.02, 0.0);
if (adjusted_position < 0.04) {
output->latch_piston = false;
@@ -294,13 +296,13 @@
output->brake_piston = false;
break;
case STATE_UNLOAD_MOVE:
- if (adjusted_position > values.shooter_total_length - 0.03) {
+ if (adjusted_position > values.shooter.upper_limit - 0.03) {
shooter_.SetGoalPosition(real_position, 0.0);
state_ = STATE_READY_UNLOAD;
} else {
shooter_.SetGoalPosition(
- real_position + values.shooter_zeroing_speed * dt,
- values.shooter_zeroing_speed);
+ real_position + values.shooter.zeroing_speed * dt,
+ values.shooter.zeroing_speed);
}
output->latch_piston = false;
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 9d134af..2946b67 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -73,9 +73,8 @@
offset_ = known_position - encoder_val;
}
- bool SetCalibrationOnEdge(
- const constants::Values::ShooterLimits &shooter_values,
- JointZeroingState zeroing_state) {
+ bool SetCalibrationOnEdge(const constants::Values::Shooter &shooter_values,
+ JointZeroingState zeroing_state) {
double edge_encoder;
double known_position;
if (GetPositionOfEdge(shooter_values, &edge_encoder, &known_position)) {
@@ -110,7 +109,7 @@
// Returns true if an edge was detected in the last cycle and then sets the
// edge_position to the absolute position of the edge.
- bool GetPositionOfEdge(const constants::Values::ShooterLimits &shooter,
+ bool GetPositionOfEdge(const constants::Values::Shooter &shooter,
double *edge_encoder, double *known_position);
#undef COUNT_SETTER_GETTER
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 4f76a57..830ab84 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -10,33 +10,31 @@
double voltage;
// true: latch engaged, false: latch open
bool latch_piston;
- // true: brake engaged false: brake released
+ // true: brake engaged false: brake released
bool brake_piston;
};
message Goal {
- // encoder ticks of shot energy.
+ // Shot power in joules.
double shot_power;
// Shoots as soon as this is true.
bool shot_requested;
bool unload_requested;
};
+
+ // Back is when the springs are all the way stretched.
message Position {
- // Gets triggered when the plunger is latched.
- HallEffectStruct plunger;
+ // If the latch piston is fired and this hall effect has been triggered, the
+ // plunger is all the way back and latched.
+ bool plunger;
// Gets triggered when the pusher is all the way back.
- HallEffectStruct pusher_distal;
+ PosedgeOnlyCountedHallEffectStruct pusher_distal;
// Triggers just before pusher_distal.
- HallEffectStruct pusher_proximal;
+ PosedgeOnlyCountedHallEffectStruct pusher_proximal;
// Triggers when the latch engages.
- HallEffectStruct latch;
+ bool latch;
// In meters, out is positive.
double position;
-
- // position at the last positive edge of either of the pusher hall effects.
- double pusher_posedge_value;
- // position at the last negative edge of either of the pusher hall effects.
- double pusher_negedge_value;
};
message Status {
// Whether it's ready to shoot right now.
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 16d4b5f..cad47af 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -23,7 +23,7 @@
virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
};
-::testing::Environment* const team_number_env =
+::testing::Environment *const team_number_env =
::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
@@ -78,7 +78,7 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
- position->plunger.current =
+ position->plunger =
CheckRange(position->position, values.shooter.plunger_back);
position->pusher_distal.current =
CheckRange(position->position, values.shooter.pusher_distal);
@@ -86,26 +86,21 @@
CheckRange(position->position, values.shooter.pusher_proximal);
}
- void UpdateEffectEdge(HallEffectStruct *sensor,
- const HallEffectStruct &last_sensor,
- const constants::Values::AnglePair &limits,
- control_loops::ShooterGroup::Position *position,
- const control_loops::ShooterGroup::Position &last_position) {
+ void UpdateEffectEdge(
+ PosedgeOnlyCountedHallEffectStruct *sensor,
+ const PosedgeOnlyCountedHallEffectStruct &last_sensor,
+ const constants::Values::AnglePair &limits,
+ const control_loops::ShooterGroup::Position &last_position) {
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
if (last_position.position < limits.lower_angle) {
- position->pusher_posedge_value = limits.lower_angle - initial_position_;
+ sensor->posedge_value = limits.lower_angle - initial_position_;
} else {
- position->pusher_posedge_value = limits.upper_angle - initial_position_;
+ sensor->posedge_value = limits.upper_angle - initial_position_;
}
}
if (!sensor->current && last_sensor.current) {
++sensor->negedge_count;
- if (position->position < limits.lower_angle) {
- position->pusher_negedge_value = limits.lower_angle - initial_position_;
- } else {
- position->pusher_negedge_value = limits.upper_angle - initial_position_;
- }
}
}
@@ -122,14 +117,14 @@
LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
if (latch_delay_count_ == 1) {
latch_piston_state_ = true;
- position->latch.current = true;
+ position->latch = true;
}
latch_delay_count_--;
} else if (latch_piston_state_ && latch_delay_count_ < 0) {
LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
if (latch_delay_count_ == -1) {
latch_piston_state_ = false;
- position->latch.current = false;
+ position->latch = false;
}
latch_delay_count_++;
}
@@ -147,32 +142,17 @@
brake_delay_count_++;
}
- // Handle plunger hall effect
- UpdateEffectEdge(&position->plunger,
- last_position_message_.plunger,
- values.shooter.plunger_back,
- position.get(),
- last_position_message_);
- LOG(INFO, "seteffect: plunger back: %d\n",
- position->plunger.current);
-
// Handle pusher distal hall effect
- UpdateEffectEdge(&position->pusher_distal,
- last_position_message_.pusher_distal,
- values.shooter.pusher_distal,
- position.get(),
- last_position_message_);
- LOG(INFO, "seteffect: pusher distal: %d\n",
- position->plunger.current);
+ UpdateEffectEdge(
+ &position->pusher_distal, last_position_message_.pusher_distal,
+ values.shooter.pusher_distal, last_position_message_);
+ LOG(INFO, "seteffect: pusher distal: %d\n", position->plunger);
// Handle pusher proximal hall effect
- UpdateEffectEdge(&position->pusher_proximal,
- last_position_message_.pusher_proximal,
- values.shooter.pusher_proximal,
- position.get(),
- last_position_message_);
- LOG(INFO, "seteffect: pusher proximal: %d\n",
- position->plunger.current);
+ UpdateEffectEdge(
+ &position->pusher_proximal, last_position_message_.pusher_proximal,
+ values.shooter.pusher_proximal, last_position_message_);
+ LOG(INFO, "seteffect: pusher proximal: %d\n", position->plunger);
last_position_message_ = *position;
position.Send();
@@ -203,7 +183,7 @@
EXPECT_GE(constants::GetValues().shooter.upper_limit,
shooter_plant_->Y(0, 0));
// we okay within a millimeter
- EXPECT_LE(constants::GetValues().shooter.lower_limit - 1.0,
+ EXPECT_LE(constants::GetValues().shooter.lower_limit - 0.001,
shooter_plant_->Y(0, 0));
last_voltage_ = shooter_queue_group_.output->voltage;
::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
@@ -223,7 +203,6 @@
int brake_delay_count_;
private:
-
ShooterGroup shooter_queue_group_;
double initial_position_;
double last_voltage_;
@@ -267,8 +246,11 @@
}
void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled).autonomous(false)
- .team_id(971).Send();
+ ::aos::robot_state.MakeWithBuilder()
+ .enabled(enabled)
+ .autonomous(false)
+ .team_id(971)
+ .Send();
::aos::robot_state.FetchLatest();
}
@@ -308,7 +290,7 @@
shooter_motor_plant_.Simulate();
SendDSPacket(true);
}
- //EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ // 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());
@@ -334,16 +316,16 @@
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;
- }
+ 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();