commit 3b0f3641dd699d3d8d3e85d67a6a44447cd35952
author Austin Schuh <austin.linux@gmail.com> Sun Feb 14 21:04:26 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Feb 14 21:04:26 2016 -0800
tree 20bc30a2ca8b6fc72a1989cf2c7cf188a7c30d36
parent 315cf39df5295338f5d798dadf8bfaa372fde276

Quietened up output when asking for the soft limit.

Change-Id: I81656f91b88a07ccfb3dcab38ea3d6c661889059

y2016/control_loops/superstructure/superstructure_controls.cc

diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index a8e3a36..a8953de 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -73,12 +73,12 @@
 
 void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
   // Limit the goal to min/max allowable angles.
-  if ((*goal)(0, 0) >= constants::Values::kIntakeRange.upper) {
+  if ((*goal)(0, 0) > constants::Values::kIntakeRange.upper) {
     LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
         constants::Values::kIntakeRange.upper);
     (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
   }
-  if ((*goal)(0, 0) <= constants::Values::kIntakeRange.lower) {
+  if ((*goal)(0, 0) < constants::Values::kIntakeRange.lower) {
     LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
         constants::Values::kIntakeRange.lower);
     (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
@@ -121,8 +121,8 @@
 bool Intake::CheckHardLimits() {
   // Returns whether hard limits have been exceeded.
 
-  if (angle() >= constants::Values::kIntakeRange.upper_hard ||
-      angle() <= constants::Values::kIntakeRange.lower_hard) {
+  if (angle() > constants::Values::kIntakeRange.upper_hard ||
+      angle() < constants::Values::kIntakeRange.lower_hard) {
     LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
         constants::Values::kIntakeRange.lower_hard, constants::Values::kIntakeRange.upper_hard);
     return true;
@@ -241,12 +241,12 @@
 void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
   // Limit the goals to min/max allowable angles.
 
-  if ((*goal)(0, 0) >= constants::Values::kShoulderRange.upper) {
+  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);
     (*goal)(0, 0) = constants::Values::kShoulderRange.upper;
   }
-  if ((*goal)(0, 0) <= constants::Values::kShoulderRange.lower) {
+  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);
     (*goal)(0, 0) = constants::Values::kShoulderRange.lower;
@@ -254,12 +254,12 @@
 
   const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
 
-  if (wrist_goal_angle_ungrounded >= constants::Values::kWristRange.upper) {
+  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);
     (*goal)(2, 0) = constants::Values::kWristRange.upper + (*goal)(0, 0);
   }
-  if (wrist_goal_angle_ungrounded <= constants::Values::kWristRange.lower) {
+  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);
     (*goal)(2, 0) = constants::Values::kWristRange.lower + (*goal)(0, 0);
@@ -297,16 +297,16 @@
 }
 
 bool Arm::CheckHardLimits() {
-  if (shoulder_angle() >= constants::Values::kShoulderRange.upper_hard ||
-      shoulder_angle() <= constants::Values::kShoulderRange.lower_hard) {
+  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);
     return true;
   }
 
-  if (wrist_angle() - shoulder_angle() >= constants::Values::kWristRange.upper_hard ||
-      wrist_angle() - shoulder_angle() <= constants::Values::kWristRange.lower_hard) {
+  if (wrist_angle() - shoulder_angle() > 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);