Merge "Fix LED PWM frequency on button board"
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index 1e9b936..394d574 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -20,6 +20,11 @@
bool autonomous;
uint16_t team_id;
+ // 2018 scale and switch positions.
+ // TODO(austin): Push these out to a new message?
+ bool switch_left;
+ bool scale_left;
+
// If this is true, then this message isn't actually from the control
// system and so should not be trusted as evidence that the button inputs
// etc are actually real and should be acted on.
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 7d406d0..741b49c 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -31,13 +31,18 @@
ds->WaitForData();
auto new_state = ::aos::joystick_state.MakeMessage();
-#if defined(WPILIB2017) || defined(WPILIB2018)
HAL_ControlWord control_word;
HAL_GetControlWord(&control_word);
-#else
- HALControlWord control_word;
- HALGetControlWord(&control_word);
-#endif
+ HAL_MatchInfo match_info;
+ auto status = HAL_GetMatchInfo(&match_info);
+ if (status == 0) {
+ new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l';
+ new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l';
+ }
+ HAL_FreeMatchInfo(&match_info);
+
new_state->test_mode = control_word.test;
new_state->fms_attached = control_word.fmsAttached;
new_state->enabled = control_word.enabled;
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index 83a1295..a31d2f3 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -27,8 +27,13 @@
.count();
}
-const ProfileParameters kDrive = {1.5, 2.0};
-const ProfileParameters kTurn = {3.0, 3.0};
+const ProfileParameters kFinalSwitchDrive = {0.5, 0.5};
+const ProfileParameters kDrive = {5.0, 2.5};
+const ProfileParameters kSlowDrive = {1.5, 2.5};
+const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
+const ProfileParameters kTurn = {4.0, 2.0};
+const ProfileParameters kSweepingTurn = {5.0, 7.0};
+const ProfileParameters kFastTurn = {5.0, 7.0};
} // namespace
@@ -43,13 +48,107 @@
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
Reset();
- for (int i = 0; i < 4; ++i) {
- StartDrive(0.0, M_PI / 2.0, kDrive, kTurn);
- if (!WaitForTurnProfileDone()) return true;
+ switch (params.mode) {
+ case 1: {
+ constexpr double kDriveDistance = 3.2;
+ // Start on the left. Drive, arc a turn, and drop in the close switch.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(2.0, M_PI / 2.0)) return true;
- // Drive, but get within 0.3 meters
- StartDrive(1.0, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::BackSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, -M_PI / 4.0 - 0.2, kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(0.2, 0.2)) return true;
+ set_max_drivetrain_voltage(6.0);
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(0.2, 0.0, kDrive, kTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+
+ } break;
+ case 0: {
+ // Start on the left. Hit the scale.
+ constexpr double kFullDriveLength = 9.95;
+ constexpr double kTurnDistance = 4.40;
+ StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kTurnDistance - 1.4)))
+ return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance))
+ return true;
+ StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchAutoIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.35, 0.0, kFinalSwitchDrive, kTurn);
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ //if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(1500));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1500));
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+ if (ShouldCancel()) return true;
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(-0.5, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ } break;
+
+ case 3:
+ case 2: {
+ // Start on the left. Hit the scale.
+ constexpr double kDriveDistance = 7.0;
+ // Drive.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(kDriveDistance - 1.0, M_PI / 2.0)) return true;
+ // Once we are away from the wall, start the arm.
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ SendSuperstructureGoal();
+
+ // We are starting to get close. Slow down for the turn.
+ if (!WaitForDriveNear(2.5, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
+
+ // Once we've gotten slowed down a bit, start turning.
+ if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
+ StartDrive(0.0, -M_PI / 4.0 - 0.1, kSlowDrive, kSweepingTurn);
+
+ // Get close and open the claw.
+ if (!WaitForDriveNear(0.25, 0.25)) return true;
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ StartDrive(0.25, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ } break;
}
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index b8d58e2..cf5766c 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -77,6 +77,25 @@
LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
+
+ bool WaitForArmTrajectoryClose(double threshold) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ superstructure_queue.status.FetchLatest();
+ if (superstructure_queue.status.get()) {
+ if (superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go < threshold) {
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
};
} // namespace actors
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 6d7b628..aa91137 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -386,8 +386,8 @@
]
-tall_box_x = 0.401
-tall_box_y = 0.14
+tall_box_x = 0.411
+tall_box_y = 0.125
short_box_x = 0.431
short_box_y = 0.082
@@ -401,17 +401,19 @@
# TODO(austin): Drive the front/back off the same numbers a bit better.
front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(
+ 0.700, 2.125, circular_index=-1.000000)
front_middle2_box = to_theta_with_circular_index(
- 0.732, 2.268, circular_index=-1)
+ 0.700, 2.268, circular_index=-1)
front_middle1_box = to_theta_with_circular_index(
- 0.878, 1.885, circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.926, 1.542, circular_index=-1)
+ 0.800, 1.915, circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
back_middle2_box = to_theta_with_circular_index(
- -0.732, 2.268, circular_index=0)
+ -0.700, 2.268, circular_index=0)
back_middle1_box = to_theta_with_circular_index(
- -0.878, 1.885, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.926, 1.542, circular_index=0)
+ -0.800, 1.915, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.572, circular_index=0)
front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
@@ -420,15 +422,23 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+front_switch_auto = to_theta_with_circular_index(
+ 0.750, 2.20, circular_index=-1.000000)
+
+starting = numpy.array(
+ [numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array(
+ [numpy.pi / 2.0, -numpy.pi / 2.0])
+
self_hang = numpy.array(
[numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
partner_hang = numpy.array(
[numpy.pi / 2.0 - (-0.25), numpy.pi / 2.0])
above_hang = numpy.array(
- [numpy.pi / 2.0 - 0.008739, numpy.pi / 2.0 - (-0.101927)])
+ [numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
below_hang = numpy.array(
- [numpy.pi / 2.0 - 0.329954, numpy.pi / 2.0 - (-0.534816)])
+ [numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -452,6 +462,7 @@
(tall_box_grab, "TallBoxGrab"),
(short_box_grab, "ShortBoxGrab"),
(front_high_box, "FrontHighBox"),
+ (front_middle3_box, "FrontMiddle3Box"),
(front_middle2_box, "FrontMiddle2Box"),
(front_middle1_box, "FrontMiddle1Box"),
(front_low_box, "FrontLowBox"),
@@ -467,6 +478,9 @@
(below_hang, "BelowHang"),
(self_hang, "SelfHang"),
(partner_hang, "PartnerHang"),
+ (front_switch_auto, "FrontSwitchAuto"),
+ (starting, "Starting"),
+ (vertical_starting, "VerticalStarting"),
] # yapf: disable
# We need to define critical points so we can create paths connecting them.
@@ -494,9 +508,20 @@
AngleSegment(neutral, back_switch),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+ AngleSegment(starting, vertical_starting),
+ AngleSegment(vertical_starting, neutral),
+
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
+
+ XYSegment(front_middle3_box, up),
+ XYSegment(front_middle3_box, front_high_box),
+ XYSegment(front_middle3_box, front_middle2_box),
+ XYSegment(front_middle3_box, front_middle1_box),
+
+ XYSegment(neutral, front_switch_auto),
+
XYSegment(up, front_middle1_box),
XYSegment(up, front_low_box),
XYSegment(front_high_box, front_middle2_box),
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index f69bab3..4912359 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -336,11 +336,13 @@
}
}
- follower_.Update(
- arm_ekf_.X_hat(), disable, kDt(), kVMax(),
- close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
- LOG(INFO, "Max voltage: %f\n",
- close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ const double max_operating_voltage =
+ close_enough_for_full_power_
+ ? kOperatingVoltage()
+ : (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ max_operating_voltage);
+ LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
status->goal_theta0 = follower_.theta(0);
status->goal_theta1 = follower_.theta(1);
status->goal_omega0 = follower_.omega(0);
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 4609827..840ce53 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -31,6 +31,7 @@
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
+ static constexpr double kGotoPathVMax() { return 12.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
@@ -71,7 +72,7 @@
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
- bool NearEnd(double threshold = 0.02) const {
+ bool NearEnd(double threshold = 0.03) const {
return ::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= threshold &&
::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= threshold &&
follower_.path_distance_to_go() < 1e-3;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 37fb4f9..e22219e 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -37,15 +37,15 @@
const double left_intake_goal = ::std::min(
arm_.max_intake_override(),
(unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
+ const double right_intake_goal = ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
+
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
- const double right_intake_goal = ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
-
intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
&(position->right_intake),
output != nullptr ? &(output->right_intake) : nullptr,
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 81791e5..a828048 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -35,6 +35,7 @@
namespace arm = ::y2018::control_loops::superstructure::arm;
const ButtonLocation kIntakeClosed(4, 1);
+const ButtonLocation kSmallBox(4, 4);
const ButtonLocation kIntakeIn(3, 16);
const ButtonLocation kIntakeOut(4, 3);
@@ -56,7 +57,7 @@
const ButtonLocation kArmAboveHang(3, 7);
const ButtonLocation kArmBelowHang(3, 2);
-const ButtonLocation kWinch(3, 5);
+const ButtonLocation kWinch(3, 4);
const ButtonLocation kArmNeutral(3, 13);
const ButtonLocation kArmUp(3, 9);
@@ -65,9 +66,6 @@
const ButtonLocation kClawOpen(3, 1);
-const ButtonLocation kForkDeploy(3, 11);
-const ButtonLocation kForkStow(3, 10);
-
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
class Reader : public ::aos::input::JoystickInput {
@@ -129,26 +127,14 @@
return;
}
- if (data.IsPressed(kIntakeClosed)) {
- // Deploy the intake.
- if (superstructure_queue.position->box_back_beambreak_triggered) {
- intake_goal_ = 0.30;
- } else {
- intake_goal_ = 0.07;
- }
- } else {
- // Bring in the intake.
- intake_goal_ = -3.3;
- }
-
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->intake.left_intake_angle = intake_goal_;
new_superstructure_goal->intake.right_intake_angle = intake_goal_;
- if (data.IsPressed(kIntakeIn) || data.IsPressed(kArmPickupBoxFromIntake)) {
+ if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage = 8.0;
+ new_superstructure_goal->intake.roller_voltage = 7.5;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
new_superstructure_goal->intake.roller_voltage = -12.0;
@@ -157,6 +143,52 @@
new_superstructure_goal->intake.roller_voltage = 0.0;
}
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ SendColors(0.0, 0.5, 0.0);
+ } else if (superstructure_queue.position->box_distance < 0.2) {
+ SendColors(0.0, 0.0, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+
+ if (data.IsPressed(kSmallBox)) {
+ // Deploy the intake.
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ intake_goal_ = 0.30;
+ } else {
+ if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
+ superstructure_queue.position->box_distance < 0.15) {
+ intake_goal_ = 0.18;
+ } else {
+ intake_goal_ = -0.60;
+ }
+ }
+ } else if (data.IsPressed(kIntakeClosed)) {
+ // Deploy the intake.
+ if (superstructure_queue.position->box_back_beambreak_triggered) {
+ intake_goal_ = 0.30;
+ } else {
+ if (new_superstructure_goal->intake.roller_voltage > 0.1) {
+ if (superstructure_queue.position->box_distance < 0.15) {
+ intake_goal_ = 0.23;
+ } else if (superstructure_queue.position->box_distance < 0.20) {
+ intake_goal_ = 0.13;
+ } else if (superstructure_queue.position->box_distance < 0.25) {
+ intake_goal_ = -0.05;
+ } else if (superstructure_queue.position->box_distance < 0.28) {
+ intake_goal_ = -0.20;
+ } else {
+ intake_goal_ = -0.40;
+ }
+ } else {
+ intake_goal_ = -0.60;
+ }
+ }
+ } else {
+ // Bring in the intake.
+ intake_goal_ = -3.3;
+ }
+
// If we are disabled, stay at the node closest to where we start. This
// should remove long motions when enabled.
if (!data.GetControlBit(ControlBit::kEnabled)) {
@@ -173,19 +205,20 @@
arm_goal_position_ = arm::UpIndex();
} else if (data.IsPressed(kArmFrontSwitch)) {
arm_goal_position_ = arm::FrontSwitchIndex();
- } else if (data.IsPressed(kArmFrontHighBox) ||
- data.IsPressed(kArmFrontExtraHighBox)) {
+ } else if (data.IsPressed(kArmFrontExtraHighBox)) {
arm_goal_position_ = arm::FrontHighBoxIndex();
- } else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ } else if (data.IsPressed(kArmFrontHighBox)) {
arm_goal_position_ = arm::FrontMiddle2BoxIndex();
+ } else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ arm_goal_position_ = arm::FrontMiddle3BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle1Box)) {
arm_goal_position_ = arm::FrontMiddle1BoxIndex();
} else if (data.IsPressed(kArmFrontLowBox)) {
arm_goal_position_ = arm::FrontLowBoxIndex();
- } else if (data.IsPressed(kArmBackHighBox) ||
- data.IsPressed(kArmBackExtraHighBox)) {
+ } else if (data.IsPressed(kArmBackExtraHighBox)) {
arm_goal_position_ = arm::BackHighBoxIndex();
- } else if (data.IsPressed(kArmBackMiddle2Box)) {
+ } else if (data.IsPressed(kArmBackHighBox) ||
+ data.IsPressed(kArmBackMiddle2Box)) {
arm_goal_position_ = arm::BackMiddle2BoxIndex();
} else if (data.IsPressed(kArmBackMiddle1Box)) {
arm_goal_position_ = arm::BackMiddle1BoxIndex();
@@ -228,11 +261,6 @@
new_superstructure_goal->open_claw = false;
}
- if (data.IsPressed(kForkDeploy)) {
- new_superstructure_goal->deploy_fork = true;
- } else if (data.IsPressed(kForkStow)) {
- new_superstructure_goal->deploy_fork = false;
- }
new_superstructure_goal->grab_box = grab_box;
LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
@@ -248,11 +276,14 @@
::frc971::autonomous::AutonomousActionParams params;
::frc971::autonomous::auto_mode.FetchLatest();
if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
+ params.mode = ::frc971::autonomous::auto_mode->mode << 2;
} else {
LOG(WARNING, "no auto mode values\n");
params.mode = 0;
}
+ // TODO(austin): use the mode later if we care. We don't care right now.
+ params.mode = static_cast<int>(::aos::joystick_state->switch_left) |
+ (static_cast<int>(::aos::joystick_state->scale_left) << 1);
action_queue_.EnqueueAction(
::frc971::autonomous::MakeAutonomousAction(params));
}