Merge changes I4058077b,I25ec37a6,If21a0a97,Iff72d608,If1750d4b, ...
* changes:
Fixed fridge lurching down.
Score is faster.
Horizontal can is faster now.
No longer lower the claw in as many places and stack faster.
Rounded score set down corner.
Sped up vertical can pickup
Sped up stack.
Start fridge and claw at a reasonable position after zeroing.
Added XY to the fridge profile lib.
diff --git a/frc971/actors/can_pickup_action.q b/frc971/actors/can_pickup_action.q
index 95e0b3f..4846722 100644
--- a/frc971/actors/can_pickup_action.q
+++ b/frc971/actors/can_pickup_action.q
@@ -7,13 +7,19 @@
// It starts by lifting up, then moving the arm out, then lifting the can out of
// the claw, and then ends with the can inside the bot.
struct CanPickupParams {
- // Angle to move the claw to when picking up.
- double pickup_angle;
- // Height to move the elevator to when picking up.
- double pickup_height;
+ // X position for the fridge when picking up.
+ double pickup_x;
+ // Y position for the fridge when picking up.
+ double pickup_y;
// Height to move the elevator to to lift the can out of the claw.
double lift_height;
+ // Height to use when lifting before moving it back.
+ double pickup_goal_before_move_height;
+ // The height at which to start pulling back.
+ double before_place_height;
+ // X at which to start lowering the can.
+ double start_lowering_x;
// End position with the can.
double end_height;
double end_angle;
diff --git a/frc971/actors/can_pickup_actor.cc b/frc971/actors/can_pickup_actor.cc
index ee1d21e..ca493c3 100644
--- a/frc971/actors/can_pickup_actor.cc
+++ b/frc971/actors/can_pickup_actor.cc
@@ -7,16 +7,41 @@
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw.q.h"
+using ::frc971::control_loops::fridge_queue;
+
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kArmMove{1.0, 1.6};
-constexpr ProfileParams kElevatorMove{0.6, 2.2};
+constexpr ProfileParams kHorizontalMove{1.1, 2.5};
+constexpr ProfileParams kVerticalMove{0.3, 2.0};
+constexpr ProfileParams kFastHorizontalMove{1.25, 5.0};
+constexpr ProfileParams kFastVerticalMove{0.40, 2.0};
+constexpr ProfileParams kPureVerticalMove{1.20, 5.0};
} // namespace
CanPickupActor::CanPickupActor(CanPickupActionQueueGroup *queues)
: FridgeActorBase<CanPickupActionQueueGroup>(queues) {}
+double CanPickupActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
+}
+
+double CanPickupActor::CurrentGoalHeight() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_y;
+}
+
bool CanPickupActor::RunAction(const CanPickupParams ¶ms) {
// Make sure the claw is down.
{
@@ -33,23 +58,43 @@
}
// Go around the can.
- DoFridgeProfile(params.pickup_height, params.pickup_angle, kElevatorMove,
- kArmMove, false);
+ DoFridgeXYProfile(params.pickup_x, params.pickup_y, kFastHorizontalMove,
+ kFastVerticalMove, true, false, false);
if (ShouldCancel()) return true;
- // Lift and grab.
- DoFridgeProfile(params.lift_height, params.pickup_angle, kElevatorMove,
- kArmMove, true);
+ if (!StartFridgeXYProfile(
+ params.pickup_x, params.pickup_goal_before_move_height,
+ kHorizontalMove, kPureVerticalMove, true, true, true)) {
+ return false;
+ }
+
+ bool above_claw = false;
+ while (true) {
+ if (CurrentGoalHeight() > params.lift_height && !above_claw) {
+ if (!StartFridgeXYProfile(0.0, params.before_place_height,
+ kHorizontalMove, kVerticalMove, true, true,
+ true)) {
+ return false;
+ }
+ above_claw = true;
+ }
+ if (CurrentGoalX() < params.start_lowering_x) {
+ // Getting close, start lowering.
+ LOG(DEBUG, "Starting to lower the can onto the tray.\n");
+ break;
+ }
+ ProfileStatus status =
+ IterateXYProfile(0.0, params.before_place_height, kHorizontalMove,
+ kVerticalMove, true, true, true);
+ if (status == DONE || status == CANCELED) {
+ break;
+ }
+ }
if (ShouldCancel()) return true;
- // Pull it back in.
- DoFridgeProfile(params.lift_height, params.end_angle, kElevatorMove, kArmMove,
- true);
- if (ShouldCancel()) return true;
-
- // Pull it back in.
- DoFridgeProfile(params.end_height, params.end_angle, kElevatorMove, kArmMove,
- true);
+ // Lower it.
+ DoFridgeXYProfile(0.0, params.end_height, kHorizontalMove, kPureVerticalMove,
+ true);
if (ShouldCancel()) return true;
return true;
diff --git a/frc971/actors/can_pickup_actor.h b/frc971/actors/can_pickup_actor.h
index f343214..3e109d1 100644
--- a/frc971/actors/can_pickup_actor.h
+++ b/frc971/actors/can_pickup_actor.h
@@ -18,6 +18,10 @@
explicit CanPickupActor(CanPickupActionQueueGroup *queues);
bool RunAction(const CanPickupParams ¶ms) override;
+
+ private:
+ double CurrentGoalHeight();
+ double CurrentGoalX();
};
typedef aos::common::actions::TypedAction<CanPickupActionQueueGroup>
diff --git a/frc971/actors/fridge_profile_lib.h b/frc971/actors/fridge_profile_lib.h
index 747f635..2e0dcc2 100644
--- a/frc971/actors/fridge_profile_lib.h
+++ b/frc971/actors/fridge_profile_lib.h
@@ -150,6 +150,133 @@
}
}
}
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool grabbers) {
+ DoFridgeXYProfile(x, y, x_parameters, y_parameters, grabbers, grabbers,
+ grabbers);
+ }
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (!StartFridgeXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers)) {
+ return;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers);
+ if (status == DONE || status == CANCELED) {
+ return;
+ }
+ }
+ }
+
+ void CancelXYMotion(ProfileParams x_parameters, ProfileParams y_parameters,
+ bool top_grabbers, bool front_grabbers,
+ bool back_grabbers) {
+ LOG(INFO, "Canceling fridge movement\n");
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(WARNING, "no fridge status so can't really cancel\n");
+ return;
+ }
+
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x =
+ control_loops::fridge_queue.status->x +
+ (control_loops::fridge_queue.status->goal_x_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity)) /
+ (2.0 * new_fridge_goal->max_x_acceleration);
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y =
+ control_loops::fridge_queue.status->y +
+ (control_loops::fridge_queue.status->goal_y_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity)) /
+ (2.0 * new_fridge_goal->max_y_acceleration);
+ new_fridge_goal->y_velocity = 0.0;
+
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ }
+ }
+
+ ProfileStatus IterateXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (this->ShouldCancel()) {
+ CancelXYMotion(x_parameters, y_parameters, top_grabbers, front_grabbers,
+ back_grabbers);
+ return CANCELED;
+ }
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kXEpsilon = 0.02, kYEpsilon = 0.02;
+
+ if (control_loops::fridge_queue.status->state != 4) {
+ LOG(ERROR, "Fridge no longer running, aborting action\n");
+ return CANCELED;
+ }
+
+ if (::std::abs(control_loops::fridge_queue.status->goal_x - x) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y - y) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::fridge_queue.status->x - x) < kXEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->y - y) < kYEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return DONE;
+ }
+ }
+
+ return RUNNING;
+ }
+
+ bool StartFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x = x;
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y = y;
+ new_fridge_goal->y_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+ LOG(INFO, "Starting xy profile to %f, %f\n", x, y);
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return false;
+ }
+ return true;
+ }
};
} // namespace actors
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/actors/score_action.q b/frc971/actors/score_action.q
index c2a5dba..5b14210 100644
--- a/frc971/actors/score_action.q
+++ b/frc971/actors/score_action.q
@@ -13,7 +13,9 @@
double upper_move_height;
double begin_horizontal_move_height;
double horizontal_move_target;
+ double horizontal_start_lowering;
double place_height;
+ double home_lift_horizontal_start_position;
double home_return_height;
};
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index faa2b07..d650bb6 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -15,17 +15,19 @@
namespace actors {
namespace {
-const double kSlowMaxXVelocity = 0.60;
-const double kSlowMaxYVelocity = 0.25;
+const double kSlowMaxXVelocity = 0.80;
+const double kSlowMaxYVelocity = 0.40;
const double kFastMaxXVelocity = 0.80;
const double kFastMaxYVelocity = 0.30;
-const double kReallyFastMaxXVelocity = 1;
-const double kReallyFastMaxYVelocity = 0.30;
+const double kReallyFastMaxXVelocity = 1.0;
+const double kReallyFastMaxYVelocity = 0.6;
const double kMaxXAcceleration = 0.5;
-const double kMaxYAcceleration = 0.5;
-const double kFastMaxXAcceleration = 1.2;
-const double kFastMaxYAcceleration = 1.2;
+const double kMaxYAcceleration = 0.7;
+const double kLiftYAcceleration = 2.0;
+const double kLowerYAcceleration = 2.0;
+const double kFastMaxXAcceleration = 1.5;
+const double kFastMaxYAcceleration = 1.5;
const double kSlowMaxXAcceleration = 0.3;
const double kSlowMaxYAcceleration = 0.5;
@@ -58,7 +60,7 @@
bool ScoreActor::MoveStackIntoPosition(const ScoreParams& params) {
// Move the fridge up a little bit.
if (!SendGoal(0.0, params.upper_move_height, true, kSlowMaxXVelocity,
- kSlowMaxYVelocity, kMaxXAcceleration, kMaxYAcceleration)) {
+ kSlowMaxYVelocity, kMaxXAcceleration, kLiftYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -79,20 +81,33 @@
// Move the fridge out.
if (!SendGoal(params.horizontal_move_target,
params.begin_horizontal_move_height, true, kSlowMaxXVelocity,
- kSlowMaxYVelocity, kSlowMaxXAcceleration,
+ kFastMaxYVelocity, kSlowMaxXAcceleration,
kSlowMaxYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
+ bool started_lowering = false;
+
while (true) {
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
if (ShouldCancel()) {
return true;
}
+ // Round the moving out corner and start setting down.
+ if (params.place_the_stack && !started_lowering) {
+ if (CurrentGoalX() < params.horizontal_start_lowering) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true,
+ kSlowMaxXVelocity, kFastMaxYVelocity,
+ kSlowMaxXAcceleration, kMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ started_lowering = true;
+ }
+ }
- if (NearGoal(params.horizontal_move_target,
- params.begin_horizontal_move_height)) {
+ if (NearHorizontalGoal(params.horizontal_move_target)) {
LOG(INFO, "reached goal\n");
break;
}
@@ -107,7 +122,7 @@
// Once the fridge is way out, put it on the ground.
if (!SendGoal(params.horizontal_move_target, params.place_height, true,
kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
- kMaxYAcceleration)) {
+ kLowerYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
@@ -126,36 +141,38 @@
// Release and give the grabers a chance to get out of the way.
if (!SendGoal(params.horizontal_move_target, params.place_height, false,
kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
- kMaxYAcceleration)) {
+ kLowerYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
if (!WaitOrCancel(::aos::time::Time::InSeconds(0.1))) return true;
- // Release the grabbers.
- if (!SendGoal(params.horizontal_move_target, params.place_height, false,
- kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
- kMaxYAcceleration)) {
- LOG(ERROR, "Sending fridge message failed.\n");
- return false;
- }
-
- if (ShouldCancel()) return true;
-
// Go back to the home position.
- if (!SendGoal(0.0, params.home_return_height, false, kReallyFastMaxXVelocity,
+ if (!SendGoal(0.0, params.place_height, false, kReallyFastMaxXVelocity,
kReallyFastMaxYVelocity, kFastMaxXAcceleration,
kFastMaxYAcceleration)) {
LOG(ERROR, "Sending fridge message failed.\n");
return false;
}
+ bool has_lifted = false;
while (true) {
::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
if (ShouldCancel()) {
return true;
}
+ if (!has_lifted &&
+ CurrentGoalX() > params.home_lift_horizontal_start_position) {
+ if (!SendGoal(0.0, params.home_return_height, false,
+ kReallyFastMaxXVelocity, kReallyFastMaxYVelocity,
+ kFastMaxXAcceleration, kFastMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ has_lifted = true;
+ }
+
if (NearGoal(0.0, params.home_return_height)) {
break;
}
@@ -200,11 +217,17 @@
return false;
}
- ::aos::util::ElevatorArmKinematics::KinematicResult results;
- kinematics_.ForwardKinematic(fridge_queue.status->height,
- fridge_queue.status->angle, 0.0, 0.0, &results);
+ return fridge_queue.status->x;
+}
- return results.fridge_x;
+double ScoreActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
}
bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled,
@@ -230,7 +253,7 @@
return new_fridge_goal.Send();
}
-bool ScoreActor::NearGoal(double x, double y) {
+bool ScoreActor::NearHorizontalGoal(double x) {
fridge_queue.status.FetchLatest();
if (!fridge_queue.status.get()) {
LOG(ERROR, "Reading from fridge status queue failed.\n");
@@ -246,9 +269,21 @@
&goal_results);
return (::std::abs(results.fridge_x - x) < 0.020 &&
- ::std::abs(results.fridge_h - y) < 0.020 &&
- ::std::abs(goal_results.fridge_x - x) < 0.0001 &&
- ::std::abs(goal_results.fridge_h - y) < 0.0001);
+ ::std::abs(goal_results.fridge_x - x) < 0.0001);
+}
+
+bool ScoreActor::NearGoal(double x, double y) {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ const auto &status = *fridge_queue.status;
+ return (::std::abs(status.x - x) < 0.020 &&
+ ::std::abs(status.y - y) < 0.020 &&
+ ::std::abs(status.goal_x - x) < 0.0001 &&
+ ::std::abs(status.goal_y - y) < 0.0001);
}
::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams& params) {
diff --git a/frc971/actors/score_actor.h b/frc971/actors/score_actor.h
index c2d7868..a524d36 100644
--- a/frc971/actors/score_actor.h
+++ b/frc971/actors/score_actor.h
@@ -20,6 +20,7 @@
::aos::util::ElevatorArmKinematics kinematics_;
bool NearGoal(double x, double y);
+ bool NearHorizontalGoal(double x);
bool PlaceTheStack(const ScoreParams ¶ms);
bool MoveStackIntoPosition(const ScoreParams ¶ms);
bool SendGoal(double x, double y, bool grabbers_enabled,
@@ -28,6 +29,7 @@
double CurrentHeight();
double CurrentGoalHeight();
double CurrentX();
+ double CurrentGoalX();
};
typedef aos::common::actions::TypedAction<ScoreActionQueueGroup> ScoreAction;
diff --git a/frc971/actors/score_actor_test.cc b/frc971/actors/score_actor_test.cc
index e462c54..450939f 100644
--- a/frc971/actors/score_actor_test.cc
+++ b/frc971/actors/score_actor_test.cc
@@ -59,7 +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.
- const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.10, 0.1};
+ const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
score.RunAction(params);
SUCCEED();
@@ -87,7 +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.
- const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.10, 0.1};
+ const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
score.RunAction(params);
SUCCEED();
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index 49fbf98..c9622c2 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -11,12 +11,13 @@
namespace frc971 {
namespace actors {
namespace {
-constexpr ProfileParams kArmWithStackMove{1.75, 0.60};
+constexpr ProfileParams kArmWithStackMove{1.75, 4.20};
constexpr ProfileParams kSlowArmMove{1.3, 1.4};
-constexpr ProfileParams kSlowElevatorMove{0.5, 3.0};
+constexpr ProfileParams kSlowElevatorMove{1.0, 3.0};
constexpr ProfileParams kFastArmMove{0.8, 4.0};
constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
+constexpr ProfileParams kReallyFastElevatorMove{1.2, 6.0};
} // namespace
StackActor::StackActor(StackActionQueueGroup *queues)
@@ -41,10 +42,6 @@
}
// Set the current stack down on top of the bottom box.
- DoFridgeProfile(params.over_box_before_place_height, 0.0, kSlowElevatorMove,
- kArmWithStackMove, true);
- if (ShouldCancel()) return true;
- // Set down on the box.
DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowElevatorMove,
kSlowArmMove, true);
if (ShouldCancel()) return true;
@@ -82,20 +79,10 @@
return true;
}
- // 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);*/
if (ShouldCancel()) return true;
// grab can (if in fang mode the grabber stays closed)
- DoFridgeProfile(params.bottom, 0.0, kFastElevatorMove, kFastArmMove, false,
+ DoFridgeProfile(params.bottom, 0.0, kReallyFastElevatorMove, kFastArmMove, true,
true, true);
- if (ShouldCancel()) return true;
- aos::time::SleepFor(aos::time::Time::InMS(200));
return true;
}
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 14514a9..b5e0ab6 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -340,9 +340,7 @@
// We want it stopped at whatever height it's currently set to.
elevator_goal_velocity_ = 0;
}
- break;
}
-
} else if (!disable) {
elevator_goal_velocity_ = elevator_zeroing_velocity();
elevator_goal_ += elevator_goal_velocity_ *
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 95f369a..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);
@@ -106,12 +107,13 @@
// Set stack down in the bot.
const POVLocation kSetStackDownAndHold(4, 90);
-const double kClawTotePackAngle = 0.95;
+const double kClawTotePackAngle = 0.90;
const double kArmRaiseLowerClearance = -0.08;
const double kClawStackClearance = 0.55;
+const double kHorizontalCanClawAngle = 0.12;
-const double kStackUpHeight = 0.55;
-const double kStackUpArm = 0.1;
+const double kStackUpHeight = 0.60;
+const double kStackUpArm = 0.0;
class Reader : public ::aos::input::JoystickInput {
public:
@@ -123,8 +125,10 @@
r.upper_move_height = 0.14;
r.begin_horizontal_move_height = 0.13;
r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = -0.65;
+ r.home_lift_horizontal_start_position = -0.60;
r.place_height = -0.10;
- r.home_return_height = 0.1;
+ r.home_return_height = 0.05;
return r;
}
@@ -135,7 +139,9 @@
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.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
+ r.place_height = 0.35;
r.home_return_height = 0.1;
return r;
}
@@ -147,6 +153,8 @@
r.upper_move_height = 0.17;
r.begin_horizontal_move_height = 0.16;
r.horizontal_move_target = -0.7;
+ r.horizontal_start_lowering = r.horizontal_move_target;
+ r.home_lift_horizontal_start_position = -0.3;
r.place_height = 0.0;
r.home_return_height = 0.1;
return r;
@@ -195,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
@@ -214,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;
@@ -235,10 +248,13 @@
// Vertical can pickup.
if (data.PosEdge(kCanPickup)) {
actors::CanPickupParams params;
- params.pickup_angle = -0.93;
- params.pickup_height = 0.265;
- params.lift_height = 0.65;
+ params.pickup_x = 0.6;
+ params.pickup_y = 0.1;
+ params.lift_height = 0.2;
+ params.pickup_goal_before_move_height = 0.3;
+ params.start_lowering_x = 0.1;
// End low so the can is supported.
+ params.before_place_height = 0.4;
params.end_height = 0.3;
params.end_angle = 0.0;
action_queue_.EnqueueAction(actors::MakeCanPickupAction(params));
@@ -253,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)) {
@@ -262,13 +278,13 @@
if (data.PosEdge(kStackAndLift)) {
actors::StackAndLiftParams params;
- params.stack_params.claw_out_angle = kClawStackClearance;
+ params.stack_params.claw_out_angle = kClawTotePackAngle;
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;
+ params.clamp_pause_time = 0.0;
params.lift_params.lift_height = kStackUpHeight;
params.lift_params.lift_arm = kStackUpArm;
params.grab_after_lift = true;
@@ -287,7 +303,7 @@
params.claw_clamp_angle = kClawTotePackAngle;
params.hold_height = 0.68;
- params.claw_out_angle = kClawStackClearance;
+ params.claw_out_angle = kClawTotePackAngle;
if (data.PosEdge(kSetStackDownAndHold)) {
params.place_not_stack = true;
@@ -307,11 +323,11 @@
// Lower the fridge from holding the stack, grab the stack, and then lift.
if (data.PosEdge(kHeldToLift)) {
actors::HeldToLiftParams params;
- params.arm_clearance = kArmRaiseLowerClearance;
+ params.arm_clearance = kClawStackClearance;
params.clamp_pause_time = 0.1;
params.before_lift_settle_time = 0.1;
params.bottom_height = 0.020;
- params.claw_out_angle = kClawStackClearance;
+ params.claw_out_angle = kClawTotePackAngle;
params.lift_params.lift_height = kStackUpHeight;
params.lift_params.lift_arm = kStackUpArm;
fridge_closed_ = true;
@@ -350,7 +366,7 @@
// Place stack on a tote in the tray, and grab it.
if (data.PosEdge(kStack)) {
actors::StackParams params;
- params.claw_out_angle = kClawStackClearance;
+ params.claw_out_angle = kClawTotePackAngle;
params.bottom = 0.020;
params.only_place = false;
params.arm_clearance = kArmRaiseLowerClearance;
@@ -412,9 +428,9 @@
waiting_for_zero_ = false;
// Set the initial goals to where we are now.
- elevator_goal_ = fridge_queue.status->goal_height;
- arm_goal_ = fridge_queue.status->goal_angle;
- claw_goal_ = claw_queue.status->angle;
+ elevator_goal_ = 0.3;
+ arm_goal_ = 0.0;
+ claw_goal_ = 0.6;
}
} else {
waiting_for_zero_ = true;
@@ -446,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()) {