Merge "Fix the name of a variable"
diff --git a/WORKSPACE b/WORKSPACE
index 01187a5..4db4ba8 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -21,8 +21,8 @@
new_http_archive(
name = 'arm_frc_linux_gnueabi_repo',
build_file = 'tools/cpp/arm-frc-linux-gnueabi/arm-frc-linux-gnueabi.BUILD',
- sha256 = '9e93b0712e90d11895444f720f0c90c649fb9becb4ca28bb50749d9074eb1306',
- url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2016.tar.gz',
+ sha256 = '9e2e58f0a668c22e46486a76df8b9da08f526cd8bf4e579f19b461f70a358bf8',
+ url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2017.tar.xz',
)
# Recompressed version of the one downloaded from Linaro at
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index 3ecb653..1e9b936 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -4,8 +4,8 @@
// A bitmask of the button state.
uint16_t buttons;
- // The 4 joystick axes.
- double[4] axis;
+ // The 6 joystick axes.
+ double[6] axis;
// The POV axis.
int32_t pov;
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..7dbd1a8 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,9 +10,12 @@
#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"
+#include "frc971/constants.h"
namespace frc971 {
namespace control_loops {
@@ -142,28 +146,34 @@
// 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.
void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+ const ::frc971::constants::Range &range() const { return range_; }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -182,6 +192,8 @@
const double default_velocity_;
const double default_acceleration_;
+
+ double last_position_ = 0;
};
namespace internal {
@@ -213,6 +225,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 +235,40 @@
}
template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
+ ProfiledJointStatus *status) {
+ status->zeroed = zeroed();
+ status->state = -1;
+ // 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 +284,8 @@
set_zeroed(0, true);
}
- Y_ << position.encoder;
+ last_position_ = position();
+ Y_ << new_position.encoder;
Y_ += offset_;
loop_->Correct(Y_);
}
@@ -251,14 +293,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 +347,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 +359,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/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
new file mode 100644
index 0000000..11d641d
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -0,0 +1,41 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/control_loops.q";
+
+struct ProfiledJointStatus {
+ // Is the turret zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // 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;
+};
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index b4866e9..a490e67 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -47,7 +47,7 @@
for (int i = 0; i < 4; ++i) {
new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 4; ++j) {
+ for (int j = 0; j < 6; ++j) {
new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
}
new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
diff --git a/y2012/control_loops/python/polydrivetrain.py b/y2012/control_loops/python/polydrivetrain.py
index c19c888..944c09b 100755
--- a/y2012/control_loops/python/polydrivetrain.py
+++ b/y2012/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
- self.R = self._drivetrain.R
+ self.resistance = self._drivetrain.R
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
[[0.0],
[0.0]])
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
@@ -239,9 +243,9 @@
self.CurrentDrivetrain().r)
#print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
@@ -407,27 +411,26 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2012', 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high],
- namespaces=namespaces)
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2012', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
- dog_loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
- cim_writer.Write(argv[3], argv[4])
- return
+ cim_writer.Write(argv[3], argv[4])
+ return
vl_plot = []
vr_plot = []
@@ -475,22 +478,6 @@
else:
radius_plot.append(turn_velocity / fwd_velocity)
- cim_velocity_plot = []
- cim_voltage_plot = []
- cim_time = []
- cim = CIM()
- R = numpy.matrix([[300]])
- for t in numpy.arange(0, 0.5, cim.dt):
- U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
- cim.Update(U)
- cim_velocity_plot.append(cim.X[0, 0])
- cim_voltage_plot.append(U[0, 0] * 10)
- cim_time.append(t)
- pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- pylab.legend()
- pylab.show()
-
# TODO(austin):
# Shifting compensation.
@@ -509,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 387fd78..83fff45 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
- self.R = self._drivetrain.R
+ self.resistance = self._drivetrain.R
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
[[0.0],
[0.0]])
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
@@ -239,9 +243,9 @@
self.CurrentDrivetrain().r)
#print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
@@ -407,27 +411,26 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2014', 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high],
- namespaces=namespaces)
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2014', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
- dog_loop_writer.Write(argv[1], argv[2])
+ dog_loop_writer.Write(argv[1], argv[2])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
- cim_writer.Write(argv[3], argv[4])
- return
+ cim_writer.Write(argv[3], argv[4])
+ return
vl_plot = []
vr_plot = []
@@ -474,7 +477,6 @@
radius_plot.append(turn_velocity)
else:
radius_plot.append(turn_velocity / fwd_velocity)
-
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
@@ -509,4 +511,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 1d426d9..9bffa2f 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 9a5b48b..29a55a6 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -515,4 +513,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index e85d131..1d2a74e 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -482,22 +480,6 @@
else:
radius_plot.append(turn_velocity / fwd_velocity)
- cim_velocity_plot = []
- cim_voltage_plot = []
- cim_time = []
- cim = CIM()
- R = numpy.matrix([[300]])
- for t in numpy.arange(0, 0.5, cim.dt):
- U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
- cim.Update(U)
- cim_velocity_plot.append(cim.X[0, 0])
- cim_voltage_plot.append(U[0, 0] * 10)
- cim_time.append(t)
- pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- pylab.legend()
- pylab.show()
-
# TODO(austin):
# Shifting compensation.
@@ -516,4 +498,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 62a4895..c131326 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
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/y2016_bot3/control_loops/python/polydrivetrain.py b/y2016_bot3/control_loops/python/polydrivetrain.py
index aed8ebe..ed799f4 100755
--- a/y2016_bot3/control_loops/python/polydrivetrain.py
+++ b/y2016_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016_bot4/control_loops/python/polydrivetrain.py b/y2016_bot4/control_loops/python/polydrivetrain.py
index 6c70946..e0f94d0 100755
--- a/y2016_bot4/control_loops/python/polydrivetrain.py
+++ b/y2016_bot4/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 5be85cd..376a22a 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -42,8 +42,10 @@
self.G2 = self.G1 * (14.0 / 36.0)
# Third axle gear ratio off the motor
self.G3 = self.G2 * (14.0 / 36.0)
+ # The last gear reduction (encoder -> hood angle)
+ self.last_G = (18.0 / 345.0)
# Gear ratio
- self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * (18.0 / 345.0)
+ self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
# 36 tooth gear inertia in kg * m^2
self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025) ** 2)
@@ -319,6 +321,8 @@
integral_hood = IntegralHood('IntegralHood')
integral_loop_writer = control_loop.ControlLoopWriter('IntegralHood', [integral_hood],
namespaces=namespaces)
+ integral_loop_writer.AddConstant(control_loop.Constant('kLastReduction', '%f',
+ integral_hood.last_G))
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
index 29cb209..df1f18f 100755
--- a/y2017/control_loops/python/polydrivetrain.py
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
def main(argv):
- argv = FLAGS(argv)
-
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
@@ -498,4 +496,6 @@
return 0
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
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 5fe207e..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,
@@ -43,7 +17,7 @@
.frc971.ProfileParameters profile_params;
// Voltage to send to the rollers. Positive is sucking in.
- float voltage_rollers;
+ double voltage_rollers;
};
struct SerializerGoal {
@@ -65,7 +39,7 @@
struct HoodGoal {
// Angle the hood is currently at
- double angle_hood;
+ double angle;
// Caps on velocity/acceleration for profiling. 0 for the default.
.frc971.ProfileParameters profile_params;
@@ -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,27 +99,31 @@
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;
};
message Position {
+ // TODO(austin): The turret and intake really should be absolute. Switch
+ // them over when that class is ready.
+
// Position of the intake, zero when the intake is in, positive when it is
// out.
- .frc971.PotAndAbsolutePosition intake;
+ .frc971.PotAndIndexPosition intake;
// Serializer angle in radians.
double theta_serializer;
// The sensor readings for the turret. The units and sign are defined the
// same as what's in the Goal message.
- .frc971.PotAndAbsolutePosition turret;
+ .frc971.PotAndIndexPosition turret;
- // Position of the hood in radians
- double theta_hood;
+ // The sensor readings for the hood. The units and sign are defined the
+ // same as what's in the Goal message.
+ .frc971.PotAndIndexPosition hood;
// Shooter wheel angle in radians.
double theta_shooter;
@@ -186,17 +131,17 @@
message Output {
// Voltages for some of the subsystems.
- float voltage_intake;
- float voltage_serializer;
- float voltage_shooter;
+ double voltage_intake;
+ double voltage_serializer;
+ double voltage_shooter;
// Rollers on the intake.
- float voltage_intake_rollers;
+ double voltage_intake_rollers;
// Roller on the serializer
- float voltage_serializer_rollers;
+ double voltage_serializer_rollers;
- float voltage_turret;
- float voltage_hood;
+ double voltage_turret;
+ double voltage_hood;
};
queue Goal goal;