Added more internal state logging to arm.
Change-Id: Ic4c07c74f740af4e5af32d2d140a76ba78f00ee5
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 2dbf8d7..ccd2388 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,12 +14,12 @@
namespace superstructure {
namespace {
-constexpr double kZeroingVoltage = 6.0;
-constexpr double kOperatingVoltage = 6.0;
-constexpr double kLandingShoulderDownVoltage = -1.5;
+constexpr double kZeroingVoltage = 5.0;
+constexpr double kOperatingVoltage = 12.0;
+constexpr double kLandingShoulderDownVoltage = -2.0;
// The maximum voltage the intake roller will be allowed to use.
-constexpr float kMaxIntakeTopVoltage = 8.0;
-constexpr float kMaxIntakeBottomVoltage = 8.0;
+constexpr float kMaxIntakeTopVoltage = 12.0;
+constexpr float kMaxIntakeBottomVoltage = 12.0;
// Aliases to reduce typing.
constexpr double kIntakeEncoderIndexDifference =
@@ -559,6 +559,20 @@
}
// Calculate the loops for a cycle.
+ {
+ Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
+ status->intake.position_power = intake_.controller().K(0, 0) * error(0, 0);
+ status->intake.velocity_power = intake_.controller().K(0, 1) * error(1, 0);
+ }
+
+ {
+ Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
+ status->shoulder.position_power = arm_.controller().K(0, 0) * error(0, 0);
+ status->shoulder.velocity_power = arm_.controller().K(0, 1) * error(1, 0);
+ status->wrist.position_power = arm_.controller().K(0, 2) * error(2, 0);
+ status->wrist.velocity_power = arm_.controller().K(0, 3) * error(3, 0);
+ }
+
arm_.Update(disable);
intake_.Update(disable);
@@ -592,6 +606,9 @@
status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
status->shoulder.unprofiled_goal_angular_velocity =
arm_.unprofiled_goal(1, 0);
+ status->shoulder.voltage_error = arm_.X_hat(4, 0);
+ status->shoulder.calculated_velocity =
+ (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
status->wrist.angle = arm_.X_hat(2, 0);
@@ -600,6 +617,9 @@
status->wrist.goal_angular_velocity = arm_.goal(3, 0);
status->wrist.unprofiled_goal_angle = arm_.unprofiled_goal(2, 0);
status->wrist.unprofiled_goal_angular_velocity = arm_.unprofiled_goal(3, 0);
+ status->wrist.voltage_error = arm_.X_hat(5, 0);
+ status->wrist.calculated_velocity =
+ (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
status->wrist.estimator_state = arm_.WristEstimatorState();
status->intake.angle = intake_.X_hat(0, 0);
@@ -609,7 +629,14 @@
status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
status->intake.unprofiled_goal_angular_velocity =
intake_.unprofiled_goal(1, 0);
+ status->intake.calculated_velocity = (intake_.angle() - last_intake_angle_) / 0.005;
+ status->intake.voltage_error = intake_.X_hat(2, 0);
status->intake.estimator_state = intake_.IntakeEstimatorState();
+ status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+
+ last_shoulder_angle_ = arm_.shoulder_angle();
+ last_wrist_angle_ = arm_.wrist_angle();
+ last_intake_angle_ = intake_.angle();
status->estopped = (state_ == ESTOP);
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 05998c0..2967a72 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -214,6 +214,10 @@
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
+ float last_shoulder_angle_ = 0.0;
+ float last_wrist_angle_ = 0.0;
+ float last_intake_angle_ = 0.0;
+
// Returns true if the profile has finished, and the joint is within the
// specified tolerance.
bool IsArmNear(double tolerance);
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 8a3b518..277e3ce 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -17,6 +17,17 @@
// Unprofiled goal angular velocity of the joint in radians/second.
float unprofiled_goal_angular_velocity;
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
// State of the estimator.
.frc971.EstimatorState estimator_state;
};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 3bc41cb..b8c674f 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -57,6 +57,9 @@
// Returns the current position.
double angle() const { return Y_(0, 0); }
+ // Returns the controller error.
+ const StateFeedbackLoop<3, 1, 1> &controller() const { return *loop_; }
+
// Returns the filtered goal.
const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }
@@ -159,6 +162,9 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+ // Returns the controller error.
+ const StateFeedbackLoop<6, 2, 2> &controller() const { return *loop_; }
+
// Returns the unprofiled goal.
const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
return unprofiled_goal_;