converted the shooter over to using hall effect structs
diff --git a/frc971/constants.h b/frc971/constants.h
index a5b7fb1..3891663 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -26,9 +26,9 @@
// This structure contains current values for all of the things that change.
struct Values {
// This is useful for representing the 2 sides of a hall effect sensor etc.
- struct Pair {
- double lower_limit;
- double upper_limit;
+ struct AnglePair {
+ double lower_angle;
+ double upper_angle;
};
// The ratio from the encoder shaft to the drivetrain wheels.
@@ -49,9 +49,9 @@
struct ShooterLimits {
double lower_limit;
double upper_limit;
- Pair plunger_back;
- Pair pusher_distal;
- Pair pusher_proximal;
+ AnglePair plunger_back;
+ AnglePair pusher_distal;
+ AnglePair pusher_proximal;
};
ShooterLimits shooter;
@@ -73,10 +73,7 @@
double claw_max_seperation;
// Three hall effects are known as front, calib and back
- struct AnglePair {
- double lower_angle;
- double upper_angle;
- };
+ typedef Values::AnglePair AnglePair;
struct Claw {
double lower_hard_limit;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 831ef19..02766ba 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -97,43 +97,43 @@
calibration_position_, state_);
}
- // don't even let the control loops run
+ // Don't even let the control loops run.
bool shooter_loop_disable = false;
- // adds voltage to take up slack in gears before shot
+ // Adds voltage to take up slack in gears before shot.
bool apply_some_voltage = false;
switch (state_) {
case STATE_INITIALIZE:
- // start off with the assumption that we are at the value
- // futhest back given our sensors
- if (position && position->pusher_distal_hall_effect) {
+ // 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
calibration_position_ =
- position->position - values.shooter.pusher_distal.lower_limit;
- } else if (position && position->pusher_proximal_hall_effect) {
+ position->position - values.shooter.pusher_distal.lower_angle;
+ } else if (position && position->pusher_proximal.current) {
//TODO_ben: use posedge
calibration_position_ =
- position->position - values.shooter.pusher_proximal.lower_limit;
+ position->position - values.shooter.pusher_proximal.lower_angle;
}
state_ = STATE_REQUEST_LOAD;
- // zero out initial goal
+ // Zero out initial goal.
shooter_.SetGoalPosition(real_position, 0.0);
if (position) {
- output->latch_piston = position->plunger_back_hall_effect;
+ output->latch_piston = position->plunger.current;
} else {
- // we don't know what is going on so just close the latch to be safe
+ // We don't know what is going on so just close the latch to be safe.
output->latch_piston = true;
}
output->brake_piston = false;
break;
case STATE_REQUEST_LOAD:
- if (position->plunger_back_hall_effect && position->latch_hall_effect) {
- // already latched
+ if (position->plunger.current && position->latch.current) {
+ // Already latched.
state_ = STATE_PREPARE_SHOT;
- } else if (position->pusher_distal_hall_effect ||
+ } 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
@@ -146,11 +146,11 @@
shooter_.SetGoalPosition(0.0, 0.0);
if (position && output)
- output->latch_piston = position->plunger_back_hall_effect;
+ output->latch_piston = position->plunger.current;
if (output) output->brake_piston = false;
break;
case STATE_LOAD_BACKTRACK:
- if (adjusted_position > values.shooter.pusher_distal.upper_limit + 0.01) {
+ if (adjusted_position > values.shooter.pusher_distal.upper_angle + 0.01) {
shooter_.SetGoalPosition(
real_position - values.shooter_zeroing_speed * dt,
values.shooter_zeroing_speed);
@@ -162,27 +162,27 @@
if (output) output->brake_piston = true;
break;
case STATE_LOAD:
- if (position && position->pusher_proximal_hall_effect &&
- !last_position_.pusher_proximal_hall_effect) {
+ if (position && position->pusher_proximal.current &&
+ !last_position_.pusher_proximal.current) {
//TODO_ben: use posedge
calibration_position_ =
- position->position - values.shooter.pusher_proximal.upper_limit;
+ position->position - values.shooter.pusher_proximal.upper_angle;
}
- if (position && position->pusher_distal_hall_effect &&
- !last_position_.pusher_distal_hall_effect) {
+ if (position && position->pusher_distal.current &&
+ !last_position_.pusher_distal.current) {
//TODO_ben: use posedge
calibration_position_ =
- position->position - values.shooter.pusher_distal.lower_limit;
+ position->position - values.shooter.pusher_distal.lower_angle;
}
shooter_.SetGoalPosition(calibration_position_, 0.0);
if (position && output) {
- output->latch_piston = position->plunger_back_hall_effect;
+ output->latch_piston = position->plunger.current;
}
- if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ if (position->plunger.current && position->latch.current) {
state_ = STATE_PREPARE_SHOT;
- } else if (position->plunger_back_hall_effect &&
+ } else if (position->plunger.current &&
fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
0.05) {
state_ = STATE_LOADING_PROBLEM;
@@ -194,13 +194,13 @@
case STATE_LOADING_PROBLEM:
if (Time::Now() > loading_problem_end_time_) {
state_ = STATE_UNLOAD;
- } else if (position->plunger_back_hall_effect &&
- position->latch_hall_effect) {
+ } else if (position->plunger.current &&
+ position->latch.current) {
state_ = STATE_PREPARE_SHOT;
}
shooter_.SetGoalPosition(calibration_position_, 0.0);
LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger_back_hall_effect, position->latch_hall_effect);
+ position->plunger.current, position->latch.current);
if (output) output->latch_piston = true;
if (output) output->brake_piston = false;
@@ -242,7 +242,7 @@
break;
case STATE_REQUEST_FIRE:
shooter_loop_disable = true;
- if (position->plunger_back_hall_effect) {
+ if (position->plunger.current) {
prepare_fire_end_time_ =
Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
apply_some_voltage = true;
@@ -281,7 +281,7 @@
output->brake_piston = true;
break;
case STATE_UNLOAD:
- if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ if (position->plunger.current && position->latch.current) {
shooter_.SetGoalPosition(0.02, 0.0);
if (adjusted_position < 0.04) {
output->latch_piston = false;
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index 9811918..efcc0e7 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -9,11 +9,11 @@
},
'dependencies': [
'<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
'includes': ['../../../aos/build/queues.gypi'],
},
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 4350bd2..4f76a57 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -1,6 +1,7 @@
package frc971.control_loops;
import "aos/common/control_loop/control_loops.q";
+import "frc971/control_loops/control_loops.q";
queue_group ShooterGroup {
implements aos.control_loops.ControlLoop;
@@ -20,42 +21,23 @@
bool unload_requested;
};
message Position {
- // back on the plunger
- bool plunger_back_hall_effect;
- // truely back on the pusher
- bool pusher_distal_hall_effect;
- // warning that we are back on the pusher
- bool pusher_proximal_hall_effect;
- // the latch is closed
- bool latch_hall_effect;
-
- // count of positive edges
- int64_t plunger_back_hall_effect_posedge_count;
- // count of negative edges
- int64_t plunger_back_hall_effect_negedge_count;
- // count of positive edges
- int64_t pusher_distal_hall_effect_posedge_count;
- // count of negative edges
- int64_t pusher_distal_hall_effect_negedge_count;
- // count of positive edges
- int64_t pusher_proximal_hall_effect_posedge_count;
- // count of negative edges
- int64_t pusher_proximal_hall_effect_negedge_count;
- // count of positive edges
- int64_t latch_hall_effect_posedge_count;
- // count of negative edges
- int64_t latch_hall_effect_negedge_count;
+ // Gets triggered when the plunger is latched.
+ HallEffectStruct plunger;
+ // Gets triggered when the pusher is all the way back.
+ HallEffectStruct pusher_distal;
+ // Triggers just before pusher_distal.
+ HallEffectStruct pusher_proximal;
+ // Triggers when the latch engages.
+ HallEffectStruct latch;
// In meters, out is positive.
double position;
- // last positive edge
- double posedge_value;
- // last negative edge
- double negedge_value;
+ // 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;
};
- // I don't think this is needed, but it is here
- // so I won't delete it yet.
message Status {
// Whether it's ready to shoot right now.
bool ready;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 4371f17..39280f4 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -53,8 +53,8 @@
}
// Makes sure pos is inside range (inclusive)
- bool CheckRange(double pos, struct constants::Values::Pair pair) {
- return (pos >= pair.lower_limit && pos <= pair.upper_limit);
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
}
// Sets the values of the physical sensors that can be directly observed
@@ -67,33 +67,33 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
- position->plunger_back_hall_effect =
+ position->plunger.current =
CheckRange(position->position, values.shooter.plunger_back);
- position->pusher_distal_hall_effect =
+ position->pusher_distal.current =
CheckRange(position->position, values.shooter.pusher_distal);
- position->pusher_proximal_hall_effect =
+ position->pusher_proximal.current =
CheckRange(position->position, values.shooter.pusher_proximal);
}
- void UpdateEffectEdge(bool &effect, bool last_effect, double upper_limit,
- double lower_limit, double position,
- double &posedge_value, double &negedge_value,
- int64_t &posedge_count, int64_t &negedge_count) {
- if (effect && !last_effect) {
- ++posedge_count;
- if (last_position_message_.position < lower_limit) {
- posedge_value = lower_limit - initial_position_;
+ 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) {
+ 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_;
} else {
- posedge_value = upper_limit - initial_position_;
+ position->pusher_posedge_value = limits.upper_angle - initial_position_;
}
}
-
- if (!effect && last_effect) {
- ++negedge_count;
- if (position < lower_limit) {
- negedge_value = lower_limit - 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 {
- negedge_value = upper_limit - initial_position_;
+ position->pusher_negedge_value = limits.upper_angle - initial_position_;
}
}
}
@@ -111,14 +111,14 @@
LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
if (latch_delay_count_ == 1) {
latch_piston_state_ = true;
- position->latch_hall_effect = true;
+ position->latch.current = 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_hall_effect = false;
+ position->latch.current = false;
}
latch_delay_count_++;
}
@@ -137,42 +137,31 @@
}
// Handle plunger hall effect
- UpdateEffectEdge(position->plunger_back_hall_effect,
- last_position_message_.plunger_back_hall_effect,
- values.shooter.plunger_back.upper_limit,
- values.shooter.plunger_back.lower_limit,
- position->position, position->posedge_value,
- position->negedge_value,
- position->plunger_back_hall_effect_posedge_count,
- position->plunger_back_hall_effect_negedge_count);
+ 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_back_hall_effect);
+ position->plunger.current);
// Handle pusher distal hall effect
- UpdateEffectEdge(position->pusher_distal_hall_effect,
- last_position_message_.pusher_distal_hall_effect,
- values.shooter.pusher_distal.upper_limit,
- values.shooter.pusher_distal.lower_limit,
- position->position, position->posedge_value,
- position->negedge_value,
- position->pusher_distal_hall_effect_posedge_count,
- position->pusher_distal_hall_effect_negedge_count);
+ 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_back_hall_effect);
+ position->plunger.current);
// Handle pusher proximal hall effect
- UpdateEffectEdge(position->pusher_proximal_hall_effect,
- last_position_message_.pusher_proximal_hall_effect,
- values.shooter.pusher_proximal.upper_limit,
- values.shooter.pusher_proximal.lower_limit,
- position->position, position->posedge_value,
- position->negedge_value,
- position->pusher_proximal_hall_effect_posedge_count,
- position->pusher_proximal_hall_effect_negedge_count);
+ 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_back_hall_effect);
-
-
+ position->plunger.current);
last_position_message_ = *position;
position.Send();