Merge "fix the dependencies between our action .q files"
diff --git a/aos/common/actions/actions.h b/aos/common/actions/actions.h
index c729b3f..fa89995 100644
--- a/aos/common/actions/actions.h
+++ b/aos/common/actions/actions.h
@@ -11,6 +11,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/queue.h"
+#include "aos/common/logging/queue_logging.h"
namespace aos {
namespace common {
@@ -126,6 +127,9 @@
queue_group_->goal.name());
// Clear out any old status messages from before now.
queue_group_->status.FetchLatest();
+ if (queue_group_->status.get()) {
+ LOG_STRUCT(DEBUG, "have status", *queue_group_->status);
+ }
}
virtual ~TypedAction() {
@@ -225,8 +229,12 @@
if (has_started_) {
queue_group_->status.FetchNext();
CheckInterrupted();
- } else if (queue_group_->status.FetchLatest()) {
- CheckStarted();
+ } else {
+ while (queue_group_->status.FetchNext()) {
+ LOG_STRUCT(DEBUG, "got status", *queue_group_->status);
+ CheckStarted();
+ if (has_started_) CheckInterrupted();
+ }
}
if (interrupted_) return false;
// We've asked it to start but haven't gotten confirmation that it's started
@@ -240,6 +248,7 @@
void TypedAction<T>::DoWaitUntilDone() {
CHECK(sent_started_);
queue_group_->status.FetchNext();
+ LOG_STRUCT(DEBUG, "got status", *queue_group_->status);
CheckInterrupted();
while (true) {
if (DoCheckIteration(true)) {
@@ -254,11 +263,13 @@
if (interrupted_) return true;
CheckStarted();
if (blocking) {
- queue_group_->status.FetchAnother();
+ queue_group_->status.FetchNextBlocking();
+ LOG_STRUCT(DEBUG, "got status", *queue_group_->status);
} else {
if (!queue_group_->status.FetchNext()) {
return false;
}
+ LOG_STRUCT(DEBUG, "got status", *queue_group_->status);
}
CheckStarted();
CheckInterrupted();
@@ -278,7 +289,7 @@
queue_group_->status->last_running == run_value_)) {
// It's currently running our instance.
has_started_ = true;
- LOG(DEBUG, "Action %" PRIx32 " on queue %s has been started\n",
+ LOG(INFO, "Action %" PRIx32 " on queue %s has been started\n",
run_value_, queue_group_->goal.name());
} else if (old_run_value_ != 0 &&
queue_group_->status->running == old_run_value_) {
@@ -320,7 +331,10 @@
// Don't wait to see a message with it.
has_started_ = true;
}
- queue_group_->status.FetchLatest();
+ queue_group_->status.FetchNext();
+ if (queue_group_->status.get()) {
+ LOG_STRUCT(DEBUG, "got status", *queue_group_->status);
+ }
if (queue_group_->status.get() && queue_group_->status->running != 0) {
old_run_value_ = queue_group_->status->running;
LOG(INFO, "Action %" PRIx32 " on queue %s already running\n",
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
index 4aa5285..cd8d02c 100644
--- a/aos/common/util/trapezoid_profile.cc
+++ b/aos/common/util/trapezoid_profile.cc
@@ -117,6 +117,11 @@
CHECK_GT(top_velocity, -maximum_velocity_);
+ if (output_(1) > maximum_velocity_) {
+ constant_time_ = 0;
+ acceleration_time_ = 0;
+ }
+
deceleration_time_ = (goal_velocity - top_velocity) /
deceleration_;
}
diff --git a/aos/common/util/trapezoid_profile_test.cc b/aos/common/util/trapezoid_profile_test.cc
index 882d1b5..1dfeff3 100644
--- a/aos/common/util/trapezoid_profile_test.cc
+++ b/aos/common/util/trapezoid_profile_test.cc
@@ -58,6 +58,28 @@
EXPECT_TRUE(At(3, 0));
}
+// Tests that decresing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly.
+TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
+ profile_.set_maximum_velocity(1.75);
+ RunIteration(12.0, 0);
+ double last_pos = position()(0);
+ double last_vel = 1.75;
+ for (int i = 0; i < 1600; ++i) {
+ if (i == 400) {
+ profile_.set_maximum_velocity(0.75);
+ }
+ RunIteration(12.0, 0);
+ if (i >= 400) {
+ EXPECT_TRUE(::std::abs(last_pos - position()(0)) <= 1.75 * 0.01);
+ EXPECT_NEAR(last_vel, ::std::abs(last_pos - position()(0)), 0.0001);
+ }
+ last_vel = ::std::abs(last_pos - position()(0));
+ last_pos = position()(0);
+ }
+ EXPECT_TRUE(At(12.0, 0));
+}
+
// There is some somewhat tricky code for dealing with going backwards.
TEST_F(TrapezoidProfileTest, Backwards) {
for (int i = 0; i < 400; ++i) {
diff --git a/aos/config/robotCommand b/aos/config/robotCommand
index afc3a70..7b726a7 100755
--- a/aos/config/robotCommand
+++ b/aos/config/robotCommand
@@ -1 +1 @@
-/home/admin/robot_code/starter_roborio.sh /home/admin/robot_code/start_list.txt "$@"
+/home/admin/robot_code/starter.sh
diff --git a/aos/linux_code/starter/starter.sh b/aos/linux_code/starter/starter.sh
index d2a0ae1..ee2ae1f 100755
--- a/aos/linux_code/starter/starter.sh
+++ b/aos/linux_code/starter/starter.sh
@@ -4,4 +4,4 @@
#echo '/home/driver/tmp/robot_logs/%e-%s-%p-%t.coredump' > /proc/sys/kernel/core_pattern
export PATH=$PATH:/home/admin/robot_code
-exec starter_loop.sh "$@"
+exec starter_exe /home/admin/robot_code/start_list.txt
diff --git a/frc971/actors/fridge_profile_lib.h b/frc971/actors/fridge_profile_lib.h
index fd17b1f..36ea16b 100644
--- a/frc971/actors/fridge_profile_lib.h
+++ b/frc971/actors/fridge_profile_lib.h
@@ -58,15 +58,13 @@
return true;
}
- enum ProfileStatus { RUNNING, DONE, CANCELING, CANCELED };
+ enum ProfileStatus { RUNNING, DONE, CANCELED };
ProfileStatus IterateProfile(double height, double angle,
ProfileParams elevator_parameters,
ProfileParams arm_parameters, bool top_grabbers,
bool front_grabbers, bool back_grabbers) {
- bool should_cancel = false;
if (this->ShouldCancel()) {
- should_cancel = true;
LOG(INFO, "Canceling fridge movement\n");
auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
new_fridge_goal->profiling_type = 0;
@@ -96,8 +94,8 @@
if (!new_fridge_goal.Send()) {
LOG(ERROR, "Failed to send fridge goal\n");
- return CANCELED;
}
+ return CANCELED;
}
control_loops::fridge_queue.status.FetchAnother();
@@ -118,10 +116,7 @@
::std::abs(control_loops::fridge_queue.status->goal_velocity) <
kProfileError) {
LOG(INFO, "Profile done.\n");
- if (should_cancel) {
- LOG(INFO, "Canceling.\n");
- return CANCELED;
- } else if (::std::abs(control_loops::fridge_queue.status->angle - angle) <
+ if (::std::abs(control_loops::fridge_queue.status->angle - angle) <
kAngleEpsilon &&
::std::abs(control_loops::fridge_queue.status->height -
height) < kHeightEpsilon) {
@@ -130,11 +125,7 @@
}
}
- if (should_cancel) {
- return CANCELING;
- } else {
- return RUNNING;
- }
+ return RUNNING;
}
void DoFridgeProfile(double height, double angle,
diff --git a/frc971/actors/score_action.q b/frc971/actors/score_action.q
index f8401d2..79b8be3 100644
--- a/frc971/actors/score_action.q
+++ b/frc971/actors/score_action.q
@@ -4,10 +4,15 @@
// Parameters to send with start.
struct ScoreParams {
- // Height for the upper fridge pivot to be when we're scoring.
- double height;
// If false, then extend. Otherwise, place the stack and retract.
bool place_the_stack;
+
+ // TODO(Brian): Comments (by somebody who knows what these all mean).
+ double upper_move_height;
+ double begin_horizontal_move_height;
+ double horizontal_move_target;
+ double place_height;
+ double home_return_height;
};
queue_group ScoreActionQueueGroup {
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index f0ac01e..d866f6a 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -12,20 +12,13 @@
namespace frc971 {
namespace actors {
-
namespace {
-const double kUpperMoveHeight = 0.14;
-const double kBeginHorizontalMoveHeight = 0.13;
-
-const double kHorizontalMoveTarget = -0.7;
-
-const double kPlaceHeight = -0.10;
-const double kHomeReturnHeight = 0.1;
const double kMaxXVelocity = 0.45;
const double kMaxYVelocity = 0.20;
const double kMaxXAcceleration = 0.5;
const double kMaxYAcceleration = 0.5;
+
} // namespace
ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
@@ -44,9 +37,9 @@
}
}
-bool ScoreActor::MoveStackIntoPosition(const ScoreParams& /*params*/) {
+bool ScoreActor::MoveStackIntoPosition(const ScoreParams& params) {
// Move the fridge up a little bit.
- if (!SendGoal(0.0, kUpperMoveHeight, true)) {
+ if (!SendGoal(0.0, params.upper_move_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -58,13 +51,14 @@
}
// Move on when it is clear of the tote knobs.
- if (CurrentGoalHeight() > kBeginHorizontalMoveHeight) {
+ if (CurrentGoalHeight() > params.begin_horizontal_move_height) {
break;
}
}
// Move the fridge out.
- if (!SendGoal(kHorizontalMoveTarget, kBeginHorizontalMoveHeight, true)) {
+ if (!SendGoal(params.horizontal_move_target,
+ params.begin_horizontal_move_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -75,7 +69,9 @@
return true;
}
- if (NearGoal(kHorizontalMoveTarget, kUpperMoveHeight)) {
+ if (NearGoal(params.horizontal_move_target,
+ params.begin_horizontal_move_height)) {
+ LOG(INFO, "reached goal\n");
break;
}
}
@@ -85,9 +81,9 @@
return true;
}
-bool ScoreActor::PlaceTheStack(const ScoreParams& /*params*/) {
+bool ScoreActor::PlaceTheStack(const ScoreParams& params) {
// Once the fridge is way out, put it on the ground.
- if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, true)) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -98,7 +94,7 @@
return true;
}
- if (NearGoal(kHorizontalMoveTarget, kPlaceHeight)) {
+ if (NearGoal(params.horizontal_move_target, params.place_height)) {
break;
}
}
@@ -106,7 +102,7 @@
if (ShouldCancel()) return true;
// Release the grabbers.
- if (!SendGoal(kHorizontalMoveTarget, kPlaceHeight, false)) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, false)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -114,7 +110,7 @@
if (ShouldCancel()) return true;
// Go back to the home position.
- if (!SendGoal(0.0, kHomeReturnHeight, false)) {
+ if (!SendGoal(0.0, params.home_return_height, false)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -125,7 +121,7 @@
return true;
}
- if (NearGoal(0.0, kHomeReturnHeight)) {
+ if (NearGoal(0.0, params.home_return_height)) {
break;
}
}
diff --git a/frc971/actors/score_actor_test.cc b/frc971/actors/score_actor_test.cc
index 3d0c6c3..25f3aa1 100644
--- a/frc971/actors/score_actor_test.cc
+++ b/frc971/actors/score_actor_test.cc
@@ -59,8 +59,7 @@
frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- bool place_the_stack = true;
- const ScoreParams params = {0.75, place_the_stack};
+ const ScoreParams params = {true, 0.14, 0.13, -0.7, -0.10, 0.1};
score.RunAction(params);
SUCCEED();
@@ -88,8 +87,7 @@
frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
// let the action start running, if we return from this call it has worked.
- bool place_the_stack = false;
- const ScoreParams params = {0.75, place_the_stack};
+ const ScoreParams params = {false, 0.14, 0.13, -0.7, -0.10, 0.1};
score.RunAction(params);
SUCCEED();
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 3f86257..ec98743 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -60,8 +60,8 @@
const double kArmZeroingHeight = 0.2;
-const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
-const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
+const double kMaxAllowedLeftRightArmDifference = 0.12; // radians
+const double kMaxAllowedLeftRightElevatorDifference = 0.04; // meters
const Values::ClawGeometry kClawGeometry{
// Horizontal distance from the center of the grabber to the end.
@@ -300,8 +300,8 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.973311, 0.3},
- 6.1663463999999992,
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.952602, 0.3},
+ 6.1663463999999992 + 0.015241,
kClawPistonSwitchTime,
kClawZeroingRange},
@@ -317,16 +317,18 @@
{-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
// Elevator zeroing constants: left, right.
+ // These are the encoder offsets.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kElevatorEncoderIndexDifference,
- 0.016041 + 0.001290, 0.3},
+ 0.240286 - 0.007969 - 0.025, 0.3},
{kZeroingSampleSize, kElevatorEncoderIndexDifference,
- 0.011367 + 0.003216, 0.3},
+ 0.234583 - 0.000143, 0.3},
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.060592, 0.3},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.210155, 0.3},
- 0.72069366666666679 - 0.026008,
- -0.078959636363636357 - 0.024646,
+ // These are the potentiometer offsets.
+ 0.72069366666666679 - 0.026008 - 0.024948 + 0.025,
+ -0.078959636363636357 - 0.024646 - 0.020260,
-3.509611 - 0.007415 - -0.019081 - 0.029393 - -0.013585,
3.506927 - 0.170017 - -0.147970 - 0.005045 - -0.026504,
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index ff378c8..74c9060 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -159,7 +159,7 @@
break;
case ZEROING:
- LOG(INFO, "Zeroing.\n");
+ LOG(DEBUG, "Zeroing.\n");
// Update state_.
UpdateZeroingState();
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index bcfc6a1..b348bf8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -241,21 +241,21 @@
// Stall Torque in N m
static constexpr double kStallTorque = 2.42;
// Stall Current in Amps
- static constexpr double kStallCurrent = 133;
+ static constexpr double kStallCurrent = 133.0;
// Free Speed in RPM. Used number from last year.
static constexpr double kFreeSpeed = 4650.0;
// Free Current in Amps
static constexpr double kFreeCurrent = 2.7;
// Moment of inertia of the drivetrain in kg m^2
// Just borrowed from last year.
- static constexpr double J = 6.4;
+ static constexpr double J = 10;
// Mass of the robot, in kg.
static constexpr double m = 68;
// Radius of the robot, in meters (from last year).
- static constexpr double rb = 0.617998644 / 2.0;
- static constexpr double kWheelRadius = 0.04445;
+ static constexpr double rb = 0.9603 / 2.0;
+ static constexpr double kWheelRadius = 0.0515938;
// Resistance of the motor, divided by the number of motors.
- static constexpr double kR = (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
+ static constexpr double kR = (12.0 / kStallCurrent / 2 + 0.03) / (0.93 * 0.93);
// Motor velocity constant
static constexpr double Kv =
((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -322,11 +322,13 @@
}
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- const double kWheelNonLinearity = 0.3;
+ const double kWheelNonLinearity = 0.5;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
+
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+ wheel_ *= 2.3;
quickturn_ = quickturn;
static const double kThrottleDeadband = 0.05;
@@ -481,9 +483,10 @@
const double adjusted_ff_voltage = ::aos::Clip(
throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
- return ((adjusted_ff_voltage +
- ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
- (ttrust_ * min_K_sum + min_FF_sum));
+ return (adjusted_ff_voltage +
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+ 2.0) /
+ (ttrust_ * min_K_sum + min_FF_sum);
}
double MaxVelocity() {
@@ -580,6 +583,7 @@
}
const double left_velocity = fvel - steering_velocity;
const double right_velocity = fvel + steering_velocity;
+ LOG(DEBUG, "l=%f r=%f\n", left_velocity, right_velocity);
// Integrate velocity to get the position.
// This position is used to get integral control.
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index a925a89..b04c5af 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+ A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
Eigen::Matrix<double, 4, 2> B;
- B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+ B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+ A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
Eigen::Matrix<double, 4, 2> B;
- B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+ B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+ A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
Eigen::Matrix<double, 4, 2> B;
- B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+ B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+ A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
Eigen::Matrix<double, 4, 2> B;
- B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+ B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+ L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+ A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+ L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+ A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+ L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+ A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+ L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+ A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 3d08774..1641306 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+ A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+ B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+ A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+ B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+ A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+ B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+ A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+ B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+ L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+ K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+ A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+ L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+ K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+ A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+ L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+ K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+ A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+ L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
Eigen::Matrix<double, 2, 2> K;
- K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+ K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+ A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 0c96b23..48f6cf4 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -514,7 +514,9 @@
right_arm(), values.max_allowed_left_right_arm_difference);
// Indicate an ESTOP condition and stop the motors.
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
disable = true;
}
@@ -525,7 +527,9 @@
values.max_allowed_left_right_elevator_difference);
// Indicate an ESTOP condition and stop the motors.
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
disable = true;
}
@@ -563,7 +567,9 @@
LOG(ERROR, "Left arm at %f out of bounds [%f, %f], ESTOPing\n",
left_arm(), values.fridge.arm.lower_hard_limit,
values.fridge.arm.upper_hard_limit);
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
}
if (right_arm() >= values.fridge.arm.upper_hard_limit ||
@@ -571,23 +577,27 @@
LOG(ERROR, "Right arm at %f out of bounds [%f, %f], ESTOPing\n",
right_arm(), values.fridge.arm.lower_hard_limit,
values.fridge.arm.upper_hard_limit);
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
}
- if (left_elevator() >= values.fridge.elevator.upper_hard_limit ||
- left_elevator() <= values.fridge.elevator.lower_hard_limit) {
+ if (left_elevator() >= values.fridge.elevator.upper_hard_limit) {
LOG(ERROR, "Left elevator at %f out of bounds [%f, %f], ESTOPing\n",
left_elevator(), values.fridge.elevator.lower_hard_limit,
values.fridge.elevator.upper_hard_limit);
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
}
- if (right_elevator() >= values.fridge.elevator.upper_hard_limit ||
- right_elevator() <= values.fridge.elevator.lower_hard_limit) {
+ if (right_elevator() >= values.fridge.elevator.upper_hard_limit) {
LOG(ERROR, "Right elevator at %f out of bounds [%f, %f], ESTOPing\n",
right_elevator(), values.fridge.elevator.lower_hard_limit,
values.fridge.elevator.upper_hard_limit);
- state_ = ESTOP;
+ if (output) {
+ state_ = ESTOP;
+ }
}
}
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index b5e7bbe..0fd3501 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -27,7 +27,7 @@
// position.
class FridgeSimulation {
public:
- static constexpr double kNoiseScalar = 1.0;
+ static constexpr double kNoiseScalar = 0.1;
// Constructs a simulation.
FridgeSimulation()
: arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index e2b42f5..8ed62d6 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -56,14 +56,14 @@
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 133.0
# Free Speed in RPM. Used number from last year.
self.free_speed = 4650.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ self.J = 10
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
@@ -71,7 +71,7 @@
# Radius of the wheels, in meters.
self.r = .0515938
# Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 4
+ self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 2b24396..a62c8c8 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -177,7 +177,7 @@
# 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.
- self.ttrust = 1.0
+ self.ttrust = 1.1
self.left_gear = VelocityDrivetrain.LOW
self.right_gear = VelocityDrivetrain.LOW
@@ -302,26 +302,30 @@
# This is the same as sending the most torque down to the floor at the
# wheel.
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.left_gear = self.right_gear = True
+ if False:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ steering *= 2.3
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
- left_velocity = fvel - steering * numpy.abs(fvel)
- right_velocity = fvel + steering * numpy.abs(fvel)
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
@@ -379,7 +383,7 @@
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
@@ -441,13 +445,13 @@
else:
print "Right is low"
- for t in numpy.arange(0, 4.0, vdrivetrain.dt):
- if t < 1.0:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
@@ -474,10 +478,10 @@
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()
+ 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.
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index 4e4b257..4707e66 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -44,6 +44,7 @@
'<(DEPTH)/frc971/actors/actors.gyp:lift_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:held_to_lift_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:can_pickup_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:score_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:horizontal_can_pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:fridge_profile_lib',
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index c16026d..8ef2966 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -19,6 +19,7 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/actors/pickup_actor.h"
#include "frc971/actors/stack_actor.h"
+#include "frc971/actors/score_actor.h"
#include "frc971/actors/stack_and_lift_actor.h"
#include "frc971/actors/stack_and_hold_actor.h"
#include "frc971/actors/held_to_lift_actor.h"
@@ -73,16 +74,17 @@
const ButtonLocation kRollersIn(4, 5);
const ButtonLocation kClawToggle(4, 1);
-const POVLocation kElevatorCanUp(3, 0);
+const POVLocation kElevatorCanUp(4, 0);
// POV stick 3, 180
const POVLocation kCanPickup(4, 180);
const ButtonLocation kToteChute(4, 6);
const ButtonLocation kStackAndLift(4, 7);
-const ButtonLocation kStackAndHold(2, 5);
+const ButtonLocation kStackAndHold(3, 5);
// Pull in the 6th tote.
-const ButtonLocation kSixthTote(4, 10);
+//const ButtonLocation kSixthTote(4, 10);
+const ButtonLocation kCanUp(4, 10);
const ButtonLocation kHeldToLift(4, 11);
const ButtonLocation kPickup(4, 9);
@@ -90,24 +92,61 @@
const ButtonLocation kStack(4, 2);
// Move the fridge out with the stack in preparation for scoring.
-// PosEdge(4, 8)
-
+const ButtonLocation kScore(4, 8);
// Release the stack and retract back in.
-// PosEdge(4, 12)
+const ButtonLocation kRetractFromScore(4, 12);
-const POVLocation kFridgeOpen(4, 270);
+const ButtonLocation kCoopTop(3, 8);
+const ButtonLocation kCoopTopRetract(3, 7);
+const ButtonLocation kCoopBottom(3, 6);
+const ButtonLocation kCoopBottomRetract(3, 9);
+
+const POVLocation kFridgeToggle(4, 270);
const ButtonLocation kSpit(4, 3);
// Set stack down in the bot.
-// TODO(austin): Make this work.
-//const POVLocation kSetStackDownAndHold(4, 90);
+const POVLocation kSetStackDownAndHold(4, 90);
const double kClawTotePackAngle = 0.95;
+const double kArmRaiseLowerClearance = -0.08;
class Reader : public ::aos::input::JoystickInput {
public:
Reader() : was_running_(false) {}
+ static actors::ScoreParams MakeScoreParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.14;
+ r.begin_horizontal_move_height = 0.13;
+ r.horizontal_move_target = -0.7;
+ r.place_height = -0.10;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopTopParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.52;
+ r.begin_horizontal_move_height = 0.5;
+ r.horizontal_move_target = -0.48;
+ r.place_height = 0.39;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
+ static actors::ScoreParams MakeCoopBottomParams(bool place_the_stack) {
+ actors::ScoreParams r;
+ r.place_the_stack = place_the_stack;
+ r.upper_move_height = 0.17;
+ r.begin_horizontal_move_height = 0.16;
+ r.horizontal_move_target = -0.7;
+ r.place_height = 0.0;
+ r.home_return_height = 0.1;
+ return r;
+ }
+
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
bool last_auto_running = auto_running_;
auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
@@ -170,8 +209,8 @@
if (data.PosEdge(kElevatorCanUp)) {
actors::HorizontalCanPickupParams params;
params.elevator_height = 0.3;
- params.pickup_angle = 0.54;
- params.suck_time = 0.05;
+ params.pickup_angle = 0.40;
+ params.suck_time = 0.08;
params.suck_power = 8.0;
params.claw_settle_time = 0.05;
@@ -179,8 +218,9 @@
params.claw_full_lift_angle = 1.35;
params.claw_end_angle = 0.5;
- params.elevator_end_height = 0.68;
- params.arm_end_angle = 0.2;
+ // End low so we don't drop it.
+ params.elevator_end_height = 0.3;
+ params.arm_end_angle = 0.0;
action_queue_.EnqueueAction(
actors::MakeHorizontalCanPickupAction(params));
fridge_closed_ = true;
@@ -193,8 +233,9 @@
params.pickup_angle = -0.93;
params.pickup_height = 0.265;
params.lift_height = 0.65;
- params.end_height = 0.68;
- params.end_angle = 0.2;
+ // End low so the can is supported.
+ params.end_height = 0.3;
+ params.end_angle = 0.0;
action_queue_.EnqueueAction(actors::MakeCanPickupAction(params));
fridge_closed_ = true;
}
@@ -213,6 +254,7 @@
params.stack_params.claw_out_angle = 0.6;
params.stack_params.bottom = 0.020;
params.stack_params.over_box_before_place_height = 0.39;
+ params.stack_params.arm_clearance = kArmRaiseLowerClearance;
params.grab_after_stack = true;
params.clamp_pause_time = 0.1;
@@ -224,15 +266,25 @@
action_queue_.EnqueueAction(actors::MakeStackAndLiftAction(params));
}
- if (data.PosEdge(kStackAndHold)) {
+ if (data.PosEdge(kStackAndHold) || data.PosEdge(kSetStackDownAndHold)) {
actors::StackAndHoldParams params;
params.bottom = 0.020;
params.over_box_before_place_height = 0.39;
- params.arm_clearance = -0.05;
+ params.arm_clearance = kArmRaiseLowerClearance;
params.clamp_pause_time = 0.25;
+ params.claw_clamp_angle = kClawTotePackAngle;
params.hold_height = 0.68;
+ params.claw_out_angle = 0.6;
+
+ if (data.PosEdge(kSetStackDownAndHold)) {
+ params.place_not_stack = true;
+ params.clamp_pause_time = 0.1;
+ //params.claw_clamp_angle = kClawTotePackAngle - 0.5;
+ } else {
+ params.place_not_stack = false;
+ }
action_queue_.EnqueueAction(actors::MakeStackAndHoldAction(params));
fridge_closed_ = true;
@@ -244,8 +296,9 @@
// Lower the fridge from holding the stack, grab the stack, and then lift.
if (data.PosEdge(kHeldToLift)) {
actors::HeldToLiftParams params;
- params.arm_clearance = -0.05;
+ params.arm_clearance = kArmRaiseLowerClearance;
params.clamp_pause_time = 0.1;
+ params.before_lift_settle_time = 0.1;
params.bottom_height = 0.020;
params.claw_out_angle = 0.6;
params.lift_params.lift_height = 0.45;
@@ -255,6 +308,16 @@
action_queue_.EnqueueAction(actors::MakeHeldToLiftAction(params));
}
+ // Lift the can up.
+ if (data.PosEdge(kCanUp)) {
+ actors::LiftParams params;
+ params.lift_height = 0.68;
+ params.lift_arm = 0.3;
+ fridge_closed_ = true;
+
+ action_queue_.EnqueueAction(actors::MakeLiftAction(params));
+ }
+
// Pick up a tote from the ground and put it in the bottom of the bot.
if (data.PosEdge(kPickup)) {
actors::PickupParams params;
@@ -276,26 +339,48 @@
actors::StackParams params;
params.claw_out_angle = 0.6;
params.bottom = 0.020;
+ params.only_place = false;
+ params.arm_clearance = kArmRaiseLowerClearance;
params.over_box_before_place_height = 0.39;
action_queue_.EnqueueAction(actors::MakeStackAction(params));
claw_rollers_closed_ = true;
+ fridge_closed_ = true;
}
- // TODO(austin): Set down?
-
- // TODO(austin): Score!
-
- // TODO(austin): Release.
-
- // Unknown actions...
-
- //if (data.PosEdge(kFridgeClosed)) {
- //fridge_closed_ = true;
- //}
- if (data.PosEdge(kFridgeOpen)) {
+ if (data.PosEdge(kScore)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeScoreParams(false)));
+ }
+ if (data.PosEdge(kRetractFromScore)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeScoreParams(true)));
fridge_closed_ = false;
}
+ if (data.PosEdge(kCoopTop)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(false)));
+ }
+ if (data.PosEdge(kCoopTopRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopTopParams(true)));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kCoopBottom)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(false)));
+ }
+ if (data.PosEdge(kCoopBottomRetract)) {
+ action_queue_.EnqueueAction(
+ actors::MakeScoreAction(MakeCoopBottomParams(true)));
+ fridge_closed_ = false;
+ }
+
+ if (data.PosEdge(kFridgeToggle)) {
+ fridge_closed_ = !fridge_closed_;
+ }
+
if (data.PosEdge(ControlBit::kEnabled)) {
// If we got enabled, wait for everything to zero.
LOG(INFO, "Waiting for zero.\n");
@@ -355,6 +440,8 @@
if (!claw_queue.goal.MakeWithBuilder()
.angle(claw_goal_)
.rollers_closed(claw_rollers_closed_)
+ .max_velocity(4.0)
+ .max_acceleration(6.0)
.intake(intake_power)
.Send()) {
LOG(ERROR, "Sending claw goal failed.\n");
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index 0207742..3422782 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -31,9 +31,21 @@
'interrupt_edge_counting',
'encoder_and_potentiometer',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ 'logging_queue',
],
},
{
+ 'target_name': 'logging_queue',
+ 'type': 'static_library',
+ 'sources': [
+ 'logging.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/wpilib',
+ },
+ 'includes': [ '../../aos/build/queues.gypi' ],
+ },
+ {
'target_name': 'encoder_and_potentiometer',
'type': 'static_library',
'sources': [
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 7364722..8f41300 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/logging.q.h"
#include "Encoder.h"
#include "Talon.h"
@@ -422,13 +423,23 @@
claw_pinchers_->Set(claw_->rollers_closed);
}
}
- if (!pressure_switch_->Get()) {
- compressor_relay_->Set(Relay::kForward);
- } else {
- compressor_relay_->Set(Relay::kOff);
- }
- pcm_->Flush();
+ {
+ PneumaticsToLog to_log;
+ {
+ const bool compressor_on = !pressure_switch_->Get();
+ to_log.compressor_on = compressor_on;
+ if (compressor_on) {
+ compressor_relay_->Set(Relay::kForward);
+ } else {
+ compressor_relay_->Set(Relay::kOff);
+ }
+ }
+
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
}
}