Wrist now moves in the test, but doesn't zero.
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index e057c3e..fc5e917 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -9,8 +9,6 @@
#include "frc971/control_loops/claw/top_claw_motor_plant.h"
#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
-#include "frc971/control_loops/zeroed_joint.h"
-
namespace frc971 {
namespace control_loops {
namespace testing {
@@ -18,33 +16,134 @@
class ClawTest_NoWindupNegative_Test;
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawLoop> {
+// 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
+// that isn't true.
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+// It does not have any zeroing logic in it, only logic to deal with a delta U
+// controller.
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
public:
- explicit ClawMotor(
- control_loops::ClawLoop *my_claw = &control_loops::claw);
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ voltage_(0.0),
+ last_voltage_(0.0),
+ uncapped_voltage_(0.0),
+ front_hall_effect_posedge_count_(0.0),
+ previous_front_hall_effect_posedge_count_(0.0),
+ front_hall_effect_negedge_count_(0.0),
+ previous_front_hall_effect_negedge_count_(0.0),
+ calibration_hall_effect_posedge_count_(0.0),
+ previous_calibration_hall_effect_posedge_count_(0.0),
+ calibration_hall_effect_negedge_count_(0.0),
+ previous_calibration_hall_effect_negedge_count_(0.0),
+ back_hall_effect_posedge_count_(0.0),
+ previous_back_hall_effect_posedge_count_(0.0),
+ back_hall_effect_negedge_count_(0.0),
+ previous_back_hall_effect_negedge_count_(0.0),
+ zeroing_state_(UNKNOWN_POSITION) {}
- // True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+ const static int kZeroingMaxVoltage = 5;
- // True if the claw is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
- // True if the claw is zeroing.
- bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
- // True if the state machine is uninitialized.
- bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
+
+ enum JointZeroingState {
+ // We don't know where the joint is at all.
+ UNKNOWN_POSITION,
+ // We have an approximate position for where the claw is using.
+ APPROXIMATE_CALIBRATION,
+ // We observed the calibration edge while disabled. This is good enough for
+ // autonomous mode.
+ DISABLED_CALIBRATION,
+ // Ready for use during teleop.
+ CALIBRATED
+ };
+
+ void set_zeroing_state(JointZeroingState zeroing_state) {
+ zeroing_state_ = zeroing_state;
+ }
+ JointZeroingState zeroing_state() const { return zeroing_state_; }
+
+#define COUNT_SETTER_GETTER(variable) \
+ void set_##variable(int32_t value) { \
+ previous_##variable##_ = variable##_; \
+ variable##_ = value; \
+ } \
+ int32_t variable() const { return variable##_; } \
+ bool variable##_changed() const { \
+ return previous_##variable##_ != variable##_; \
+ }
+
+ COUNT_SETTER_GETTER(front_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(front_hall_effect_negedge_count);
+ COUNT_SETTER_GETTER(calibration_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(calibration_hall_effect_negedge_count);
+ COUNT_SETTER_GETTER(back_hall_effect_posedge_count);
+ COUNT_SETTER_GETTER(back_hall_effect_negedge_count);
+
+ bool any_hall_effect_changed() const {
+ return front_hall_effect_posedge_count_changed() ||
+ front_hall_effect_negedge_count_changed() ||
+ calibration_hall_effect_posedge_count_changed() ||
+ calibration_hall_effect_negedge_count_changed() ||
+ back_hall_effect_posedge_count_changed() ||
+ back_hall_effect_negedge_count_changed();
+ }
+
+#undef COUNT_SETTER_GETTER
+
+ private:
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
+
+ int32_t front_hall_effect_posedge_count_;
+ int32_t previous_front_hall_effect_posedge_count_;
+ int32_t front_hall_effect_negedge_count_;
+ int32_t previous_front_hall_effect_negedge_count_;
+ int32_t calibration_hall_effect_posedge_count_;
+ int32_t previous_calibration_hall_effect_posedge_count_;
+ int32_t calibration_hall_effect_negedge_count_;
+ int32_t previous_calibration_hall_effect_negedge_count_;
+ int32_t back_hall_effect_posedge_count_;
+ int32_t previous_back_hall_effect_posedge_count_;
+ int32_t back_hall_effect_negedge_count_;
+ int32_t previous_back_hall_effect_negedge_count_;
+
+ JointZeroingState zeroing_state_;
+};
+
+class ClawMotor
+ : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+ public:
+ explicit ClawMotor(control_loops::ClawGroup *my_claw =
+ &control_loops::claw_queue_group);
// True if the state machine is ready.
- bool is_ready() const { return zeroed_joint_.is_ready(); }
+ bool is_ready() const { return false; }
protected:
- virtual void RunIteration(
- const control_loops::ClawLoop::Goal *goal,
- const control_loops::ClawLoop::Position *position,
- control_loops::ClawLoop::Output *output,
- ::aos::control_loops::Status *status);
+ virtual void RunIteration(const control_loops::ClawGroup::Goal *goal,
+ const control_loops::ClawGroup::Position *position,
+ control_loops::ClawGroup::Output *output,
+ ::aos::control_loops::Status *status);
+
+ double top_absolute_position() const { return top_claw_.X_hat(0, 0); }
+ double bottom_absolute_position() const { return bottom_claw_.X_hat(0, 0); }
private:
// Friend the test classes for acces to the internal state.
@@ -52,7 +151,15 @@
friend class testing::ClawTest_NoWindupNegative_Test;
// The zeroed joint to use.
- ZeroedJoint<1> zeroed_joint_;
+ bool has_top_claw_goal_;
+ double top_claw_goal_;
+ ZeroedStateFeedbackLoop top_claw_;
+
+ bool has_bottom_claw_goal_;
+ double bottom_claw_goal_;
+ ZeroedStateFeedbackLoop bottom_claw_;
+
+ bool was_enabled_;
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};