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/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 7a28a23..14dbf11 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -94,7 +94,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
- LOG_MATRIX(DEBUG, "U at start", U());
+ AOS_LOG_MATRIX(DEBUG, "U at start", U());
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -116,7 +116,7 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
- LOG_MATRIX(DEBUG, "error", error);
+ AOS_LOG_MATRIX(DEBUG, "error", error);
const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
@@ -183,28 +183,28 @@
}
}
- LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ AOS_LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
mutable_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U());
+ AOS_LOG_MATRIX(DEBUG, "U is now", U());
{
const auto values = constants::GetValues().claw;
if (top_known_) {
if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit && U(1, 0) > 0) {
- LOG(WARNING, "upper claw too high and moving up\n");
+ AOS_LOG(WARNING, "upper claw too high and moving up\n");
mutable_U(1, 0) = 0;
} else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
U(1, 0) < 0) {
- LOG(WARNING, "upper claw too low and moving down\n");
+ AOS_LOG(WARNING, "upper claw too low and moving down\n");
mutable_U(1, 0) = 0;
}
}
if (bottom_known_) {
if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
- LOG(WARNING, "lower claw too high and moving up\n");
+ AOS_LOG(WARNING, "lower claw too high and moving up\n");
mutable_U(0, 0) = 0;
} else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
- LOG(WARNING, "lower claw too low and moving down\n");
+ AOS_LOG(WARNING, "lower claw too low and moving down\n");
mutable_U(0, 0) = 0;
}
}
@@ -318,7 +318,7 @@
double edge_encoder;
double edge_angle;
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
+ AOS_LOG(INFO, "Calibration edge edge should be %f.\n", edge_angle);
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(zeroing_state);
return true;
@@ -333,9 +333,9 @@
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
const double calibration_error =
ComputeCalibrationChange(edge_encoder, edge_angle);
- LOG(INFO, "Top calibration error is %f\n", calibration_error);
+ AOS_LOG(INFO, "Top calibration error is %f\n", calibration_error);
if (::std::abs(calibration_error) > kRezeroThreshold) {
- LOG(WARNING, "rezeroing top\n");
+ AOS_LOG(WARNING, "rezeroing top\n");
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
}
@@ -350,9 +350,9 @@
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
const double calibration_error =
ComputeCalibrationChange(edge_encoder, edge_angle);
- LOG(INFO, "Bottom calibration error is %f\n", calibration_error);
+ AOS_LOG(INFO, "Bottom calibration error is %f\n", calibration_error);
if (::std::abs(calibration_error) > kRezeroThreshold) {
- LOG(WARNING, "rezeroing bottom\n");
+ AOS_LOG(WARNING, "rezeroing bottom\n");
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
}
@@ -365,7 +365,7 @@
double edge_encoder;
double edge_angle;
if (GetPositionOfEdge(claw_values, &edge_encoder, &edge_angle)) {
- LOG(INFO, "Calibration edge.\n");
+ AOS_LOG(INFO, "Calibration edge.\n");
SetCalibration(edge_encoder, edge_angle);
set_zeroing_state(zeroing_state);
return true;
@@ -437,20 +437,22 @@
if (SawFilteredPosedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_off_angle_ == max_hall_effect_off_angle_) {
- LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
+ AOS_LOG(WARNING, "%s: Uncertain which side, rejecting posedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_off_angle_ + max_hall_effect_off_angle_) / 2.0;
if (this_sensor.posedge_value() < average_last_encoder) {
*edge_angle = angles.upper_decreasing_angle;
- LOG(INFO, "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, this_sensor.posedge_value(),
- average_last_encoder);
+ AOS_LOG(INFO,
+ "%s Posedge upper of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle,
+ this_sensor.posedge_value(), average_last_encoder);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, this_sensor.posedge_value(),
- average_last_encoder);
+ AOS_LOG(INFO,
+ "%s Posedge lower of %s -> %f posedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle,
+ this_sensor.posedge_value(), average_last_encoder);
}
*edge_encoder = this_sensor.posedge_value();
found_edge = true;
@@ -459,20 +461,22 @@
if (SawFilteredNegedge(this_sensor, sensorA, sensorB)) {
if (min_hall_effect_on_angle_ == max_hall_effect_on_angle_) {
- LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
+ AOS_LOG(WARNING, "%s: Uncertain which side, rejecting negedge\n", name_);
} else {
const double average_last_encoder =
(min_hall_effect_on_angle_ + max_hall_effect_on_angle_) / 2.0;
if (this_sensor.negedge_value() > average_last_encoder) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, this_sensor.negedge_value(),
- average_last_encoder);
+ AOS_LOG(INFO,
+ "%s Negedge upper of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle,
+ this_sensor.negedge_value(), average_last_encoder);
} else {
*edge_angle = angles.lower_decreasing_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
- name_, hall_effect_name, *edge_angle, this_sensor.negedge_value(),
- average_last_encoder);
+ AOS_LOG(INFO,
+ "%s Negedge lower of %s -> %f negedge: %f avg_encoder: %f\n",
+ name_, hall_effect_name, *edge_angle,
+ this_sensor.negedge_value(), average_last_encoder);
}
*edge_encoder = this_sensor.negedge_value();
found_edge = true;
@@ -546,12 +550,12 @@
void ClawLimitedLoop::ChangeTopOffset(double doffset) {
mutable_X_hat()(1, 0) += doffset;
- LOG(INFO, "Changing top offset by %f\n", doffset);
+ AOS_LOG(INFO, "Changing top offset by %f\n", doffset);
}
void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
mutable_X_hat()(0, 0) += doffset;
mutable_X_hat()(1, 0) -= doffset;
- LOG(INFO, "Changing bottom offset by %f\n", doffset);
+ AOS_LOG(INFO, "Changing bottom offset by %f\n", doffset);
}
void LimitClawGoal(double *bottom_goal, double *top_goal,
@@ -559,45 +563,45 @@
// first update position based on angle limit
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.soft_max_separation) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (separation < values.claw.soft_min_separation) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
// now move both goals in unison
if (*bottom_goal < values.claw.lower_claw.lower_limit) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
*bottom_goal = values.claw.lower_claw.lower_limit;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*bottom_goal > values.claw.lower_claw.upper_limit) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
*bottom_goal = values.claw.lower_claw.upper_limit;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal < values.claw.upper_claw.lower_limit) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
*top_goal = values.claw.upper_claw.lower_limit;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal > values.claw.upper_claw.upper_limit) {
- LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
*top_goal = values.claw.upper_claw.upper_limit;
- LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
+ AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
}
@@ -667,9 +671,9 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- LOG_STRUCT(DEBUG, "absolute position",
- ClawPositionToLog(top_claw_.absolute_position(),
- bottom_claw_.absolute_position()));
+ AOS_LOG_STRUCT(DEBUG, "absolute position",
+ ClawPositionToLog(top_claw_.absolute_position(),
+ bottom_claw_.absolute_position()));
}
bool autonomous, enabled;
@@ -717,7 +721,7 @@
}
if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
// always get the bottom claw to calibrated first
- LOG(DEBUG, "Calibrating the bottom of the claw\n");
+ AOS_LOG(DEBUG, "Calibrating the bottom of the claw\n");
if (!doing_calibration_fine_tune_) {
if (::std::abs(bottom_absolute_position() -
values.claw.start_fine_tune_pos) <
@@ -726,12 +730,12 @@
bottom_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
- LOG(DEBUG, "Ready to fine tune the bottom\n");
+ AOS_LOG(DEBUG, "Ready to fine tune the bottom\n");
mode_ = FINE_TUNE_BOTTOM;
} else {
// send bottom to zeroing start
bottom_claw_goal_ = values.claw.start_fine_tune_pos;
- LOG(DEBUG, "Going to the start position for the bottom\n");
+ AOS_LOG(DEBUG, "Going to the start position for the bottom\n");
mode_ = PREP_FINE_TUNE_BOTTOM;
}
} else {
@@ -746,7 +750,7 @@
doing_calibration_fine_tune_ = false;
bottom_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- LOG(DEBUG, "Found a limit, starting over.\n");
+ AOS_LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_BOTTOM;
}
@@ -760,14 +764,15 @@
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning bottom
doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the bottom correctly!\n");
+ AOS_LOG(DEBUG, "Calibrated the bottom correctly!\n");
} else if (bottom_claw_.calibration().last_value()) {
- LOG(DEBUG, "Aborting bottom fine tune because sensor triggered\n");
+ AOS_LOG(DEBUG,
+ "Aborting bottom fine tune because sensor triggered\n");
doing_calibration_fine_tune_ = false;
bottom_claw_.set_zeroing_state(
ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
} else {
- LOG(DEBUG, "Fine tuning\n");
+ AOS_LOG(DEBUG, "Fine tuning\n");
}
}
// now set the top claw to track
@@ -783,12 +788,12 @@
top_claw_goal_ += values.claw.claw_zeroing_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_speed;
- LOG(DEBUG, "Ready to fine tune the top\n");
+ AOS_LOG(DEBUG, "Ready to fine tune the top\n");
mode_ = FINE_TUNE_TOP;
} else {
// send top to zeroing start
top_claw_goal_ = values.claw.start_fine_tune_pos;
- LOG(DEBUG, "Going to the start position for the top\n");
+ AOS_LOG(DEBUG, "Going to the start position for the top\n");
mode_ = PREP_FINE_TUNE_TOP;
}
} else {
@@ -802,7 +807,7 @@
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
- LOG(DEBUG, "Found a limit, starting over.\n");
+ AOS_LOG(DEBUG, "Found a limit, starting over.\n");
mode_ = PREP_FINE_TUNE_TOP;
}
@@ -816,9 +821,9 @@
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning top
doing_calibration_fine_tune_ = false;
- LOG(DEBUG, "Calibrated the top correctly!\n");
+ AOS_LOG(DEBUG, "Calibrated the top correctly!\n");
} else if (top_claw_.calibration().last_value()) {
- LOG(DEBUG, "Aborting top fine tune because sensor triggered\n");
+ AOS_LOG(DEBUG, "Aborting top fine tune because sensor triggered\n");
doing_calibration_fine_tune_ = false;
top_claw_.set_zeroing_state(
ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
@@ -852,7 +857,7 @@
bottom_claw_goal_ += values.claw.claw_zeroing_off_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
values.claw.claw_zeroing_off_speed;
- LOG(DEBUG, "Bottom is known.\n");
+ AOS_LOG(DEBUG, "Bottom is known.\n");
}
} else {
// We don't know where either claw is. Slowly start moving down to find
@@ -862,7 +867,7 @@
bottom_claw_goal_ -= values.claw.claw_zeroing_off_speed * kDt;
top_claw_velocity_ = bottom_claw_velocity_ =
-values.claw.claw_zeroing_off_speed;
- LOG(DEBUG, "Both are unknown.\n");
+ AOS_LOG(DEBUG, "Both are unknown.\n");
}
}
@@ -902,7 +907,7 @@
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.mutable_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
- LOG_MATRIX(DEBUG, "actual goal", claw_.R());
+ AOS_LOG_MATRIX(DEBUG, "actual goal", claw_.R());
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -937,12 +942,13 @@
claw_.R(3, 0);
claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup."
- " Uncapped is %f, max is %f, difference is %f\n",
- dx,
- claw_.uncapped_average_voltage(), values.claw.max_zeroing_voltage,
- (claw_.uncapped_average_voltage() -
- values.claw.max_zeroing_voltage));
+ AOS_LOG(DEBUG,
+ "Moving the goal by %f to prevent windup."
+ " Uncapped is %f, max is %f, difference is %f\n",
+ dx, claw_.uncapped_average_voltage(),
+ values.claw.max_zeroing_voltage,
+ (claw_.uncapped_average_voltage() -
+ values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
double dx_bot =
@@ -959,7 +965,7 @@
claw_.R(3, 0);
claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
- LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ AOS_LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
} break;
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index c88daa8..41dbfca 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -59,8 +59,8 @@
void Reinitialize(double initial_top_position,
double initial_bottom_position) {
- LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
- initial_bottom_position);
+ AOS_LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n",
+ initial_top_position, initial_bottom_position);
claw_plant_->mutable_X(0, 0) = initial_bottom_position;
claw_plant_->mutable_X(1, 0) = initial_top_position - initial_bottom_position;
claw_plant_->mutable_X(2, 0) = 0.0;
@@ -120,8 +120,8 @@
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
- LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n", pos[TOP_CLAW],
- pos[BOTTOM_CLAW]);
+ AOS_LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n",
+ pos[TOP_CLAW], pos[BOTTOM_CLAW]);
const constants::Values& values = constants::GetValues();
@@ -143,12 +143,12 @@
++position->posedge_count;
if (last_angle < pair.lower_angle) {
- LOG(DEBUG, "%s: Positive lower edge on %s hall effect\n", claw_name,
- hall_effect_name);
+ AOS_LOG(DEBUG, "%s: Positive lower edge on %s hall effect\n", claw_name,
+ hall_effect_name);
position->posedge_value = pair.lower_angle - initial_position;
} else {
- LOG(DEBUG, "%s: Positive upper edge on %s hall effect\n", claw_name,
- hall_effect_name);
+ AOS_LOG(DEBUG, "%s: Positive upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
position->posedge_value = pair.upper_angle - initial_position;
}
}
@@ -156,12 +156,12 @@
++position->negedge_count;
if (angle < pair.lower_angle) {
- LOG(DEBUG, "%s: Negative lower edge on %s hall effect\n", claw_name,
- hall_effect_name);
+ AOS_LOG(DEBUG, "%s: Negative lower edge on %s hall effect\n", claw_name,
+ hall_effect_name);
position->negedge_value = pair.lower_angle - initial_position;
} else {
- LOG(DEBUG, "%s: Negative upper edge on %s hall effect\n", claw_name,
- hall_effect_name);
+ AOS_LOG(DEBUG, "%s: Negative upper edge on %s hall effect\n", claw_name,
+ hall_effect_name);
position->negedge_value = pair.upper_angle - initial_position;
}
}
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index d1ef2a5..bf14aad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -35,17 +35,17 @@
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 4.0) {
voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
- LOG(DEBUG, "Capping due to runaway\n");
+ AOS_LOG(DEBUG, "Capping due to runaway\n");
} else if (X_hat(2, 0) < last_voltage_ - 4.0) {
voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
- LOG(DEBUG, "Capping due to runaway\n");
+ AOS_LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- LOG_STRUCT(
+ AOS_LOG_STRUCT(
DEBUG, "shooter_output",
::y2014::control_loops::ShooterVoltageToLog(X_hat(2, 0), voltage_));
@@ -67,8 +67,8 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
+ AOS_LOG_STRUCT(DEBUG, "to prevent windup",
+ ::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (index() == 0) {
@@ -82,8 +82,8 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
+ AOS_LOG_STRUCT(DEBUG, "to prevent windup",
+ ::y2014::control_loops::ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -110,10 +110,10 @@
if (index() == 0) {
mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
- LOG_STRUCT(DEBUG, "sensor edge (fake?)",
- ::y2014::control_loops::ShooterChangeCalibration(
- encoder_val, known_position, old_position, absolute_position(),
- previous_offset, offset_));
+ AOS_LOG_STRUCT(DEBUG, "sensor edge (fake?)",
+ ::y2014::control_loops::ShooterChangeCalibration(
+ encoder_val, known_position, old_position,
+ absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(::aos::EventLoop *event_loop,
@@ -137,20 +137,21 @@
(kMaxExtension - values.shooter.upper_limit) *
(kMaxExtension - values.shooter.upper_limit));
if (power < 0) {
- LOG_STRUCT(WARNING, "negative power",
- ::y2014::control_loops::PowerAdjustment(power, 0));
+ AOS_LOG_STRUCT(WARNING, "negative power",
+ ::y2014::control_loops::PowerAdjustment(power, 0));
power = 0;
} else if (power > maxpower) {
- LOG_STRUCT(WARNING, "power too high",
- ::y2014::control_loops::PowerAdjustment(power, maxpower));
+ AOS_LOG_STRUCT(WARNING, "power too high",
+ ::y2014::control_loops::PowerAdjustment(power, maxpower));
power = maxpower;
}
double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
double new_pos = 0.10;
if (mp < 0) {
- LOG(ERROR,
- "Power calculation has negative number before square root (%f).\n", mp);
+ AOS_LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n",
+ mp);
} else {
new_pos = kMaxExtension - ::std::sqrt(mp);
}
@@ -167,7 +168,7 @@
void ShooterMotor::CheckCalibrations(
const ::y2014::control_loops::ShooterQueue::Position *position) {
- CHECK_NOTNULL(position);
+ AOS_CHECK_NOTNULL(position);
const constants::Values &values = constants::GetValues();
// TODO(austin): Validate that this is the right edge.
@@ -184,7 +185,7 @@
position->pusher_proximal.posedge_value,
values.shooter.pusher_proximal.upper_angle);
- LOG(DEBUG, "Setting calibration using proximal sensor\n");
+ AOS_LOG(DEBUG, "Setting calibration using proximal sensor\n");
zeroed_ = true;
}
} else {
@@ -204,7 +205,7 @@
position->pusher_distal.posedge_value,
values.shooter.pusher_distal.upper_angle);
- LOG(DEBUG, "Setting calibration using distal sensor\n");
+ AOS_LOG(DEBUG, "Setting calibration using distal sensor\n");
zeroed_ = true;
}
} else {
@@ -221,14 +222,14 @@
::y2014::control_loops::ShooterQueue::Status *status) {
const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
if (goal && ::std::isnan(goal->shot_power)) {
- state_ = STATE_ESTOP;
- LOG(ERROR, "Estopping because got a shot power of NAN.\n");
+ state_ = STATE_ESTOP;
+ AOS_LOG(ERROR, "Estopping because got a shot power of NAN.\n");
}
// we must always have these or we have issues.
if (status == NULL) {
if (output) output->voltage = 0;
- LOG(ERROR, "Thought I would just check for null and die.\n");
+ AOS_LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
status->ready = false;
@@ -303,8 +304,8 @@
state_ = STATE_REQUEST_LOAD;
} else {
shooter_loop_disable = true;
- LOG(DEBUG,
- "Not moving on until the latch has moved to avoid a crash\n");
+ AOS_LOG(DEBUG,
+ "Not moving on until the latch has moved to avoid a crash\n");
}
} else {
// If we can't start yet because we don't know where we are, set the
@@ -419,7 +420,7 @@
(load_timeout_ + chrono::milliseconds(500) <
monotonic_now)) {
state_ = STATE_ESTOP;
- LOG(ERROR, "Estopping because took too long to load.\n");
+ AOS_LOG(ERROR, "Estopping because took too long to load.\n");
}
}
}
@@ -446,8 +447,8 @@
// up the latch.
shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
if (position) {
- LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger, position->latch);
+ AOS_LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
+ position->plunger, position->latch);
}
latch_piston_ = true;
@@ -461,10 +462,10 @@
shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
}
- LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
- shooter_.absolute_position(),
- goal ? PowerToPosition(goal->shot_power)
- : ::std::numeric_limits<double>::quiet_NaN());
+ AOS_LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
+ shooter_.absolute_position(),
+ goal ? PowerToPosition(goal->shot_power)
+ : ::std::numeric_limits<double>::quiet_NaN());
if (goal &&
::std::abs(shooter_.absolute_position() -
PowerToPosition(goal->shot_power)) < 0.001 &&
@@ -483,7 +484,7 @@
}
break;
case STATE_READY:
- LOG(DEBUG, "In ready\n");
+ AOS_LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
if (monotonic_now > shooter_brake_set_time_) {
@@ -491,9 +492,9 @@
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
- LOG(DEBUG, "Brake is now set\n");
+ AOS_LOG(DEBUG, "Brake is now set\n");
if (goal && goal->shot_requested && !disabled) {
- LOG(DEBUG, "Shooting now\n");
+ AOS_LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
shot_end_time_ = monotonic_now + kShotEndTimeout;
firing_starting_position_ = shooter_.absolute_position();
@@ -508,7 +509,7 @@
// TODO(austin): Do we want to set the brake here or after shooting?
// Depends on air usage.
status->ready = false;
- LOG(DEBUG, "Preparing shot again.\n");
+ AOS_LOG(DEBUG, "Preparing shot again.\n");
state_ = STATE_PREPARE_SHOT;
}
@@ -596,7 +597,7 @@
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
- LOG(ERROR, "Estopping because took too long to unload.\n");
+ AOS_LOG(ERROR, "Estopping because took too long to unload.\n");
}
brake_piston_ = false;
@@ -642,7 +643,7 @@
break;
case STATE_ESTOP:
- LOG(WARNING, "estopped\n");
+ AOS_LOG(WARNING, "estopped\n");
// Totally lost, go to a safe state.
shooter_loop_disable = true;
latch_piston_ = true;
@@ -651,9 +652,9 @@
}
if (!shooter_loop_disable) {
- LOG_STRUCT(DEBUG, "running the loop",
- ::y2014::control_loops::ShooterStatusToLog(
- shooter_.goal_position(), shooter_.absolute_position()));
+ AOS_LOG_STRUCT(DEBUG, "running the loop",
+ ::y2014::control_loops::ShooterStatusToLog(
+ shooter_.goal_position(), shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -678,12 +679,13 @@
}
if (position) {
- LOG_STRUCT(DEBUG, "internal state",
- ::y2014::control_loops::ShooterStateToLog(
- shooter_.absolute_position(), shooter_.absolute_velocity(),
- state_, position->latch, position->pusher_proximal.current,
- position->pusher_distal.current, position->plunger,
- brake_piston_, latch_piston_));
+ AOS_LOG_STRUCT(
+ DEBUG, "internal state",
+ ::y2014::control_loops::ShooterStateToLog(
+ shooter_.absolute_position(), shooter_.absolute_velocity(), state_,
+ position->latch, position->pusher_proximal.current,
+ position->pusher_distal.current, position->plunger, brake_piston_,
+ latch_piston_));
last_position_ = *position;
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index 01096a0..e755b0f 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -79,8 +79,8 @@
}
void SetGoalPosition(double desired_position, double desired_velocity) {
- LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position,
- desired_velocity);
+ AOS_LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position,
+ desired_velocity);
mutable_R() << desired_position - kPositionOffset, desired_velocity,
(-plant().A(1, 0) / plant().A(1, 2) *
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index b6883ad..f910b6d 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -61,7 +61,7 @@
constexpr static double kPositionOffset = kMaxExtension;
void Reinitialize(double initial_position) {
- LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
+ AOS_LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
initial_position_ = initial_position;
plant->mutable_X(0, 0) = initial_position_ - kPositionOffset;
@@ -93,9 +93,9 @@
::y2014::control_loops::ShooterQueue::Position *position) {
const constants::Values &values = constants::GetValues();
- position->position = GetPosition();
+ position->position = GetPosition();
- LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
+ AOS_LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
@@ -132,12 +132,14 @@
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
if (last_position.position + initial_position_ < limits.lower_angle) {
- LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n",
- sensor->posedge_count);
+ AOS_LOG(DEBUG,
+ "Posedge value on lower edge of sensor, count is now %d\n",
+ sensor->posedge_count);
sensor->posedge_value = limits.lower_angle - initial_position_;
} else {
- LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n",
- sensor->posedge_count);
+ AOS_LOG(DEBUG,
+ "Posedge value on upper edge of sensor, count is now %d\n",
+ sensor->posedge_count);
sensor->posedge_value = limits.upper_angle - initial_position_;
}
}
@@ -229,11 +231,11 @@
U << last_voltage_;
shooter_plant_->Update(U);
}
- LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
+ AOS_LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {
- LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
+ AOS_LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
if (latch_delay_count_ == 1) {
latch_piston_state_ = true;
EXPECT_GE(constants::GetValues().shooter.latch_max_safe_position,
@@ -242,7 +244,7 @@
}
latch_delay_count_--;
} else if (latch_piston_state_ && latch_delay_count_ < 0) {
- LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
+ AOS_LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
if (latch_delay_count_ == -1) {
latch_piston_state_ = false;
if (GetAbsolutePosition() > 0.002) {
@@ -616,7 +618,7 @@
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
- LOG(DEBUG, "State is UnloadMove\n");
+ AOS_LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
shooter_motor_.shooter_.mutable_R(0, 0) -= 100;
@@ -659,7 +661,7 @@
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
- LOG(DEBUG, "State is UnloadMove\n");
+ AOS_LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
shooter_motor_.shooter_.mutable_R(0, 0) += 0.1;