Shooter now unloads propperly. Need to figure out why it is overshooting when the python isn't.
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 7e99bfc..efb4480 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -59,6 +59,7 @@
double offset() const { return offset_; }
double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1, 0); }
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
@@ -74,11 +75,11 @@
}
void SetGoalPosition(double desired_position, double desired_velocity) {
- LOG(DEBUG, "Goal position: %.2f Goal velocity: %.2f\n", desired_position, desired_velocity);
+ LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position, desired_velocity);
R << desired_position - kPositionOffset, desired_velocity,
- -A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity;
+ (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+ A(1, 1) / A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }