Fixed profile offsetting bug in superstructure.
Change-Id: Ica0f78cfdf16fc9779b478fd8abeb0f86ccbc96d
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index d286fa8..2095d6c 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -98,6 +98,9 @@
void Intake::ForceGoal(double goal) {
set_unprofiled_goal(goal);
loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
void Intake::set_unprofiled_goal(double unprofiled_goal) {
@@ -121,9 +124,7 @@
loop_->Update(disable);
if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- ::Eigen::Matrix<double, 2, 1> new_state;
- new_state << loop_->R(0, 0), loop_->R(1, 0);
- profile_.MoveCurrentState(new_state);
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
}
@@ -182,11 +183,14 @@
loop_->mutable_X_hat()(2, 0) += doffset;
Y_(1, 0) += doffset;
loop_->mutable_R(2, 0) += doffset;
+ loop_->mutable_next_R(2, 0) += doffset;
+ unprofiled_goal_(2, 0) += doffset;
wrist_profile_.MoveGoal(doffset);
offset_(1, 0) = offset;
CapGoal("R", &loop_->mutable_R());
+ CapGoal("unprofiled R", &loop_->mutable_next_R());
}
void Arm::UpdateShoulderOffset(double offset) {
@@ -199,11 +203,17 @@
Y_(0, 0) += doffset;
loop_->mutable_R(0, 0) += doffset;
loop_->mutable_R(2, 0) += doffset;
+ loop_->mutable_next_R(0, 0) += doffset;
+ loop_->mutable_next_R(2, 0) += doffset;
+ unprofiled_goal_(0, 0) += doffset;
+ unprofiled_goal_(2, 0) += doffset;
shoulder_profile_.MoveGoal(doffset);
+ wrist_profile_.MoveGoal(doffset);
offset_(0, 0) = offset;
CapGoal("R", &loop_->mutable_R());
+ CapGoal("unprofiled R", &loop_->mutable_next_R());
}
// TODO(austin): Handle zeroing errors.
@@ -269,6 +279,10 @@
void Arm::ForceGoal(double goal_shoulder, double goal_wrist) {
set_unprofiled_goal(goal_shoulder, goal_wrist);
loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
}
void Arm::set_unprofiled_goal(double unprofiled_goal_shoulder,
@@ -337,16 +351,12 @@
// Shoulder saturated
if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- ::Eigen::Matrix<double, 2, 1> new_shoulder_state;
- new_shoulder_state << loop_->R(0, 0), loop_->R(1, 0);
- shoulder_profile_.MoveCurrentState(new_shoulder_state);
+ shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
// Wrist saturated
if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
- ::Eigen::Matrix<double, 2, 1> new_wrist_state;
- new_wrist_state << loop_->R(2, 0), loop_->R(3, 0);
- wrist_profile_.MoveCurrentState(new_wrist_state);
+ wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
}
}
@@ -510,18 +520,26 @@
status->shoulder.angular_velocity = arm_.X_hat(1, 0);
status->shoulder.goal_angle = arm_.goal(0, 0);
status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
+ status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
+ status->shoulder.unprofiled_goal_angular_velocity =
+ arm_.unprofiled_goal(1, 0);
status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
status->wrist.angle = arm_.X_hat(2, 0);
status->wrist.angular_velocity = arm_.X_hat(3, 0);
status->wrist.goal_angle = arm_.goal(2, 0);
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.estimator_state = arm_.WristEstimatorState();
status->intake.angle = intake_.X_hat(0, 0);
status->intake.angular_velocity = intake_.X_hat(1, 0);
status->intake.goal_angle = intake_.goal(0, 0);
status->intake.goal_angular_velocity = intake_.goal(1, 0);
+ status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
+ status->intake.unprofiled_goal_angular_velocity =
+ intake_.unprofiled_goal(1, 0);
status->intake.estimator_state = intake_.IntakeEstimatorState();
status->estopped = (state_ == ESTOP);
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index aa43758..3e5cde4 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -97,6 +97,14 @@
const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, 3, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
// Returns the current state estimate.
const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
@@ -172,6 +180,14 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
// Returns the filtered goal.
const Eigen::Matrix<double, 6, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 96594e1..1ca1ca4 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -5,13 +5,17 @@
struct JointState {
// Angle of the joint in radians.
- double angle;
+ float angle;
// Angular velocity of the joint in radians/second.
float angular_velocity;
// Profiled goal angle of the joint in radians.
- double goal_angle;
+ float goal_angle;
// Profiled goal angular velocity of the joint in radians/second.
- double goal_angular_velocity;
+ float goal_angular_velocity;
+ // Unprofiled goal angle of the joint in radians.
+ float unprofiled_goal_angle;
+ // Unprofiled goal angular velocity of the joint in radians/second.
+ float unprofiled_goal_angular_velocity;
// State of the estimator.
.frc971.EstimatorState estimator_state;
@@ -78,9 +82,9 @@
};
message Output {
- double voltage_intake;
- double voltage_shoulder;
- double voltage_wrist;
+ float voltage_intake;
+ float voltage_shoulder;
+ float voltage_wrist;
};
queue Goal goal;