Prefix LOG and CHECK with AOS_
This prepares us for introducing glog more widely and transitioning
things over where they make sense.
Change-Id: Ic6c208882407bc2153fe875ffc736d66f5c8ade5
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index c3155bf..99a130c 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -175,11 +175,12 @@
shoulder_angle <=
CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference &&
intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
- LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n",
- intake_angle, CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
- CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
- shoulder_angle,
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ AOS_LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n",
+ intake_angle,
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference,
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing,
+ shoulder_angle,
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
return true;
}
@@ -191,7 +192,8 @@
intake_angle > Superstructure::kIntakeLowerClear &&
(wrist_angle > CollisionAvoidance::kMaxWristAngleForMovingByIntake ||
wrist_angle < CollisionAvoidance::kMinWristAngleForMovingByIntake)) {
- LOG(DEBUG,
+ AOS_LOG(
+ DEBUG,
"Collided: Intake %f < %f < %f, shoulder %f < %f < %f, and %f < %f < "
"%f.\n",
Superstructure::kIntakeLowerClear, intake_angle,
@@ -209,10 +211,10 @@
if (shoulder_angle <
CollisionAvoidance::kMinShoulderAngleForHorizontalShooter &&
::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
- LOG(DEBUG, "Collided: Shoulder %f < %f and wrist |%f| > %f.\n",
- shoulder_angle,
- CollisionAvoidance::kMinShoulderAngleForHorizontalShooter, wrist_angle,
- kMaxWristAngleForSafeArmStowing);
+ AOS_LOG(DEBUG, "Collided: Shoulder %f < %f and wrist |%f| > %f.\n",
+ shoulder_angle,
+ CollisionAvoidance::kMinShoulderAngleForHorizontalShooter,
+ wrist_angle, kMaxWristAngleForSafeArmStowing);
return true;
}
@@ -293,7 +295,7 @@
control_loops::SuperstructureQueue::Status *status) {
const State state_before_switch = state_;
if (WasReset()) {
- LOG(ERROR, "WPILib reset, restarting\n");
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
intake_.Reset();
state_ = UNINITIALIZED;
@@ -323,7 +325,7 @@
case UNINITIALIZED:
// Wait in the uninitialized state until both the arm and intake are
// initialized.
- LOG(DEBUG, "Uninitialized, waiting for intake and arm\n");
+ AOS_LOG(DEBUG, "Uninitialized, waiting for intake and arm\n");
if (arm_.initialized() && intake_.initialized()) {
state_ = DISABLED_INITIALIZED;
}
@@ -414,10 +416,10 @@
if (arm_.zeroed() && intake_.zeroed()) {
state_ = RUNNING;
} else if (IsArmNear(kLooseTolerance)) {
- LOG(ERROR,
- "Failed to zero while executing the HIGH_ARM_ZERO sequence. "
- "Arm: %d Intake %d\n",
- arm_.zeroed(), intake_.zeroed());
+ AOS_LOG(ERROR,
+ "Failed to zero while executing the HIGH_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
state_ = ESTOP;
}
}
@@ -515,10 +517,10 @@
if (arm_.zeroed() && intake_.zeroed()) {
state_ = RUNNING;
} else {
- LOG(ERROR,
- "Failed to zero while executing the LOW_ARM_ZERO sequence. "
- "Arm: %d Intake %d\n",
- arm_.zeroed(), intake_.zeroed());
+ AOS_LOG(ERROR,
+ "Failed to zero while executing the LOW_ARM_ZERO sequence. "
+ "Arm: %d Intake %d\n",
+ arm_.zeroed(), intake_.zeroed());
state_ = ESTOP;
}
}
@@ -627,7 +629,7 @@
} break;
case ESTOP:
- LOG(ERROR, "Estop\n");
+ AOS_LOG(ERROR, "Estop\n");
disable = true;
break;
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index e787f79..b8f3a48 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -55,7 +55,8 @@
void Arm::UpdateWristOffset(double offset) {
const double doffset = offset - offset_(1, 0);
- LOG(INFO, "Adjusting Wrist offset from %f to %f\n", offset_(1, 0), offset);
+ AOS_LOG(INFO, "Adjusting Wrist offset from %f to %f\n", offset_(1, 0),
+ offset);
loop_->mutable_X_hat()(2, 0) += doffset;
Y_(1, 0) += doffset;
@@ -72,7 +73,8 @@
void Arm::UpdateShoulderOffset(double offset) {
const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0), offset);
+ AOS_LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0),
+ offset);
loop_->mutable_X_hat()(0, 0) += doffset;
loop_->mutable_X_hat()(2, 0) += doffset;
@@ -101,11 +103,11 @@
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
- LOG(ERROR, "zeroing error with shoulder_estimator\n");
+ AOS_LOG(ERROR, "zeroing error with shoulder_estimator\n");
return;
}
if (estimators_[kWristIndex].error()) {
- LOG(ERROR, "zeroing error with wrist_estimator\n");
+ AOS_LOG(ERROR, "zeroing error with wrist_estimator\n");
return;
}
@@ -138,26 +140,26 @@
// Limit the goals to min/max allowable angles.
if ((*goal)(0, 0) > constants::Values::kShoulderRange.upper) {
- LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- constants::Values::kShoulderRange.upper);
+ AOS_LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name,
+ (*goal)(0, 0), constants::Values::kShoulderRange.upper);
(*goal)(0, 0) = constants::Values::kShoulderRange.upper;
}
if ((*goal)(0, 0) < constants::Values::kShoulderRange.lower) {
- LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- constants::Values::kShoulderRange.lower);
+ AOS_LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name,
+ (*goal)(0, 0), constants::Values::kShoulderRange.lower);
(*goal)(0, 0) = constants::Values::kShoulderRange.lower;
}
const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
if (wrist_goal_angle_ungrounded > constants::Values::kWristRange.upper) {
- LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
- wrist_goal_angle_ungrounded, constants::Values::kWristRange.upper);
+ AOS_LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.upper);
(*goal)(2, 0) = constants::Values::kWristRange.upper + (*goal)(0, 0);
}
if (wrist_goal_angle_ungrounded < constants::Values::kWristRange.lower) {
- LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
- wrist_goal_angle_ungrounded, constants::Values::kWristRange.lower);
+ AOS_LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.lower);
(*goal)(2, 0) = constants::Values::kWristRange.lower + (*goal)(0, 0);
}
}
@@ -195,9 +197,9 @@
bool Arm::CheckHardLimits() {
if (shoulder_angle() > constants::Values::kShoulderRange.upper_hard ||
shoulder_angle() < constants::Values::kShoulderRange.lower_hard) {
- LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
- shoulder_angle(), constants::Values::kShoulderRange.lower_hard,
- constants::Values::kShoulderRange.upper_hard);
+ AOS_LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
+ shoulder_angle(), constants::Values::kShoulderRange.lower_hard,
+ constants::Values::kShoulderRange.upper_hard);
return true;
}
@@ -205,10 +207,10 @@
constants::Values::kWristRange.upper_hard ||
wrist_angle() - shoulder_angle() <
constants::Values::kWristRange.lower_hard) {
- LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
- wrist_angle() - shoulder_angle(),
- constants::Values::kWristRange.lower_hard,
- constants::Values::kWristRange.upper_hard);
+ AOS_LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
+ wrist_angle() - shoulder_angle(),
+ constants::Values::kWristRange.lower_hard,
+ constants::Values::kWristRange.upper_hard);
return true;
}
@@ -239,15 +241,15 @@
// Shoulder saturated
if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- LOG(DEBUG, "Moving shoulder state. U: %f, %f\n", loop_->U(0, 0),
- loop_->U_uncapped(0, 0));
+ AOS_LOG(DEBUG, "Moving shoulder state. U: %f, %f\n", loop_->U(0, 0),
+ loop_->U_uncapped(0, 0));
shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
// Wrist saturated
if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
- LOG(DEBUG, "Moving shooter state. U: %f, %f\n", loop_->U(1, 0),
- loop_->U_uncapped(1, 0));
+ AOS_LOG(DEBUG, "Moving shooter state. U: %f, %f\n", loop_->U(1, 0),
+ loop_->U_uncapped(1, 0));
wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
}
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 49c8097..8936650 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -41,9 +41,9 @@
const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
bool use_accelerating_controller = true;
- LOG(DEBUG, "Accelerating at %f, decel %f, bemf %f\n",
- accelerating_controller(0, 0), accelerating_controller(1, 0),
- bemf_voltage);
+ AOS_LOG(DEBUG, "Accelerating at %f, decel %f, bemf %f\n",
+ accelerating_controller(0, 0), accelerating_controller(1, 0),
+ bemf_voltage);
if (IsAccelerating(bemf_voltage, accelerating_controller(0, 0))) {
use_accelerating_controller = true;
} else {
@@ -71,7 +71,7 @@
const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
plant().B().block<2, 1>(2, 0))(0, 0) *
overage_amount;
- LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
+ AOS_LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
if (U(0, 0) < min_voltage(0)) {
@@ -80,7 +80,7 @@
const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
plant().B().block<2, 1>(2, 0))(0, 0) *
under_amount;
- LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
+ AOS_LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 969102f..79b4d24 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -194,24 +194,24 @@
if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
superstructure_status_fetcher_->state ==
Superstructure::LANDING_RUNNING) {
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
- Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
- Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ Superstructure::kOperatingVoltage);
} else {
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
- Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
- Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
- Superstructure::kLandingShoulderDownVoltage - 0.00001);
+ AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
+ Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
// Use the plant to generate the next physical state given the voltages to