Factored out the profiled subsystem status.
Change-Id: I03df9788bbe98d12e80dd70db8cd09d9b3651724
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8ceeace..8f8b381 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -148,6 +148,16 @@
],
)
+queue_library(
+ name = 'profiled_subsystem_queue',
+ srcs = [
+ 'profiled_subsystem.q',
+ ],
+ deps = [
+ ':queues',
+ ],
+)
+
cc_library(
name = 'profiled_subsystem',
srcs = [
@@ -157,6 +167,7 @@
'profiled_subsystem.h',
],
deps = [
+ ':profiled_subsystem_queue',
':simple_capped_state_feedback_loop',
':state_feedback_loop',
'//aos/common/controls:control_loop',
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 9444b66..379cdc9 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
#include <array>
+#include <chrono>
#include <memory>
#include <utility>
@@ -9,6 +10,8 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/profiled_subsystem.q.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
@@ -142,23 +145,27 @@
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
+ // Fills out the ProfiledJointStatus structure with the current state.
+ void PopulateStatus(ProfiledJointStatus *status);
+
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
// Sets the unprofiled goal. The profiler will generate a profile to go to
// this goal.
void set_unprofiled_goal(double unprofiled_goal);
// Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
+ // Returns the requested voltage.
+ double voltage() const { return loop_->U(0, 0); }
// Returns the current position.
- double angle() const { return Y_(0, 0); }
+ double position() const { return Y_(0, 0); }
// For testing:
// Triggers an estimator error.
@@ -182,6 +189,8 @@
const double default_velocity_;
const double default_acceleration_;
+
+ double last_position_ = 0;
};
namespace internal {
@@ -213,6 +222,7 @@
loop_->mutable_X_hat()(0, 0) += doffset;
Y_(0, 0) += doffset;
+ last_position_ += doffset;
loop_->mutable_R(0, 0) += doffset;
profile_.MoveGoal(doffset);
@@ -222,12 +232,39 @@
}
template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
+ ProfiledJointStatus *status) {
+ status->zeroed = zeroed();
+ // We don't know, so default to the bad case.
+ status->estopped = true;
+
+ status->position = X_hat(0, 0);
+ status->velocity = X_hat(1, 0);
+ status->goal_position = goal(0, 0);
+ status->goal_velocity = goal(1, 0);
+ status->unprofiled_goal_position = unprofiled_goal(0, 0);
+ status->unprofiled_goal_velocity = unprofiled_goal(1, 0);
+ status->voltage_error = X_hat(2, 0);
+ status->calculated_velocity =
+ (position() - last_position_) /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+
+ status->estimator_state = EstimatorState(0);
+
+ Eigen::Matrix<double, 3, 1> error = controller().error();
+ status->position_power = controller().K(0, 0) * error(0, 0);
+ status->velocity_power = controller().K(0, 1) * error(1, 0);
+}
+
+template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
- typename ZeroingEstimator::Position position) {
- estimators_[0].UpdateEstimate(position);
+ typename ZeroingEstimator::Position new_position) {
+ estimators_[0].UpdateEstimate(new_position);
if (estimators_[0].error()) {
- LOG(ERROR, "zeroing error with intake_estimator\n");
+ LOG(ERROR, "zeroing error\n");
return;
}
@@ -243,7 +280,8 @@
set_zeroed(0, true);
}
- Y_ << position.encoder;
+ last_position_ = position();
+ Y_ << new_position.encoder;
Y_ += offset_;
loop_->Correct(Y_);
}
@@ -251,14 +289,14 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- // Limit the goal to min/max allowable angles.
+ // Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
range_.upper);
(*goal)(0, 0) = range_.upper;
}
if ((*goal)(0, 0) < range_.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
range_.lower);
(*goal)(0, 0) = range_.lower;
}
@@ -305,10 +343,10 @@
bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
- if (angle() > range_.upper_hard || angle() < range_.lower_hard) {
+ if (position() > range_.upper_hard || position() < range_.lower_hard) {
LOG(ERROR,
"SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
- angle(), range_.lower_hard, range_.upper_hard);
+ position(), range_.lower_hard, range_.upper_hard);
return true;
}
@@ -317,6 +355,13 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+ const ::frc971::ProfileParameters &profile_parameters) {
+ AdjustProfile(profile_parameters.max_velocity,
+ profile_parameters.max_acceleration);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
double max_angular_velocity, double max_angular_acceleration) {
profile_.set_maximum_velocity(
internal::UseUnlessZero(max_angular_velocity, default_velocity_));
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index cb09b42..3bf0966 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -41,7 +41,7 @@
double shoulder_angle = arm_->shoulder_angle();
double wrist_angle = arm_->wrist_angle();
- double intake_angle = intake_->angle();
+ double intake_angle = intake_->position();
// TODO(phil): This may need tuning to account for bounciness in the limbs or
// some other thing that I haven't thought of. At the very least,
@@ -163,7 +163,7 @@
bool CollisionAvoidance::collided() const {
return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(),
- intake_->angle());
+ intake_->position());
}
bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle,
@@ -345,7 +345,7 @@
// Set the goals to where we are now so when we start back up, we don't
// jump.
- intake_.ForceGoal(intake_.angle());
+ intake_.ForceGoal(intake_.position());
arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
// Set up the profile to be the zeroing profile.
intake_.AdjustProfile(0.5, 10);
@@ -390,7 +390,7 @@
// enough.
if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
intake_.set_unprofiled_goal(
- MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+ MoveButKeepBelow(kIntakeUpperClear, intake_.position(),
kIntakeEncoderIndexDifference * 2.5));
}
@@ -428,7 +428,7 @@
// far enough to zero.
if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
intake_.set_unprofiled_goal(
- MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+ MoveButKeepBelow(kIntakeLowerClear, intake_.position(),
kIntakeEncoderIndexDifference * 2.5));
}
if (IsIntakeNear(kLooseTolerance)) {
@@ -539,7 +539,7 @@
}
// Reset the profile to the current position so it moves well from here.
- intake_.ForceGoal(intake_.angle());
+ intake_.ForceGoal(intake_.position());
arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
} else {
// If we are in slow_running and are no longer collided, let 'er rip.
@@ -687,7 +687,7 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.intake_voltage();
+ output->voltage_intake = intake_.voltage();
output->voltage_shoulder = arm_.shoulder_voltage();
output->voltage_wrist = arm_.wrist_voltage();
@@ -764,7 +764,7 @@
status->intake.unprofiled_goal_angular_velocity =
intake_.unprofiled_goal(1, 0);
status->intake.calculated_velocity =
- (intake_.angle() - last_intake_angle_) / 0.005;
+ (intake_.position() - last_intake_angle_) / 0.005;
status->intake.voltage_error = intake_.X_hat(2, 0);
status->intake.estimator_state = intake_.EstimatorState(0);
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
@@ -773,7 +773,7 @@
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
- last_intake_angle_ = intake_.angle();
+ last_intake_angle_ = intake_.position();
status->estopped = (state_ == ESTOP);
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 61fb001..d0603ba 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -9,6 +9,7 @@
],
deps = [
'//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:profiled_subsystem_queue',
'//frc971/control_loops:queues',
],
)
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 4f36449..4f854b7 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -1,36 +1,10 @@
package y2017.control_loops;
import "aos/common/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_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;
-};
+import "frc971/control_loops/profiled_subsystem.q";
+// TODO(austin): Add this back in when the queue compiler supports diamond
+// inheritance.
+//import "frc971/control_loops/control_loops.q";
struct IntakeGoal {
// Zero on the intake is when the intake is retracted inside the robot,
@@ -76,17 +50,6 @@
double angular_velocity;
};
-struct IntakeStatus {
- // Is it zeroed?
- bool zeroed;
-
- // Estimated position and velocities.
- JointState joint_state;
-
- // If true, we have aborted.
- bool estopped;
-};
-
struct SerializerStatus {
// The current average velocity in radians/second.
double avg_angular_velocity;
@@ -102,28 +65,6 @@
bool estopped;
};
-struct TurretStatus {
- // Is the turret zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // Estimate angles and angular velocities.
- JointState turret;
-};
-
-struct HoodStatus {
- // Is the turret zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // Estimate angles and angular velocities.
- JointState hood;
-};
-
struct ShooterStatus {
// The current average velocity in radians/second.
double avg_angular_velocity;
@@ -158,10 +99,10 @@
bool estopped;
// Each subsystems status.
- IntakeStatus intake;
+ .frc971.control_loops.ProfiledJointStatus intake;
+ .frc971.control_loops.ProfiledJointStatus turret;
+ .frc971.control_loops.ProfiledJointStatus hood;
SerializerStatus serializer;
- TurretStatus turret;
- HoodStatus hood;
ShooterStatus shooter;
};