merging in changes saved for after SVR
diff --git a/aos/linux_code/logging/linux_logging.cc b/aos/linux_code/logging/linux_logging.cc
index aed8697..65e6ca2 100644
--- a/aos/linux_code/logging/linux_logging.cc
+++ b/aos/linux_code/logging/linux_logging.cc
@@ -110,7 +110,6 @@
dropped_messages, dropped_start_seconds, dropped_start_nseconds);
if (queue->WriteMessage(dropped_message, RawQueue::kNonBlock)) {
dropped_messages = 0;
- internal::PrintMessage(stderr, *dropped_message);
} else {
// Don't even bother trying to write this message because it's not likely
// to work and it would be confusing to have one log in the middle of a
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 9db46fd..7052338 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -40,6 +40,9 @@
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
+// Smaller (more negative) = opening.
+const double kCompTopClawOffset = -0.120;
+
const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
case 1: // for tests
@@ -84,7 +87,7 @@
kCompHighGearRatio,
kCompLeftDriveShifter,
kCompRightDriveShifter,
- true,
+ false,
control_loops::MakeVelocityDrivetrainLoop,
control_loops::MakeDrivetrainLoop,
// ShooterLimits
@@ -102,10 +105,11 @@
1.822142,
-0.849484,
1.42309,
- {-3.328397, 2.091668, -3.166136, 1.95,
- {-3.4, -3.092051, -3.4, -3.093414},
- {-0.249073, -0.035452, -0.251800, -0.033179},
- {1.889410, 2.2, 1.883956, 2.2}},
+ // 0.0371
+ {-3.3284, 2.0917, -3.1661, 1.95,
+ {-3.4, -2.9368 + kCompTopClawOffset, -3.4, -2.9876 + kCompTopClawOffset},
+ {-0.1433 + kCompTopClawOffset, 0.0670 + kCompTopClawOffset, -0.1460 + kCompTopClawOffset, 0.0648 + kCompTopClawOffset},
+ {1.9952 + kCompTopClawOffset, 2.2, 1.9898 + kCompTopClawOffset, 2.2}},
{-2.453460, 3.082960, -2.453460, 3.082960,
{-2.6, -2.185752, -2.6, -2.184843},
{-0.280434, -0.049087, -0.277707, -0.047724},
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index dc7e112..06d95fe 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -191,21 +191,25 @@
{
const auto values = constants::GetValues().claw;
- 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");
- 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");
- U(1, 0) = 0;
+ 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");
+ 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");
+ U(1, 0) = 0;
+ }
}
- if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
- LOG(WARNING, "lower claw too high and moving up\n");
- 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");
- U(0, 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");
+ 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");
+ U(0, 0) = 0;
+ }
}
}
}
@@ -806,6 +810,7 @@
LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
}
+ claw_.set_positions_known(top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION, bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 89f36f6..ac63b36 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -29,6 +29,10 @@
virtual void CapU();
void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
+ void set_positions_known(bool top_known, bool bottom_known) {
+ top_known_ = top_known;
+ bottom_known_ = bottom_known;
+ }
void ChangeTopOffset(double doffset);
void ChangeBottomOffset(double doffset);
@@ -39,6 +43,8 @@
double uncapped_average_voltage_;
bool is_zeroing_;
+ bool top_known_ = false, bottom_known_ = false;
+
const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
};
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index ce8aa71..6c1935b 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -10,6 +10,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/queues/output_check.q.h"
namespace frc971 {
namespace control_loops {
@@ -194,6 +195,18 @@
const bool disabled =
!::aos::robot_state.get() || !::aos::robot_state->enabled;
+ output_check_received.FetchLatest();
+ // True if we're enabled but the motors aren't working.
+ // TODO(brians): Make this more general.
+ // The 100ms is the result of disabling the robot while it's putting out a lot
+ // of power and looking at the time delay between the last PWM pulse and the
+ // battery voltage coming back up.
+ const bool motors_off =
+ !disabled && (!output_check_received.get() ||
+ !output_check_received.IsNewerThanMS(100));
+ motors_off_log_.Print();
+ if (motors_off) LOG_INTERVAL(motors_off_log_);
+
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -318,6 +331,9 @@
// useful sensor data.
latch_piston_ = true;
}
+ if (motors_off) {
+ load_timeout_ += ::aos::control_loops::kLoopFrequency;
+ }
// Go to 0, which should be the latch position, or trigger a hall effect
// on the way. If we don't see edges where we are supposed to, the
// offset will be updated by code above.
@@ -625,7 +641,9 @@
if (cap_goal) {
shooter_.CapGoal();
}
- if (output) output->voltage = shooter_.voltage();
+ // We don't really want to output anything if we went through everything
+ // assuming the motors weren't working.
+ if (output && !motors_off) output->voltage = shooter_.voltage();
} else {
shooter_.Update(true);
shooter_.ZeroPower();
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index 10e0f4e..23380df 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -31,6 +31,8 @@
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(DEPTH)/frc971/queues/queues.gyp:output_check',
],
'export_dependent_settings': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index a909d40..e2d4b93 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -6,6 +6,7 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
#include "frc971/constants.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -207,6 +208,10 @@
bool last_distal_current_;
bool last_proximal_current_;
+ ::aos::util::SimpleLogInterval motors_off_log_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.05),
+ WARNING, "motors disabled");
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index e1e5139..4c3d1bb 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -95,7 +95,7 @@
// 34" between near edge of colored line and rear edge of bumper
const ShotGoal kLongShotGoal = {
- {-1.04, kShootSeparation}, 140, 0.04, kIntakePower};
+ {-1.06, kShootSeparation}, 140, 0.04, kIntakePower};
// 3/4" plunger {-1.04, kShootSeparation}, 140, 0.04, kIntakePower};
const ShotGoal kFlippedLongShotGoal = {
{0.96, kShootSeparation}, 140, 0.09, kIntakePower};
@@ -110,14 +110,20 @@
// 3/4" plunger {0.80, kShootSeparation}, 105, 0.2, kIntakePower};
const ShotGoal kShortShotGoal = {
- {-0.670, kShootSeparation}, 77.0, 0.4, kIntakePower};
-const ShotGoal kFlippedShortShotGoal = {
{0.67, kShootSeparation}, 115.0, 0.4, kIntakePower};
+const ShotGoal kFlippedShortShotGoal = kShortShotGoal;
const ShotGoal kHumanShotGoal = {
{-0.90, kShootSeparation}, 140, 0.04, kIntakePower};
+const ShotGoal kFlippedHumanShotGoal = {
+ {0.90, kShootSeparation}, 140, 0, kIntakePower};
const ShotGoal kTrussShotGoal = {
- {-0.05, kShootSeparation}, 73.0, 0, kIntakePower};
+ {-0.68, kShootSeparation}, 83.0, 0.4, kIntakePower};
+const ShotGoal kFlippedTrussShotGoal = {
+ {0.68, kShootSeparation}, 92.0, 0.4, kIntakePower};
+
+const ClawGoal k254PassGoal = {-1.95, kGrabSeparation};
+const ClawGoal kFlipped254PassGoal = {1.96, kGrabSeparation};
// Makes a new ShootAction action.
::std::unique_ptr<TypedAction< ::frc971::actions::CatchActionGroup>>
@@ -364,11 +370,15 @@
} else if (data.PosEdge(kHumanPlayerShot)) {
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
- SetGoal(kHumanShotGoal);
+ SetGoal(kFlippedHumanShotGoal);
+ } else if (data.PosEdge(kUserLeft)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(kFlipped254PassGoal);
} else if (data.PosEdge(kTrussShot)) {
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
- SetGoal(kTrussShotGoal);
+ SetGoal(kFlippedTrussShotGoal);
}
} else {
if (data.IsPressed(kIntakeOpenPosition)) {
@@ -403,6 +413,10 @@
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
SetGoal(kHumanShotGoal);
+ } else if (data.PosEdge(kUserLeft)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ SetGoal(k254PassGoal);
} else if (data.PosEdge(kTrussShot)) {
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");