shooter state machine (control loop) and test are written and build. not tested for functionality
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);
};