shooter state machine (control loop) and test are written and build. not tested for functionality
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 7541f7c..f7363c6 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -55,9 +55,12 @@
kCompDrivetrainEncoderRatio,
kCompLowGearRatio,
kCompHighGearRatio,
+ // ShooterLimits
+ // TODO_ben: make these real numbers
+ {0.0, 100.0, {-3.0, 2.0}, {-1, 4.0}, {2.0, 7.0}},
shooter_voltage,
- shooter_lower_physical_limit,
- shooter_upper_physical_limit,
+ // shooter_total_length
+ 100.0,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
@@ -71,7 +74,6 @@
0.1,
0.0,
1.57,
-
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
{0.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
0.02, // claw_unimportant_epsilon
@@ -83,9 +85,12 @@
kPracticeDrivetrainEncoderRatio,
kPracticeLowGearRatio,
kPracticeHighGearRatio,
+ // ShooterLimits
+ // TODO_ben: make these real numbers
+ {0.0, 100.0, {-3.0, 2.0}, {-1, 4.0}, {2.0, 7.0}},
shooter_voltage,
- shooter_lower_physical_limit,
- shooter_upper_physical_limit,
+ // shooter_total_length
+ 100.0,
shooter_hall_effect_start_position,
shooter_zeroing_off_speed,
shooter_zeroing_speed,
diff --git a/frc971/constants.h b/frc971/constants.h
index a3b818e..0b85d8a 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -32,10 +32,25 @@
// gear.
double low_gear_ratio;
double high_gear_ratio;
+
+ // Three hall effects are known as front, calib and back
+ struct Pair {
+ double lower_limit;
+ double upper_limit;
+ };
+
+ struct ShooterLimits {
+ double lower_limit;
+ double upper_limit;
+ Pair plunger_back;
+ Pair pusher_distal;
+ Pair pusher_proximal;
+ };
+
+ ShooterLimits shooter;
double shooter_voltage;
- double shooter_lower_physical_limit;
- double shooter_upper_physical_limit;
+ double shooter_total_length;
double shooter_hall_effect_start_position;
double shooter_zeroing_off_speed;
double shooter_zeroing_speed;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 5c6548c..dfc440c 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -5,6 +5,7 @@
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
@@ -13,6 +14,7 @@
namespace frc971 {
namespace control_loops {
+using ::aos::time::Time;
void ZeroedStateFeedbackLoop::CapU() {
const double old_voltage = voltage_;
@@ -42,38 +44,28 @@
last_voltage_ = voltage_;
}
-ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
- : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
- shooter_(MakeShooterLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<1>::ConfigurationData config_data;
-
- config_data.lower_limit = GetValues().shooter_lower_physical_limit;
- config_data.upper_limit = GetValues().shooter_upper_physical_limit;
- //config_data.hall_effect_start_position[0] =
- // GetValues().shooter_hall_effect_start_position;
- config_data.zeroing_off_speed = GetValues().shooter_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().shooter_zeroing_speed;
- config_data.max_zeroing_voltage = 5.0;
- config_data.deadband_voltage = 0.0;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
+ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterGroup>(my_shooter),
+ shooter_(MakeShooterLoop()),
+ calibration_position_(0.0),
+ state_(STATE_INITIALIZE),
+ loading_problem_end_time_(0,0),
+ shooter_brake_set_time_(0,0),
+ prepare_fire_end_time_(0,0),
+ shot_end_time_(0,0),
+ cycles_not_moved_(0) { }
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
- const ShooterLoop::Goal *goal,
- const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status * status) {
+ const control_loops::ShooterGroup::Goal *goal,
+ const control_loops::ShooterGroup::Position *position,
+ control_loops::ShooterGroup::Output *output,
+ control_loops::ShooterGroup::Status * status) {
constexpr double dt = 0.01;
// we must always have these or we have issues.
if (goal == NULL || status == NULL) {
- transform-position_ptr = NULL;
if (output) output->voltage = 0;
LOG(ERROR, "Thought I would just check for null and die.\n");
return;
@@ -83,36 +75,22 @@
// motors disabled.
if (output) output->voltage = 0;
- ZeroedJoint<1>::PositionData transformed_position;
- ZeroedJoint<1>::PositionData *transformed_position_ptr =
- &transformed_position;
- if (position) {
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
- }
-
- const double voltage = shooter_.Update(transformed_position_ptr,
- output != NULL,
- goal->goal, 0.0);
-
const frc971::constants::Values &values = constants::GetValues();
- double absolute_position = postiion->position - calibration_position_;
+ double real_position = position->position - calibration_position_;
+ bool shooter_loop_disable = false;
switch (state_) {
case STATE_INITIALIZE:
- shooter_.zeroing_state() = ZeroedStateFeedbackLoop::UNKNOWN_POSITION;
-
// start off with the assumption that we are at the value
// futhest back given our sensors
if (position && position->pusher_distal_hall_effect){
calibration_position_ = position->position -
- values.pusher_distal_heffect.lower_edge;
+ values.shooter.pusher_distal.lower_limit;
} else if (position && position->pusher_proximal_hall_effect) {
calibration_position_ = position->position -
- values.pusher_proximal_heffect.lower_edge;
+ values.shooter.pusher_proximal.lower_limit;
} else {
calibration_position_ = values.shooter_total_length;
}
@@ -120,9 +98,9 @@
state_ = STATE_REQUEST_LOAD;
// zero out initial goal
- shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ shooter_.SetGoalPosition(0.0, 0.0);
if (position) {
- output->latch_pistion = position->plunger_back_hall_effect;
+ output->latch_piston = position->plunger_back_hall_effect;
} else {
// we don't know what is going on so just close the latch to be safe
output->latch_piston = true;
@@ -133,26 +111,26 @@
if (position->plunger_back_hall_effect && position->latch_hall_effect) {
// already latched
state_ = STATE_PREPARE_SHOT;
- } else if (postion->pusher_back_distal_hall_effect ||
- (relative_position) < 0) {
- state_ = STATE_LOADING_BACKTRACK;
- if (relative_position) {
+ } else if (position->pusher_distal_hall_effect ||
+ (real_position) < 0) {
+ state_ = STATE_LOAD_BACKTRACK;
+ if (position) {
calibration_position_ = position->position;
}
} else {
state_ = STATE_LOAD;
}
- shooter_.SetGoalPositionVelocity(0.0, 0.0);
+ shooter_.SetGoalPosition(0.0, 0.0);
if (position && output) output->latch_piston = position->plunger_back_hall_effect;
output->brake_piston = false;
break;
case STATE_LOAD_BACKTRACK:
- if (absolute_position < values.pusher_back_distal_heffect.lower_edge + 0.01) {
- shooter_.SetGoalPositionVelocity(position->position + values.shooter_zero_speed*dt,
- values.shooter_zero_speed);
+ if (real_position < values.shooter.pusher_distal.upper_limit + 0.01) {
+ shooter_.SetGoalPosition(position->position + values.shooter_zeroing_speed*dt,
+ values.shooter_zeroing_speed);
} else {
- state = STATE_LOAD;
+ state_ = STATE_LOAD;
}
output->latch_piston = false;
@@ -160,18 +138,17 @@
break;
case STATE_LOAD:
if (position->pusher_proximal_hall_effect &&
- !last_position_.pusher_back_proximal_hall_effect) {
+ !last_position_.pusher_proximal_hall_effect) {
calibration_position_ = position->position -
- values.pusher_promimal_heffect.lower_edge;
+ values.shooter.pusher_proximal.upper_limit;
}
if (position->pusher_distal_hall_effect &&
- !last_position_.pusher_back_distal_hall_effect) {
+ !last_position_.pusher_distal_hall_effect) {
calibration_position_ = position->position -
- values.pusher_distal_heffect.lower_edge;
-
+ values.shooter.pusher_distal.lower_limit;
}
- shooter_.SetGoalPositionVelocity(calibration_position_, 0.0);
+ shooter_.SetGoalPosition(calibration_position_, 0.0);
if (position && output) output->latch_piston = position->plunger_back_hall_effect;
if(output) output->brake_piston = false;
@@ -181,36 +158,37 @@
position->position == PowerToPosition(goal->shot_power)) {
//TODO_ben: I'm worried it will bounce states if the position is drifting slightly
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = clock() + 3 * CLOCKS_PER_SECOND;
+ loading_problem_end_time_ = Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
}
break;
case STATE_LOADING_PROBLEM:
if (position->plunger_back_hall_effect && position->latch_hall_effect) {
state_ = STATE_PREPARE_SHOT;
- } else if (absolute_position < -0.02 || clock() > loading_problem_end_time_) {
- state = STATE_UNLOAD;
+ } else if (real_position < -0.02 || Time::Now() > loading_problem_end_time_) {
+ state_ = STATE_UNLOAD;
}
- shooter_.SetGoalPositionVelocity(position->position - values.shooter_zero_speed*dt,
- values.shooter_zero_speed);
+ shooter_.SetGoalPosition(position->position - values.shooter_zeroing_speed*dt,
+ values.shooter_zeroing_speed);
if (output) output->latch_piston = true;
if (output) output->brake_piston = false;
break;
case STATE_PREPARE_SHOT:
shooter_.SetGoalPosition(
- PowerToPosition(shot_power), 0.0);
- if (position->position == shooter.goal_position) {
+ PowerToPosition(goal->shot_power), 0.0);
+ //TODO_ben: I'm worried it will bounce states if the position is drifting slightly
+ if (position->position == PowerToPosition(goal->shot_power)) {
state_ = STATE_READY;
output->latch_piston = true;
output->brake_piston = true;
- shooter_brake_set_time_ = clock() + 5 * CLOCKS_PER_SECOND;
+ shooter_brake_set_time_ = Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
} else {
output->latch_piston =true;
output->brake_piston = false;
}
break;
case STATE_READY:
- if (clock() > shooter_brake_set_time_) {
+ if (Time::Now() > shooter_brake_set_time_) {
shooter_loop_disable = true;
if (goal->unload_requested) {
state_ = STATE_UNLOAD;
@@ -229,7 +207,9 @@
case STATE_REQUEST_FIRE:
shooter_loop_disable = true;
if (position->plunger_back_hall_effect) {
- prepare_fire_end_time_ = clock() + 10;
+ prepare_fire_end_time_ = Time::Now(Time::kDefaultClock)
+ + Time::InMS(40.0);
+ shooter_.ApplySomeVoltage();
state_ = STATE_PREPARE_FIRE;
} else {
state_ = STATE_REQUEST_LOAD;
@@ -237,12 +217,13 @@
break;
case STATE_PREPARE_FIRE:
shooter_loop_disable = true;
- if (clock() < prepare_fire_end_time_) {
+ if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
shooter_.ApplySomeVoltage();
} else {
- State_ = STATE_FIRE;
+ state_ = STATE_FIRE;
cycles_not_moved_ = 0;
- shot_end_time_ = clock() + 0.5 * CLOCKS_PER_SECOND;
+ shot_end_time_ = Time::Now(Time::kDefaultClock) +
+ Time::InMS(500);
}
output->latch_piston = true;
@@ -252,18 +233,22 @@
case STATE_FIRE:
shooter_loop_disable = true;
//TODO_ben: need approamately equal
- if (last_position->position - position->position < 7) {
- cycles_not_moved++;
+ if (last_position_.position - position->position < 7) {
+ cycles_not_moved_++;
} else {
- cycles_not_moved = 0;
+ cycles_not_moved_ = 0;
}
+ if ((real_position < 10.0 && cycles_not_moved_ > 5) ||
+ Time::Now(Time::kDefaultClock) > shot_end_time_) {
+ state_ = STATE_REQUEST_LOAD;
+ }
output->latch_piston = true;
- ouput->brake_piston = true;
+ output->brake_piston = true;
break;
case STATE_UNLOAD:
- if (position->plunger_back_hall_effect && position->latch_piston) {
- shooter_SetGoalPosition(0.02, 0.0);
- if (ablsolute_position == 0.02) {
+ if (position->plunger_back_hall_effect && position->latch_hall_effect) {
+ shooter_.SetGoalPosition(0.02, 0.0);
+ if (real_position == 0.02) {
output->latch_piston = false;
}
} else {
@@ -274,12 +259,12 @@
output->brake_piston = false;
break;
case STATE_UNLOAD_MOVE:
- if (position->position > values.shooter_length - 0.03) {
- shooter_.SetPosition(position->position, 0.0);
- state_ = STATE_READY_UNLOADED;
+ if (position->position > values.shooter_total_length - 0.03) {
+ shooter_.SetGoalPosition(position->position, 0.0);
+ state_ = STATE_READY_UNLOAD;
} else {
- shooter_.SetPosition(
- position->position + values.shooter_zeroing_speed*dt
+ shooter_.SetGoalPosition(
+ position->position + values.shooter_zeroing_speed*dt,
values.shooter_zeroing_speed);
}
@@ -297,14 +282,15 @@
}
if (position) {
- LOG(DEBUG, "pos: hall: absolute: %f\n",
- //position->pos,
- //position->hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
+ last_position_ = *position;
+ LOG(DEBUG, "pos: hall: real: %.2f absolute: %.2f\n",
+ real_position, position->position);
}
- output->voltage = voltage;
- status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
+ if (!shooter_loop_disable) {
+ output->voltage = shooter_.voltage();
+ }
+ status->done = ::std::abs(position->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 30112e1..c19328f 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -5,12 +5,12 @@
#include "aos/common/control_loop/ControlLoop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "aos/common/time.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
#include "frc971/control_loops/shooter/shooter.q.h"
-#include "frc971/control_loops/zeroed_joint.h"
-
namespace frc971 {
namespace control_loops {
namespace testing {
@@ -18,6 +18,8 @@
class ShooterTest_NoWindupNegative_Test;
};
+using ::aos::time::Time;
+
// Note: Everything in this file assumes that there is a 1 cycle delay between
// power being requested and it showing up at the motor. It assumes that
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
@@ -75,11 +77,11 @@
}
- bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ bool SetCalibrationOnEdge(const constants::Values::ShooterLimits &shooter_values,
JointZeroingState zeroing_state) {
double edge_encoder;
double known_position;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &known_position)) {
+ if (GetPositionOfEdge(shooter_values, &edge_encoder, &known_position)) {
LOG(INFO, "Calibration edge.\n");
SetCalibration(edge_encoder, known_position);
set_zeroing_state(zeroing_state);
@@ -89,19 +91,27 @@
}
- void SetPositionValues(double poistion) {
+ void SetPositionValues(double position) {
Eigen::Matrix<double, 1, 1> Y;
Y << position;
Correct(Y);
}
- void SetGoalPositionVelocity(double desired_position,
+ void SetGoalPosition(double desired_position,
double desired_velocity) {
// austin said something about which matrix to set, but I didn't under
// very much of it
//some_matrix = {desired_position, desired_velocity};
- printf("%s:%d : seg fault?\n", __FILE__, __LINE__);
+ printf("%s:%d : seg fault (%.2f, %.2f)\n", __FILE__, __LINE__,
+ desired_position, desired_velocity);
+ *(const char **)(NULL) = "seg fault";
+ }
+
+ // apply a small amout of voltage outside the loop so we can
+ // take up backlash in gears
+ void ApplySomeVoltage() {
+ printf("%s:%d : seg fault\n", __FILE__, __LINE__);
*(const char **)(NULL) = "seg fault";
}
@@ -111,7 +121,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::Shooter &shooter,
+ bool GetPositionOfEdge(const constants::Values::ShooterLimits &shooter,
double *edge_encoder, double *known_position);
#undef COUNT_SETTER_GETTER
@@ -132,34 +142,21 @@
class ShooterMotor
- : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ : public aos::control_loops::ControlLoop<control_loops::ShooterGroup> {
public:
explicit ShooterMotor(
- control_loops::ShooterLoop *my_shooter = &control_loops::shooter_queue_group);
+ control_loops::ShooterGroup *my_shooter = &control_loops::shooter_queue_group);
// True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+ //bool capped_goal() const { return shooter_.capped_goal(); }
- // True if the shooter is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
-
- // True if the shooter is zeroing.
- bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
-
- // True if the state machine is uninitialized.
- bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
-
- // True if the state machine is ready.
- bool is_ready() const { return zeroed_joint_.is_ready(); }
-
-enum {
+typedef enum {
STATE_INITIALIZE,
STATE_REQUEST_LOAD,
STATE_LOAD_BACKTRACK,
STATE_LOAD,
STATE_LOADING_PROBLEM,
STATE_PREPARE_SHOT,
- STATE_BRAKE_SET,
STATE_READY,
STATE_REQUEST_FIRE,
STATE_PREPARE_FIRE,
@@ -170,21 +167,46 @@
} State;
protected:
+
+ double PowerToPosition(double power) { return power; }
+
virtual void RunIteration(
- const ShooterLoop::Goal *goal,
- const control_loops::ShooterLoop::Position *position,
- ShooterLoop::Output *output,
- ShooterLoop::Status *status);
+ const ShooterGroup::Goal *goal,
+ const control_loops::ShooterGroup::Position *position,
+ ShooterGroup::Output *output,
+ ShooterGroup::Status *status);
private:
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_NoWindupPositive_Test;
friend class testing::ShooterTest_NoWindupNegative_Test;
- // The zeroed joint to use.
- ZeroedJoint<1> zeroed_joint_;
+ control_loops::ShooterGroup::Position last_position_;
+
ZeroedStateFeedbackLoop shooter_;
+ // position need to zero
+ double calibration_position_;
+
+ // state machine state
+ State state_;
+
+ // time to giving up on loading problem
+ Time loading_problem_end_time_;
+
+ // wait for brake to set
+ Time shooter_brake_set_time_;
+
+ // we are attempting to take up some of the backlash
+ // in the gears before the plunger hits
+ Time prepare_fire_end_time_;
+
+ // time that shot must have completed
+ Time shot_end_time_;
+
+ // track cycles that we are stuck to detect errors
+ int cycles_not_moved_;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 3f81a4f..f837293 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -23,26 +23,26 @@
public:
// Constructs a motor simulation.
ShooterSimulation(double initial_position)
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant()))
- shooter_queue_group(".frc971.control_loops.claw_queue_group", 0x9f1a99dd,
- ".frc971.control_loops.claw_queue_group.goal",
- ".frc971.control_loops.claw_queue_group.position",
- ".frc971.control_loops.claw_queue_group.output",
- ".frc971.control_loops.claw_queue_group.status") {
+ : shooter_plant_(new StateFeedbackPlant<3, 1, 1>(MakeShooterPlant())),
+ shooter_queue_group_(".frc971.control_loops.shooter_queue_group", 0x9f1a99dd,
+ ".frc971.control_loops.shooter_queue_group.goal",
+ ".frc971.control_loops.shooter_queue_group.position",
+ ".frc971.control_loops.shooter_queue_group.output",
+ ".frc971.control_loops.shooter_queue_group.status") {
Reinitialize(initial_position);
}
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {top: %f}\n", initial_position);
- StateFeedbackPlant<2, 1, 1>* plant = shooter_plant_.get();
+ StateFeedbackPlant<3, 1, 1>* plant = shooter_plant_.get();
initial_position_ = initial_position;
plant->X(0, 0) = initial_position_;
plant->X(1, 0) = 0.0;
plant->Y = plant->C() * plant->X;
last_voltage_ = 0.0;
- last_position_.Zero();
- SetPhysicalSensors(&last_position_);
+ last_plant_position_ = 0.0;
+ SetPhysicalSensors(&last_position_message_);
}
@@ -59,8 +59,8 @@
// Makes sure pos is inside range (inclusive)
- bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
- return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ bool CheckRange(double pos, struct constants::Values::Pair pair) {
+ return (pos >= pair.lower_limit && pos <= pair.upper_limit);
}
@@ -75,23 +75,21 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
position->plunger_back_hall_effect =
- CheckRange(position->position, values.plunger_heffect);
+ CheckRange(position->position, values.shooter.plunger_back);
position->pusher_distal_hall_effect =
- CheckRange(position->position, values.pusher_distal_heffect);
+ CheckRange(position->position, values.shooter.pusher_distal);
position->pusher_proximal_hall_effect =
- CheckRange(position->position, values.pusher_proximal_heffect);
- position->latch_hall_effect =
- CheckRange(position->position, values.latch_heffect);
+ CheckRange(position->position, values.shooter.pusher_proximal);
}
void UpdateEffectEdge(bool effect, bool last_effect,
double upper_angle, double lower_angle, double position,
double &posedge_value, double &negedge_value,
- int &posedge_count, int &negedge_count) {
+ int64_t &posedge_count, int64_t &negedge_count) {
if (effect && !last_effect) {
++posedge_count;
- if (last_position_.position < lower_angle) {
+ if (last_position_message_.position < lower_angle) {
posedge_value = lower_angle - initial_position_;
} else {
posedge_value = upper_angle - initial_position_;
@@ -103,7 +101,7 @@
if (position < lower_angle) {
negedge_value = lower_angle - initial_position_;
} else {
- negedge_value - upper_angle - initial_position_;
+ negedge_value = upper_angle - initial_position_;
}
}
}
@@ -113,45 +111,16 @@
void SendPositionMessage() {
const frc971::constants::Values& values = constants::GetValues();
::aos::ScopedMessagePtr<control_loops::ShooterGroup::Position> position =
- shooter_queue_group.position.MakeMessage();
-
- // Initialize all the counters to their previous values.
- *position = last_position_;
+ shooter_queue_group_.position.MakeMessage();
SetPhysicalSensors(position.get());
- // Handle the front hall effect.
- if (position->plunger_back_hall_effect &&
- !last_position_.plunger_back_hall_effect) {
- ++position->plunger_back_hall_effect_posedge_count;
-
- if (last_position_.position < values.upper_claw.front.lower_angle) {
- position->top.posedge_value =
- values.upper_claw.front.lower_angle - initial_position_;
- } else {
- position->top.posedge_value =
- values.upper_claw.front.upper_angle - initial_position_;
- }
- }
- if (!position->plunger_back_hall_effect &&
- last_position_.plunger_back_hall_effect) {
- ++position->plunger_back_hall_effect_negedge_count;
-
- if (position->top.position < values.upper_claw.front.lower_angle) {
- position->top.negedge_value =
- values.upper_claw.front.lower_angle - initial_position_;
- } else {
- position->top.negedge_value =
- values.upper_claw.front.upper_angle - initial_position_;
- }
- }
-
// Handle plunger hall effect
UpdateEffectEdge(
position->plunger_back_hall_effect,
- last_position_.plunger_back_hall_effect,
- values.plunger_back.upper_angle,
- values.plunger_back.lower_angle,
+ 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,
@@ -161,9 +130,9 @@
// Handle pusher distal hall effect
UpdateEffectEdge(
position->pusher_distal_hall_effect,
- last_position_.pusher_distal_hall_effect,
- values.pusher_distal.upper_angle,
- values.pusher_distal.lower_angle,
+ 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,
@@ -173,59 +142,46 @@
// Handle pusher proximal hall effect
UpdateEffectEdge(
position->pusher_proximal_hall_effect,
- last_position_.pusher_proximal_hall_effect,
- values.pusher_proximal.upper_angle,
- values.pusher_proximal.lower_angle,
+ 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);
- // Handle latch hall effect
- UpdateEffectEdge(
- position->latch_hall_effect,
- last_position_.latch_hall_effect,
- values.latch.upper_angle,
- values.latch.lower_angle,
- position->position,
- position->posedge_value,
- position->negedge_value,
- position->latch_hall_effect_posedge_count,
- position->latch_hall_effect_negedge_count);
-
- // Only set calibration if it changed last cycle. Calibration starts out
- // with a value of 0.
- last_position_ = *position;
+ last_position_message_ = *position;
}
// Simulates the claw moving for one timestep.
void Simulate() {
- last_position_ = shooter_plant_->Y(0, 0);
- EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ last_plant_position_ = shooter_plant_->Y(0, 0);
+ EXPECT_TRUE(shooter_queue_group_.output.FetchLatest());
shooter_plant_->U << last_voltage_;
shooter_plant_->Update();
- EXPECT_GE(constants::GetValues().shooter_upper_physical_limit,
+ EXPECT_GE(constants::GetValues().shooter.upper_limit,
shooter_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().shooter_lower_physical_limit,
+ EXPECT_LE(constants::GetValues().shooter.lower_limit,
shooter_plant_->Y(0, 0));
- last_voltage_ = my_shooter_loop_.output->voltage;
+ last_voltage_ = shooter_queue_group_.output->voltage;
}
- // Top of the claw, the one with rollers
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ // pointer to plant
+ ::std::unique_ptr<StateFeedbackPlant<3, 1, 1>> shooter_plant_;
private:
- ShooterGroup shooter_queue_group;
+ ShooterGroup shooter_queue_group_;
double initial_position_;
double last_voltage_;
- control_loops::ShooterGroup::Position last_position_;
+ control_loops::ShooterGroup::Position last_position_message_;
+ double last_plant_position_;
};
@@ -236,18 +192,18 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- ShooterGroup shooter_queue_group;
+ ShooterGroup shooter_queue_group_;
// Create a loop and simulation plant.
ShooterMotor shooter_motor_;
- ShooterMotorSimulation shooter_motor_plant_;
+ ShooterSimulation shooter_motor_plant_;
- ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ ShooterTest() : shooter_queue_group_(".frc971.control_loops.shooter",
0x1a7b7094, ".frc971.control_loops.shooter.goal",
".frc971.control_loops.shooter.position",
".frc971.control_loops.shooter.output",
".frc971.control_loops.shooter.status"),
- shooter_motor_(&my_shooter_loop_),
+ shooter_motor_(&shooter_queue_group_),
shooter_motor_plant_(0.5) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
@@ -265,10 +221,10 @@
void VerifyNearGoal() {
- shooter_queue_group.goal.FetchLatest();
- shooter_queue_group.position.FetchLatest();
+ shooter_queue_group_.goal.FetchLatest();
+ shooter_queue_group_.position.FetchLatest();
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_group.goal->shot_power, pos, 1e-4);
+ EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 1e-4);
}
virtual ~ShooterTest() {
@@ -279,7 +235,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, ZerosCorrectly) {
- shooter_queue_group.goal.MakeWithBuilder()
+ shooter_queue_group_.goal.MakeWithBuilder()
.shot_power(5050.1)
.Send();
for (int i = 0; i < 400; ++i) {