Merge "actually restart the code on the roborio when it crashes"
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index 3b80184..49fbf98 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -75,20 +75,25 @@
}
if (params.only_place) {
+ // open grabber for place only
DoFridgeProfile(params.bottom + values.tote_height, 0.0, kFastElevatorMove,
kFastArmMove, false);
// Finish early if we aren't supposed to grab.
return true;
}
- DoFridgeProfile(params.bottom + values.tote_height, params.arm_clearance,
+ // TODO(ben): I'm not sure why this liitle jog is here. we are removing it for
+ // the fangs, but I want to keep the code here so austin can explain.
+ /*DoFridgeProfile(params.bottom + values.tote_height, params.arm_clearance,
kFastElevatorMove, kFastArmMove, false);
if (ShouldCancel()) return true;
DoFridgeProfile(params.bottom, params.arm_clearance, kFastElevatorMove,
- kFastArmMove, false);
+ kFastArmMove, false);*/
if (ShouldCancel()) return true;
- DoFridgeProfile(params.bottom, 0.0, kFastElevatorMove, kFastArmMove, false);
+ // grab can (if in fang mode the grabber stays closed)
+ DoFridgeProfile(params.bottom, 0.0, kFastElevatorMove, kFastArmMove, false,
+ true, true);
if (ShouldCancel()) return true;
aos::time::SleepFor(aos::time::Time::InMS(200));
diff --git a/frc971/actors/stack_and_hold_actor.cc b/frc971/actors/stack_and_hold_actor.cc
index 2acb415..ab9b077 100644
--- a/frc971/actors/stack_and_hold_actor.cc
+++ b/frc971/actors/stack_and_hold_actor.cc
@@ -23,6 +23,9 @@
: FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
bool StackAndHoldActor::RunAction(const StackAndHoldParams ¶ms) {
+ // TODO(ben)): this action is no longer used (source Cameron) and my be broken
+ // by the stack action having the grabbers closed at the end for the fangs. So
+ // here I am disabling it until further information is provided.
if (params.place_not_stack) {
// Move the arm out of the way.
{
diff --git a/frc971/constants.cc b/frc971/constants.cc
index ec98743..785c0d7 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -248,13 +248,13 @@
{-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
// Elevator zeroing constants: left, right.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.088984, 0.3},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.104557, 0.3},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.110677, 0.3}, // Was 0.088984 (3 mm too high)
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.109974, 0.3}, // Was 0.104557 (4 mm too low)
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, -0.324437, 0.3},
{kZeroingSampleSize, kArmEncoderIndexDifference, -0.064683, 0.3},
- 0.722230 - -0.000594 - -0.026183,
- -0.081354 - -0.000374 - -0.024793,
+ 0.722230 - -0.000594 - -0.026183 - 0.003442, // Left Elevator Poteniometer adjustment
+ -0.081354 - -0.000374 - -0.024793 - -0.006916, // Right Elevator Poteniometer adjustment
-3.509611 - 0.007415 - -0.019081,
3.506927 - 0.170017 - -0.147970,
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index e414f93..14514a9 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -335,12 +335,11 @@
elevator_goal_ += kElevatorSafeHeightVelocity *
::aos::controls::kLoopFrequency.ToSeconds();
elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
+ state_ = ZEROING_ELEVATOR;
} else {
// We want it stopped at whatever height it's currently set to.
elevator_goal_velocity_ = 0;
}
-
- state_ = ZEROING_ELEVATOR;
break;
}
@@ -365,7 +364,8 @@
case ZEROING_ARM:
LOG(DEBUG, "Zeroing the arm\n");
- if (elevator() < values.fridge.arm_zeroing_height) {
+ if (elevator() < values.fridge.arm_zeroing_height - 0.10 ||
+ elevator_goal_ < values.fridge.arm_zeroing_height) {
LOG(INFO,
"Going back to ZEROING_ELEVATOR until it gets high enough to "
"safely zero the arm\n");