Wrist now moves in the test, but doesn't zero.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index bbc1966..815bebb 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -53,27 +53,13 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
- 2.05,
- 2.05,
- 0.0,
- 0.0,
1.0,
0.1,
0.0,
1.57,
- -0.1,
- 0.05,
- 1.0,
- 1.1,
- 2.0,
- 2.1,
- -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.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
};
break;
case kPracticeTeamNumber:
@@ -86,27 +72,12 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
- 2.05,
- 2.05,
- 0.0,
- 0.0,
1.0,
0.1,
0.0,
1.57,
-
- -0.1,
- 0.05,
- 1.0,
- 1.1,
- 2.0,
- 2.1,
- -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.0, 2.05, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 2e8af5a..9de3a28 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -40,10 +40,6 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
- double upper_claw_lower_limit;
- double upper_claw_upper_limit;
- double lower_claw_lower_limit;
- double lower_claw_upper_limit;
double claw_zeroing_off_speed;
double claw_zeroing_speed;
@@ -52,18 +48,21 @@
double claw_max_seperation;
// Three hall effects are known as front, calib and back
- double upper_claw_front_heffect_lower_angle;
- double upper_claw_front_heffect_upper_angle;
- double upper_claw_calib_heffect_lower_angle;
- double upper_claw_calib_heffect_upper_angle;
- double upper_claw_back_heffect_lower_angle;
- double upper_claw_back_heffect_upper_angle;
- double lower_claw_front_heffect_lower_angle;
- double lower_claw_front_heffect_upper_angle;
- double lower_claw_calib_heffect_lower_angle;
- double lower_claw_calib_heffect_upper_angle;
- double lower_claw_back_heffect_lower_angle;
- double lower_claw_back_heffect_upper_angle;
+ struct AnglePair {
+ double lower_angle;
+ double upper_angle;
+ };
+
+ struct Claw {
+ double lower_limit;
+ double upper_limit;
+ AnglePair front;
+ AnglePair calibration;
+ AnglePair back;
+ };
+
+ Claw upper_claw;
+ Claw lower_claw;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 44df982..fb16c51 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -46,26 +46,45 @@
namespace frc971 {
namespace control_loops {
+void ZeroedStateFeedbackLoop::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ double limit = zeroing_state_ != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+ LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
+ }
+
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ U(0, 0) = voltage_ - old_voltage;
+ //LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
+ //LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+
+ last_voltage_ = voltage_;
+}
+
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
: aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
- zeroed_joint_(MakeTopClawLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<1>::ConfigurationData config_data;
+ has_top_claw_goal_(false),
+ top_claw_goal_(0.0),
+ top_claw_(MakeTopClawLoop()),
+ has_bottom_claw_goal_(false),
+ bottom_claw_goal_(0.0),
+ bottom_claw_(MakeBottomClawLoop()),
+ was_enabled_(false) {}
- config_data.lower_limit = GetValues().claw_lower_limit;
- config_data.upper_limit = GetValues().claw_upper_limit;
- config_data.hall_effect_start_angle[0] =
- GetValues().claw_hall_effect_start_angle;
- config_data.zeroing_off_speed = GetValues().claw_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().claw_zeroing_speed;
-
- config_data.max_zeroing_voltage = 5.0;
- config_data.deadband_voltage = 0.0;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
+const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
// Positive angle is up, and positive power is up.
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
@@ -81,32 +100,131 @@
output->intake_voltage = 0;
}
- ZeroedJoint<1>::PositionData transformed_position;
- ZeroedJoint<1>::PositionData *transformed_position_ptr =
- &transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- transformed_position.position = position->top_position;
- transformed_position.hall_effects[0] = position->top_calibration_hall_effect;
- transformed_position.hall_effect_positions[0] = position->top_posedge_value;
+ // TODO(austin): Handle the disabled state and the disabled -> enabled
+ // transition in all of these states.
+ // TODO(austin): Handle zeroing while disabled.
+
+ // TODO(austin): Save all the counters so we know when something actually
+ // happens.
+ // TODO(austin): Helpers to find the position of the claw on an edge.
+
+ // TODO(austin): This may not be necesary because of the ControlLoop class.
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() == nullptr) {
+ return;
}
- const double voltage =
- zeroed_joint_.Update(transformed_position_ptr, output != NULL,
- goal->bottom_angle + goal->seperation_angle, 0.0);
+ if (position) {
+ if (!has_top_claw_goal_) {
+ has_top_claw_goal_ = true;
+ top_claw_goal_ = position->top.position;
+ }
+ if (!has_bottom_claw_goal_) {
+ has_bottom_claw_goal_ = true;
+ bottom_claw_goal_ = position->bottom.position;
+ }
+
+ top_claw_.set_front_hall_effect_posedge_count(
+ position->top.front_hall_effect_posedge_count);
+ top_claw_.set_front_hall_effect_negedge_count(
+ position->top.front_hall_effect_negedge_count);
+ top_claw_.set_calibration_hall_effect_posedge_count(
+ position->top.calibration_hall_effect_posedge_count);
+ top_claw_.set_calibration_hall_effect_negedge_count(
+ position->top.calibration_hall_effect_negedge_count);
+ top_claw_.set_back_hall_effect_posedge_count(
+ position->top.back_hall_effect_posedge_count);
+ top_claw_.set_back_hall_effect_negedge_count(
+ position->top.back_hall_effect_negedge_count);
+
+ bottom_claw_.set_front_hall_effect_posedge_count(
+ position->bottom.front_hall_effect_posedge_count);
+ bottom_claw_.set_front_hall_effect_negedge_count(
+ position->bottom.front_hall_effect_negedge_count);
+ bottom_claw_.set_calibration_hall_effect_posedge_count(
+ position->bottom.calibration_hall_effect_posedge_count);
+ bottom_claw_.set_calibration_hall_effect_negedge_count(
+ position->bottom.calibration_hall_effect_negedge_count);
+ bottom_claw_.set_back_hall_effect_posedge_count(
+ position->bottom.back_hall_effect_posedge_count);
+ bottom_claw_.set_back_hall_effect_negedge_count(
+ position->bottom.back_hall_effect_negedge_count);
+ }
+
+ bool autonomous = ::aos::robot_state->autonomous;
+
+ if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ // Ready to use the claw.
+ // Limit the goals here.
+ } else if (top_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ // Time to fine tune the zero.
+ // Limit the goals here.
+ if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
+ } else {
+ }
+ } else {
+ if (!was_enabled_ && enabled) {
+
+ }
+ // Limit the goals here.
+ if (top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ }
+ if (bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ }
+
+ if (bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ // Time to slowly move back up to find any position to narrow down the
+ // zero.
+ } else {
+ // We don't know where either claw is. Slowly start moving down to find
+ // any hall effect.
+ LOG(INFO, "Unknown position\n");
+ }
+ }
+
+ // TODO(austin): Handle disabled.
if (position) {
- LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
- position->top_calibration_hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
+ top_claw_.Y << position->top.position;
+ bottom_claw_.Y << position->bottom.position;
+ }
+
+ // TODO(austin): ...
+ top_claw_.R << goal->bottom_angle + goal->seperation_angle, 0.0, 0.0;
+ bottom_claw_.R << goal->bottom_angle, 0.0, 0.0;
+
+ top_claw_.Update(position != nullptr, output == nullptr);
+ bottom_claw_.Update(position != nullptr, output == nullptr);
+
+ if (position) {
+ //LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
+ //position->top_calibration_hall_effect ? "true" : "false",
+ //zeroed_joint_.absolute_position());
}
if (output) {
- output->top_claw_voltage = voltage;
+ output->top_claw_voltage = top_claw_.voltage();
+ output->bottom_claw_voltage = bottom_claw_.voltage();
}
- status->done = ::std::abs(zeroed_joint_.absolute_position() -
- goal->bottom_angle - goal->seperation_angle) < 0.004;
+ status->done = false;
+ //::std::abs(zeroed_joint_.absolute_position() - goal->bottom_angle -
+ //goal->seperation_angle) < 0.004;
+
+ was_enabled_ = ::aos::robot_state->enabled;
}
} // namespace control_loops
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);
};
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 03f1930..03dfdad 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -2,6 +2,26 @@
import "aos/common/control_loop/control_loops.q";
+struct Claw {
+ double position;
+ bool front_hall_effect;
+ int32_t front_hall_effect_posedge_count;
+ int32_t front_hall_effect_negedge_count;
+ bool calibration_hall_effect;
+ int32_t calibration_hall_effect_posedge_count;
+ int32_t calibration_hall_effect_negedge_count;
+ bool back_hall_effect;
+ int32_t back_hall_effect_posedge_count;
+ int32_t back_hall_effect_negedge_count;
+
+ // The encoder value at the last posedge of any of the claw hall effect
+ // sensors.
+ double posedge_value;
+ // The encoder value at the last negedge of any of the claw hall effect
+ // sensors.
+ double negedge_value;
+};
+
// All angles here are 0 horizontal, positive up.
queue_group ClawGroup {
implements aos.control_loops.ControlLoop;
@@ -13,49 +33,52 @@
double seperation_angle;
bool intake;
};
+
message Position {
// Top claw position relative to power on.
- double top_position;
+ //double top_position;
+ Claw top;
+ Claw bottom;
// Three Hall Effects with respect to the top claw
- bool top_front_hall_effect;
- int32_t top_front_hall_effect_posedge_count;
- int32_t top_front_hall_effect_negedge_count;
- bool top_calibration_hall_effect;
- int32_t top_calibration_hall_effect_posedge_count;
- int32_t top_calibration_hall_effect_negedge_count;
- bool top_back_hall_effect;
- int32_t top_back_hall_effect_posedge_count;
- int32_t top_back_hall_effect_negedge_count;
+ //bool top_front_hall_effect;
+ //int32_t top_front_hall_effect_posedge_count;
+ //int32_t top_front_hall_effect_negedge_count;
+ //bool top_calibration_hall_effect;
+ //int32_t top_calibration_hall_effect_posedge_count;
+ //int32_t top_calibration_hall_effect_negedge_count;
+ //bool top_back_hall_effect;
+ //int32_t top_back_hall_effect_posedge_count;
+ //int32_t top_back_hall_effect_negedge_count;
// The encoder value at the last posedge of any of the top claw hall effect
// sensors.
- double top_posedge_value;
+ //double top_posedge_value;
// The encoder value at the last negedge of any of the top claw hall effect
// sensors.
- double top_negedge_value;
+ //double top_negedge_value;
// bottom claw relative position
- double bottom_position;
+ //double bottom_position;
// Three Hall Effects with respect to the bottom claw
- bool bottom_front_hall_effect;
- int32_t bottom_front_hall_effect_posedge_count;
- int32_t bottom_front_hall_effect_negedge_count;
- bool bottom_calibration_hall_effect;
- int32_t bottom_calibration_hall_effect_posedge_count;
- int32_t bottom_calibration_hall_effect_negedge_count;
- bool bottom_back_hall_effect;
- int32_t bottom_back_hall_effect_posedge_count;
- int32_t bottom_back_hall_effect_negedge_count;
+ //bool bottom_front_hall_effect;
+ //int32_t bottom_front_hall_effect_posedge_count;
+ //int32_t bottom_front_hall_effect_negedge_count;
+ //bool bottom_calibration_hall_effect;
+ //int32_t bottom_calibration_hall_effect_posedge_count;
+ //int32_t bottom_calibration_hall_effect_negedge_count;
+ //bool bottom_back_hall_effect;
+ //int32_t bottom_back_hall_effect_posedge_count;
+ //int32_t bottom_back_hall_effect_negedge_count;
// The encoder value at the last posedge of any of the bottom claw hall
// effect sensors.
- double bottom_posedge_value;
+ //double bottom_posedge_value;
// The encoder value at the last negedge of any of the bottom claw hall
// effect sensors.
- double bottom_negedge_value;
+ //double bottom_negedge_value;
};
message Output {
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index c0480a5..9e8e097 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -36,17 +36,24 @@
: top_claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant())),
bottom_claw_plant_(
new StateFeedbackPlant<2, 1, 1>(MakeRawBottomClawPlant())),
- claw_queue_group(".frc971.control_loops.claw", 0x1a7b7094,
- ".frc971.control_loops.claw.goal",
- ".frc971.control_loops.claw.position",
- ".frc971.control_loops.claw.output",
- ".frc971.control_loops.claw.status") {
- Reinitialize(TOP_CLAW, initial_top_position);
- Reinitialize(BOTTOM_CLAW, initial_bottom_position);
+ claw_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") {
+ Reinitialize(initial_top_position, initial_bottom_position);
+ }
+
+ void Reinitialize(double initial_top_position,
+ double initial_bottom_position) {
+ ReinitializePartial(TOP_CLAW, initial_top_position);
+ ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
+ last_position_.Zero();
+ SetPhysicalSensors(&last_position_);
}
// Resets the plant so that it starts at initial_position.
- void Reinitialize(ClawType type, double initial_position) {
+ void ReinitializePartial(ClawType type, double initial_position) {
StateFeedbackPlant<2, 1, 1>* plant;
if (type == TOP_CLAW) {
plant = top_claw_plant_.get();
@@ -57,8 +64,6 @@
plant->X(0, 0) = initial_position_[type];
plant->X(1, 0) = 0.0;
plant->Y = plant->C() * plant->X;
- last_position_[type] = plant->Y(0, 0);
- calibration_value_[type] = 0.0;
last_voltage_[type] = 0.0;
}
@@ -77,59 +82,207 @@
}
// Makes sure pos is inside range (inclusive)
- bool CheckRange(double pos, double low_limit, double hi_limit) {
- return (pos >= low_limit && pos <= hi_limit);
+ bool CheckRange(double pos, struct constants::Values::AnglePair pair) {
+ return (pos >= pair.lower_angle && pos <= pair.upper_angle);
}
- double CheckCalibration(ClawType type, bool hall_effect, double start_angle,
- double stop_angle) {
- if ((last_position_[type] < start_angle ||
- last_position_[type] > stop_angle) &&
- hall_effect) {
- calibration_value_[type] = start_angle - initial_position_[type];
- }
- return calibration_value_[type];
+ // Sets the values of the physical sensors that can be directly observed
+ // (encoder, hall effect).
+ void SetPhysicalSensors(control_loops::ClawGroup::Position *position) {
+ position->top.position = GetPosition(TOP_CLAW);
+ position->bottom.position = GetPosition(BOTTOM_CLAW);
+
+ double pos[2] = {GetAbsolutePosition(TOP_CLAW),
+ GetAbsolutePosition(BOTTOM_CLAW)};
+
+ const frc971::constants::Values& values = constants::GetValues();
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ position->top.front_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.front);
+ position->top.calibration_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.calibration);
+ position->top.back_hall_effect =
+ CheckRange(pos[TOP_CLAW], values.upper_claw.back);
+
+ position->bottom.front_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.front);
+ position->bottom.calibration_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.calibration);
+ position->bottom.back_hall_effect =
+ CheckRange(pos[BOTTOM_CLAW], values.lower_claw.back);
}
// Sends out the position queue messages.
void SendPositionMessage() {
::aos::ScopedMessagePtr<control_loops::ClawGroup::Position> position =
claw_queue_group.position.MakeMessage();
- position->top_position = GetPosition(TOP_CLAW);
- position->bottom_position = GetPosition(BOTTOM_CLAW);
- // Signal that the hall effect sensor has been triggered if it is within
- // the correct range.
- double pos[2] = {GetAbsolutePosition(TOP_CLAW),
- GetAbsolutePosition(BOTTOM_CLAW)};
- const frc971::constants::Values& v = constants::GetValues();
- position->top_front_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_front_heffect_start_angle,
- v.claw_front_heffect_stop_angle);
- position->top_calibration_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_calib_heffect_start_angle,
- v.claw_calib_heffect_stop_angle);
- position->top_back_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_back_heffect_start_angle,
- v.claw_back_heffect_stop_angle);
- position->bottom_front_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_front_heffect_start_angle,
- v.claw_front_heffect_stop_angle);
- position->bottom_calibration_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_calib_heffect_start_angle,
- v.claw_calib_heffect_stop_angle);
- position->bottom_back_hall_effect =
- CheckRange(pos[TOP_CLAW], v.claw_back_heffect_start_angle,
- v.claw_back_heffect_stop_angle);
+ SetPhysicalSensors(position.get());
+
+ const frc971::constants::Values& values = constants::GetValues();
+
+ // Handle the front hall effect.
+ if (position->top.front_hall_effect &&
+ !last_position_.top.front_hall_effect) {
+ ++position->top.front_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.front.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.front_hall_effect &&
+ last_position_.top.front_hall_effect) {
+ ++position->top.front_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_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the calibration hall effect.
+ if (position->top.calibration_hall_effect &&
+ !last_position_.top.calibration_hall_effect) {
+ ++position->top.calibration_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.calibration.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.calibration_hall_effect &&
+ last_position_.top.calibration_hall_effect) {
+ ++position->top.calibration_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.calibration.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the back hall effect.
+ if (position->top.back_hall_effect &&
+ !last_position_.top.back_hall_effect) {
+ ++position->top.back_hall_effect_posedge_count;
+
+ if (last_position_.top.position < values.upper_claw.back.lower_angle) {
+ position->top.posedge_value =
+ values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.posedge_value =
+ values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->top.back_hall_effect &&
+ last_position_.top.back_hall_effect) {
+ ++position->top.back_hall_effect_negedge_count;
+
+ if (position->top.position < values.upper_claw.back.lower_angle) {
+ position->top.negedge_value =
+ values.upper_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->top.negedge_value =
+ values.upper_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Now deal with the bottom part of the claw.
+ // Handle the front hall effect.
+ if (position->bottom.front_hall_effect &&
+ !last_position_.bottom.front_hall_effect) {
+ ++position->bottom.front_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.front.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.front_hall_effect &&
+ last_position_.bottom.front_hall_effect) {
+ ++position->bottom.front_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.front.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.front.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.front.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the calibration hall effect.
+ if (position->bottom.calibration_hall_effect &&
+ !last_position_.bottom.calibration_hall_effect) {
+ ++position->bottom.calibration_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.calibration.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.calibration_hall_effect &&
+ last_position_.bottom.calibration_hall_effect) {
+ ++position->bottom.calibration_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.calibration.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.calibration.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.calibration.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+
+ // Handle the back hall effect.
+ if (position->bottom.back_hall_effect &&
+ !last_position_.bottom.back_hall_effect) {
+ ++position->bottom.back_hall_effect_posedge_count;
+
+ if (last_position_.bottom.position < values.lower_claw.back.lower_angle) {
+ position->bottom.posedge_value =
+ values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.posedge_value =
+ values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
+ if (!position->bottom.back_hall_effect &&
+ last_position_.bottom.back_hall_effect) {
+ ++position->bottom.back_hall_effect_negedge_count;
+
+ if (position->bottom.position < values.lower_claw.back.lower_angle) {
+ position->bottom.negedge_value =
+ values.lower_claw.back.lower_angle - initial_position_[TOP_CLAW];
+ } else {
+ position->bottom.negedge_value =
+ values.lower_claw.back.upper_angle - initial_position_[TOP_CLAW];
+ }
+ }
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- position->top_calibration = CheckCalibration(
- TOP_CLAW, position->top_calibration_hall_effect,
- v.claw_calib_heffect_start_angle, v.claw_calib_heffect_stop_angle);
- position->bottom_calibration = CheckCalibration(
- BOTTOM_CLAW, position->bottom_calibration_hall_effect,
- v.claw_calib_heffect_start_angle, v.claw_calib_heffect_stop_angle);
+ last_position_ = *position;
position.Send();
}
@@ -137,37 +290,35 @@
void Simulate() {
const frc971::constants::Values& v = constants::GetValues();
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
- Simulate(TOP_CLAW, top_claw_plant_.get(), v.claw_upper_limit,
- v.claw_lower_limit, claw_queue_group.output->top_claw_voltage);
- Simulate(BOTTOM_CLAW, bottom_claw_plant_.get(), v.claw_upper_limit,
- v.claw_lower_limit, claw_queue_group.output->bottom_claw_voltage);
+ Simulate(TOP_CLAW, top_claw_plant_.get(), v.upper_claw,
+ claw_queue_group.output->top_claw_voltage);
+ Simulate(BOTTOM_CLAW, bottom_claw_plant_.get(), v.lower_claw,
+ claw_queue_group.output->bottom_claw_voltage);
}
-
- void Simulate(ClawType type, StateFeedbackPlant<2, 1, 1>* plant,
- double upper_limit, double lower_limit, double nl_voltage) {
- last_position_[type] = plant->Y(0, 0);
- plant->U << last_voltage_[type];
- plant->Update();
-
- // check top claw inside limits
- EXPECT_GE(upper_limit, plant->Y(0, 0));
- EXPECT_LE(lower_limit, plant->Y(0, 0));
- // check bottom claw inside limits
- EXPECT_GE(upper_limit, plant->Y(0, 0));
- EXPECT_LE(lower_limit, plant->Y(0, 0));
- last_voltage_[type] = nl_voltage;
- }
-
// Top of the claw, the one with rollers
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> top_claw_plant_;
// Bottom of the claw, the one with tusks
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> bottom_claw_plant_;
+
private:
+ void Simulate(ClawType type, StateFeedbackPlant<2, 1, 1>* plant,
+ const constants::Values::Claw &claw, double nl_voltage) {
+ plant->U << last_voltage_[type];
+ plant->Update();
+
+ // check top claw inside limits
+ EXPECT_GE(claw.upper_limit, plant->Y(0, 0));
+ EXPECT_LE(claw.lower_limit, plant->Y(0, 0));
+
+ // TODO(austin): Check that the claws aren't too close to eachother.
+ last_voltage_[type] = nl_voltage;
+ }
+
ClawGroup claw_queue_group;
double initial_position_[CLAW_COUNT];
- double last_position_[CLAW_COUNT];
- double calibration_value_[CLAW_COUNT];
double last_voltage_[CLAW_COUNT];
+
+ control_loops::ClawGroup::Position last_position_;
};
@@ -189,13 +340,13 @@
double min_seperation_;
ClawTest()
- : claw_queue_group(".frc971.control_loops.wrist", 0x1a7b7094,
- ".frc971.control_loops.wrist.goal",
- ".frc971.control_loops.wrist.position",
- ".frc971.control_loops.wrist.output",
- ".frc971.control_loops.wrist.status"),
+ : claw_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"),
claw_motor_(&claw_queue_group),
- claw_motor_plant_(0.5, 1.0),
+ claw_motor_plant_(0.2, 0.4),
min_seperation_(constants::GetValues().claw_min_seperation) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
@@ -235,7 +386,6 @@
for (int i = 0; i < 400; ++i) {
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
- bottom_claw_motor_.Iterate();
claw_motor_plant_.Simulate();
SendDSPacket(true);
}
@@ -244,8 +394,7 @@
// Tests that the wrist zeros correctly starting on the hall effect sensor.
TEST_F(ClawTest, ZerosStartingOn) {
- claw_motor_plant_.Reinitialize(TOP_CLAW, 90 * M_PI / 180.0);
- claw_motor_plant_.Reinitialize(BOTTOM_CLAW, 100 * M_PI / 180.0);
+ claw_motor_plant_.Reinitialize(90 * M_PI / 180.0, 100 * M_PI / 180.0);
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.1)
@@ -254,7 +403,6 @@
for (int i = 0; i < 500; ++i) {
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
- bottom_claw_motor_.Iterate();
claw_motor_plant_.Simulate();
SendDSPacket(true);
@@ -262,6 +410,7 @@
VerifyNearGoal();
}
+/*
// Tests that missing positions are correctly handled.
TEST_F(ClawTest, HandleMissingPosition) {
claw_queue_group.goal.MakeWithBuilder()
@@ -273,7 +422,6 @@
claw_motor_plant_.SendPositionMessage();
}
claw_motor_.Iterate();
- bottom_claw_motor_.Iterate();
claw_motor_plant_.Simulate();
SendDSPacket(true);
@@ -297,7 +445,6 @@
if (i > 310) {
// Should be re-zeroing now.
EXPECT_TRUE(claw_motor_.is_uninitialized());
- EXPECT_TRUE(bottom_claw_motor_.is_uninitialized());
}
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.2)
@@ -333,8 +480,7 @@
if (i > 100) {
// Give the loop a couple cycled to get the message and then verify that
// it is in the correct state.
- EXPECT_TRUE(top_claw_motor_.is_uninitialized());
- EXPECT_TRUE(bottom_claw_motor_.is_uninitialized());
+ EXPECT_TRUE(claw_motor_.is_uninitialized());
// When disabled, we should be applying 0 power.
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
EXPECT_EQ(0, claw_queue_group.output->top_claw_voltage);
@@ -368,8 +514,7 @@
for (int i = 0; i < 500; ++i) {
claw_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_TRUE(top_claw_motor_.is_zeroing());
- EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
+ EXPECT_TRUE(claw_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
saved_zeroing_position[TOP_CLAW] =
top_claw_motor_.zeroed_joint_.zeroing_position_;
@@ -378,15 +523,15 @@
bottom_claw_motor_.zeroed_joint_.zeroing_position_;
bottom_claw_motor_.zeroed_joint_.zeroing_position_ = -100.0;
} else if (i == 51) {
- EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_TRUE(claw_motor_.is_zeroing());
+
EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
top_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
- EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
bottom_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
- if (!top_claw_motor_.is_ready()) {
- if (top_claw_motor_.capped_goal()) {
+ if (!claw_motor_.top().is_ready()) {
+ if (claw_motor_.top().capped_goal()) {
++capped_count[TOP_CLAW];
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
@@ -395,8 +540,8 @@
ASSERT_GT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
}
}
- if (!bottom_claw_motor_.is_ready()) {
- if (bottom_claw_motor_.capped_goal()) {
+ if (!claw_motor_.bottom().is_ready()) {
+ if (claw_motor_.bottom().capped_goal()) {
++capped_count[BOTTOM_CLAW];
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
@@ -406,8 +551,7 @@
}
}
- top_claw_motor_.Iterate();
- bottom_claw_motor_.Iterate();
+ claw_motor_.Iterate();
claw_motor_plant_.Simulate();
SendDSPacket(true);
}
@@ -439,12 +583,12 @@
top_claw_motor_.zeroed_joint_.zeroing_position_ = 100.0;
} else {
if (i == 51) {
- EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_TRUE(claw_motor_.top().is_zeroing());
+ EXPECT_TRUE(claw_motor_.bottom().is_zeroing());
EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
- top_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
- EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
+ claw_motor_.top().zeroing_position_, 0.4);
EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
- bottom_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ claw_motor_.bottom().zeroing_position_, 0.4);
}
}
if (!top_claw_motor_.is_ready()) {
@@ -452,9 +596,9 @@
++capped_count[TOP_CLAW];
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_LT(5, ::std::abs(claw_motor_.top().zeroed_joint_.U_uncapped()));
} else {
- ASSERT_GT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_GT(5, ::std::abs(claw_motor_.top().zeroed_joint_.U_uncapped()));
}
}
if (!bottom_claw_motor_.is_ready()) {
@@ -462,14 +606,13 @@
++capped_count[BOTTOM_CLAW];
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_LT(5, ::std::abs(claw_motor_.bottom().zeroed_joint_.U_uncapped()));
} else {
- ASSERT_GT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_GT(5, ::std::abs(claw_motor_.bottom().zeroed_joint_.U_uncapped()));
}
}
- top_claw_motor_.Iterate();
- bottom_claw_motor_.Iterate();
+ claw_motor_.Iterate();
claw_motor_plant_.Simulate();
SendDSPacket(true);
}
@@ -477,6 +620,7 @@
EXPECT_GT(3, capped_count[TOP_CLAW]);
EXPECT_GT(3, capped_count[BOTTOM_CLAW]);
}
+*/
} // namespace testing
} // namespace control_loops