Claw now zeros!
- Debugged plant
- Switched to a simple controller architecture.
- Fixed controller.
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index c045d6f..8157b67 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -7,8 +7,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/control_loops/claw/top_claw_motor_plant.h"
-#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+#include "frc971/control_loops/claw/claw_motor_plant.h"
namespace frc971 {
namespace control_loops {
@@ -22,18 +21,28 @@
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
// that isn't true.
+class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
+ : StateFeedbackLoop<4, 2, 2>(loop) {}
+ virtual void CapU();
+
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+};
+
+class ClawMotor;
+
// 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> {
+class ZeroedStateFeedbackLoop {
public:
- ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop)
- : StateFeedbackLoop<3, 1, 1>(loop),
- voltage_(0.0),
- last_voltage_(0.0),
- uncapped_voltage_(0.0),
- offset_(0.0),
+ ZeroedStateFeedbackLoop(const char *name, ClawMotor *motor)
+ : offset_(0.0),
+ name_(name),
+ motor_(motor),
front_hall_effect_posedge_count_(0.0),
previous_front_hall_effect_posedge_count_(0.0),
front_hall_effect_negedge_count_(0.0),
@@ -46,26 +55,17 @@
previous_back_hall_effect_posedge_count_(0.0),
back_hall_effect_negedge_count_(0.0),
previous_back_hall_effect_negedge_count_(0.0),
+ front_hall_effect_(false),
+ calibration_hall_effect_(false),
+ back_hall_effect_(false),
zeroing_state_(UNKNOWN_POSITION),
posedge_value_(0.0),
negedge_value_(0.0),
encoder_(0.0),
- last_encoder_(0.0){}
+ last_encoder_(0.0) {}
const static int kZeroingMaxVoltage = 5;
- // Caps U, but this time respects the state of the wrist as well.
- virtual void CapU();
-
- // Returns the accumulated voltage.
- double voltage() const { return voltage_; }
-
- // 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,
@@ -83,25 +83,6 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- // Sets the calibration offset given the absolute angle and the corrisponding
- // encoder value.
- void SetCalibration(double edge_encoder, double edge_angle) {
- offset_ = edge_angle - edge_encoder;
- }
-
- bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
- JointZeroingState zeroing_state) {
- double edge_encoder;
- double edge_angle;
- if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
- SetCalibration(edge_encoder, edge_angle);
- set_zeroing_state(zeroing_state);
- return true;
- }
- return false;
- }
-
void SetPositionValues(const HalfClawPosition &claw) {
set_front_hall_effect_posedge_count(claw.front_hall_effect_posedge_count);
set_front_hall_effect_negedge_count(claw.front_hall_effect_negedge_count);
@@ -114,11 +95,20 @@
posedge_value_ = claw.posedge_value;
negedge_value_ = claw.negedge_value;
- Eigen::Matrix<double, 1, 1> Y;
- Y << claw.position;
- Correct(Y);
+ last_encoder_ = encoder_;
+ encoder_ = claw.position;
+
+ front_hall_effect_ = claw.front_hall_effect;
+ calibration_hall_effect_ = claw.calibration_hall_effect;
+ back_hall_effect_ = claw.back_hall_effect;
}
+ double absolute_position() const { return encoder() + offset(); }
+
+ bool front_hall_effect() const { return front_hall_effect_; }
+ bool calibration_hall_effect() const { return calibration_hall_effect_; }
+ bool back_hall_effect() const { return back_hall_effect_; }
+
#define COUNT_SETTER_GETTER(variable) \
void set_##variable(int32_t value) { \
previous_##variable##_ = variable##_; \
@@ -146,10 +136,11 @@
back_hall_effect_negedge_count_changed();
}
- double position() const { return X_hat(0, 0); }
double encoder() const { return encoder_; }
double last_encoder() const { return last_encoder_; }
+ double offset() const { return offset_; }
+
// 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::Claw &claw,
@@ -157,14 +148,12 @@
#undef COUNT_SETTER_GETTER
- private:
+ protected:
// The accumulated voltage to apply to the motor.
- double voltage_;
- double last_voltage_;
- double uncapped_voltage_;
double offset_;
+ const char *name_;
- double previous_position_;
+ ClawMotor *motor_;
int32_t front_hall_effect_posedge_count_;
int32_t previous_front_hall_effect_posedge_count_;
@@ -178,6 +167,9 @@
int32_t previous_back_hall_effect_posedge_count_;
int32_t back_hall_effect_negedge_count_;
int32_t previous_back_hall_effect_negedge_count_;
+ bool front_hall_effect_;
+ bool calibration_hall_effect_;
+ bool back_hall_effect_;
JointZeroingState zeroing_state_;
double posedge_value_;
@@ -186,6 +178,49 @@
double last_encoder_;
};
+class TopZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ TopZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("top", motor) {}
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+ }
+};
+
+class BottomZeroedStateFeedbackLoop : public ZeroedStateFeedbackLoop {
+ public:
+ BottomZeroedStateFeedbackLoop(ClawMotor *motor)
+ : ZeroedStateFeedbackLoop("bottom", motor) {}
+ // Sets the calibration offset given the absolute angle and the corrisponding
+ // encoder value.
+ void SetCalibration(double edge_encoder, double edge_angle);
+
+ bool SetCalibrationOnEdge(const constants::Values::Claw &claw_values,
+ JointZeroingState zeroing_state) {
+ double edge_encoder;
+ double edge_angle;
+ if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
+ LOG(INFO, "Calibration edge.\n");
+ SetCalibration(edge_encoder, edge_angle);
+ set_zeroing_state(zeroing_state);
+ return true;
+ }
+ return false;
+ }
+};
class ClawMotor
: public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
@@ -196,14 +231,19 @@
// True if the state machine is ready.
bool is_ready() const { return false; }
+ void ChangeTopOffset(double doffset);
+ void ChangeBottomOffset(double doffset);
+
protected:
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_.position(); }
- double bottom_absolute_position() const { return bottom_claw_.position(); }
+ double top_absolute_position() const {
+ return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
+ }
+ double bottom_absolute_position() const { return claw_.X_hat(0, 0); }
private:
// Friend the test classes for acces to the internal state.
@@ -213,15 +253,22 @@
// The zeroed joint to use.
bool has_top_claw_goal_;
double top_claw_goal_;
- ZeroedStateFeedbackLoop top_claw_;
+ TopZeroedStateFeedbackLoop top_claw_;
bool has_bottom_claw_goal_;
double bottom_claw_goal_;
- ZeroedStateFeedbackLoop bottom_claw_;
+ BottomZeroedStateFeedbackLoop bottom_claw_;
+
+ // The claw loop.
+ ClawLimitedLoop claw_;
bool was_enabled_;
bool doing_calibration_fine_tune_;
+ // The initial seperation when disabled. Used as the safe seperation
+ // distance.
+ double initial_seperation_;
+
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};