Horizontal can is faster now.
Change-Id: If21a0a97f57d6476808db5c611469c0e2e35ef1d
diff --git a/frc971/actors/horizontal_can_pickup_actor.cc b/frc971/actors/horizontal_can_pickup_actor.cc
index cab89f3..6f77261 100644
--- a/frc971/actors/horizontal_can_pickup_actor.cc
+++ b/frc971/actors/horizontal_can_pickup_actor.cc
@@ -12,8 +12,9 @@
namespace frc971 {
namespace actors {
namespace {
-constexpr double kClawPickupVelocity = 3.00;
-constexpr double kClawPickupAcceleration = 2.0;
+constexpr ProfileParams kClawPickup{3.0, 2.0};
+constexpr ProfileParams kClawBackDown{7.0, 10.0};
+constexpr ProfileParams kClawInitialLift{7.0, 8.0};
constexpr ProfileParams kArmMove{1.0, 1.6};
constexpr ProfileParams kElevatorMove{0.6, 2.2};
@@ -22,6 +23,7 @@
constexpr ProfileParams kFastElevatorMove{1.0, 3.0};
constexpr double kAngleEpsilon = 0.10;
+constexpr double kGoalAngleEpsilon = 0.01;
} // namespace
@@ -29,6 +31,19 @@
HorizontalCanPickupActionQueueGroup *queues)
: FridgeActorBase<HorizontalCanPickupActionQueueGroup>(queues) {}
+bool HorizontalCanPickupActor::WaitUntilGoalNear(double angle) {
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return false;
+ const double goal_angle = control_loops::claw_queue.status->goal_angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(goal_angle - angle) < kGoalAngleEpsilon) {
+ return true;
+ }
+ }
+}
+
bool HorizontalCanPickupActor::WaitUntilNear(double angle) {
while (true) {
control_loops::claw_queue.status.FetchAnother();
@@ -41,20 +56,22 @@
}
}
}
-
void HorizontalCanPickupActor::MoveArm(double angle, double intake_power) {
- {
- auto message = control_loops::claw_queue.goal.MakeMessage();
- message->angle = angle;
- message->max_velocity = kClawPickupVelocity;
- message->max_acceleration = kClawPickupAcceleration;
- message->angular_velocity = 0.0;
- message->intake = intake_power;
- message->rollers_closed = true;
+ MoveArm(angle, intake_power, kClawPickup);
+}
- LOG_STRUCT(DEBUG, "Sending claw goal", *message);
- message.Send();
- }
+void HorizontalCanPickupActor::MoveArm(double angle, double intake_power,
+ const ProfileParams profile_params) {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = angle;
+ message->max_velocity = profile_params.velocity;
+ message->max_acceleration = profile_params.acceleration;
+ message->angular_velocity = 0.0;
+ message->intake = intake_power;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
}
bool HorizontalCanPickupActor::RunAction(
@@ -65,7 +82,7 @@
return true;
}
- MoveArm(params.pickup_angle, 0.0);
+ MoveArm(params.pickup_angle, 0.0, kClawInitialLift);
if (!WaitUntilNear(params.pickup_angle)) {
return true;
@@ -77,9 +94,9 @@
return true;
}
- MoveArm(0.0, 0.0);
+ MoveArm(0.0, 0.0, kClawBackDown);
- if (!WaitUntilNear(0.0)) {
+ if (!WaitUntilGoalNear(0.0)) {
return true;
}
diff --git a/frc971/actors/horizontal_can_pickup_actor.h b/frc971/actors/horizontal_can_pickup_actor.h
index c69b5a1..f3bb0f0 100644
--- a/frc971/actors/horizontal_can_pickup_actor.h
+++ b/frc971/actors/horizontal_can_pickup_actor.h
@@ -25,8 +25,11 @@
// Waits until we are near the angle.
// Returns false if we should cancel.
bool WaitUntilNear(double angle);
+ bool WaitUntilGoalNear(double angle);
void MoveArm(double angle, double intake_power);
+ void MoveArm(double angle, double intake_power,
+ const ProfileParams profile_params);
};
typedef aos::common::actions::TypedAction<HorizontalCanPickupActionQueueGroup>
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index f1d71fd..1ce323a 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -71,7 +71,9 @@
//const ButtonLocation kFridgeClosed(3, 1);
-const ButtonLocation kRollersIn(4, 5);
+const ButtonLocation kStackAndHold(3, 5);
+const ButtonLocation kRollersIn(3, 2);
+const ButtonLocation kHorizontalCanRollersIn(4, 5);
const ButtonLocation kClawToggle(4, 1);
const POVLocation kElevatorCanUp(4, 0);
@@ -80,7 +82,6 @@
const POVLocation kCanPickup(4, 180);
const ButtonLocation kToteChute(4, 6);
const ButtonLocation kStackAndLift(4, 7);
-const ButtonLocation kStackAndHold(3, 5);
// Pull in the 6th tote.
//const ButtonLocation kSixthTote(4, 10);
@@ -109,6 +110,7 @@
const double kClawTotePackAngle = 0.90;
const double kArmRaiseLowerClearance = -0.08;
const double kClawStackClearance = 0.55;
+const double kHorizontalCanClawAngle = 0.12;
const double kStackUpHeight = 0.60;
const double kStackUpArm = 0.0;
@@ -139,7 +141,7 @@
r.horizontal_move_target = -0.48;
r.horizontal_start_lowering = r.horizontal_move_target;
r.home_lift_horizontal_start_position = -0.3;
- r.place_height = 0.39;
+ r.place_height = 0.35;
r.home_return_height = 0.1;
return r;
}
@@ -201,8 +203,13 @@
intake_power = 10.0;
claw_goal_ = 0.0;
}
+ if (data.IsPressed(kHorizontalCanRollersIn)) {
+ intake_power = 10.0;
+ claw_goal_ = kHorizontalCanClawAngle;
+ }
if (data.IsPressed(kSpit)) {
intake_power = -12.0;
+ action_queue_.CancelAllActions();
}
// Toggle button for the claw
@@ -220,9 +227,9 @@
if (data.PosEdge(kElevatorCanUp)) {
actors::HorizontalCanPickupParams params;
params.elevator_height = 0.3;
- params.pickup_angle = 0.40;
- params.suck_time = 0.08;
- params.suck_power = 8.0;
+ params.pickup_angle = 0.60 + kHorizontalCanClawAngle;
+ params.suck_time = 0.10;
+ params.suck_power = 10.0;
params.claw_settle_time = 0.05;
params.claw_settle_power = 5.0;
@@ -262,7 +269,7 @@
// Tote chute pull in when button is pressed, pack when done.
if (data.IsPressed(kToteChute)) {
- claw_goal_ = 0.8;
+ claw_goal_ = 0.75;
intake_power = 7.0;
}
if (data.NegEdge(kToteChute)) {
@@ -455,7 +462,7 @@
if (!claw_queue.goal.MakeWithBuilder()
.angle(claw_goal_)
.rollers_closed(claw_rollers_closed_)
- .max_velocity(4.0)
+ .max_velocity(5.0)
.max_acceleration(6.0)
.intake(intake_power)
.Send()) {