Merge changes Id8c2db11,I5d0e43d8,I9e1cadea
* changes:
Move ADC code into individual boards where it belongs
Move all the motors stuff into a saner namespace
Name big motor controller files appropriately
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index a31d2f3..edbe406 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -29,11 +29,21 @@
const ProfileParameters kFinalSwitchDrive = {0.5, 0.5};
const ProfileParameters kDrive = {5.0, 2.5};
+const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
const ProfileParameters kSlowDrive = {1.5, 2.5};
+const ProfileParameters kScaleTurnDrive = {3.0, 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};
+const ProfileParameters kReallyFastTurn = {1.5, 9.0};
+
+const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
+const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
+
+const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.3};
+
+const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
} // namespace
@@ -80,74 +90,284 @@
} 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);
+ StartDrive(-3.2, 0.0, kDrive, kTurn);
if (!WaitForDriveProfileDone()) return true;
+ } break;
+ case 200:
+ {
+ constexpr bool kDriveBehind = true;
+ if (kDriveBehind) {
+ // Start on the left. Hit the switch.
+ 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);
- // Now, close so let's move the arm up.
- set_arm_goal_position(arm::FrontSwitchAutoIndex());
- SendSuperstructureGoal();
+ 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;
- StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
- if (!WaitForTurnProfileDone()) return true;
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchAutoIndex());
+ SendSuperstructureGoal();
- 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));
+ StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
- set_open_claw(true);
- SendSuperstructureGoal();
+ 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));
- ::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;
+ 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;
+ } else {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 5.55;
+ constexpr double kTurnDistance = 0.35;
+ StartDrive(-kFullDriveLength, 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::FrontSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, -M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_max_drivetrain_voltage(10.0);
+ StartDrive(1.1, 0.0, kDrive, kTurn);
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ if (!WaitForDriveNear(0.6, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ if (!WaitForDriveNear(0.3, M_PI / 2.0)) return true;
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+ 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;
+ constexpr double kDriveDistance = 6.95;
+ // Distance and angle to do the big drive to the third cube.
+ constexpr double kThirdCubeDrive = 1.57;
+ constexpr double kThirdCubeTurn = M_PI / 4.0 - 0.1;
+ // Angle to do the slow pickup turn on the third box.
+ constexpr double kThirdBoxEndTurnAngle = 0.20;
+
+ // Distance to drive back to the scale with the third cube.
+ constexpr double kThirdCubeDropDrive = kThirdCubeDrive + 0.30;
+
// 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());
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
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);
+ if (!WaitForDriveNear(4.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kScaleTurnDrive, kFastTurn);
// 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);
+ if (!WaitForDriveNear(3.25, M_PI / 2.0)) return true;
+ StartDrive(0.0, -M_PI / 6.0, kScaleTurnDrive, kFastTurn);
+
+ if (!WaitForDriveNear(1.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, M_PI / 6.0, kScaleTurnDrive, kFastTurn);
// Get close and open the claw.
- if (!WaitForDriveNear(0.25, 0.25)) return true;
+ if (!WaitForDriveNear(0.15, 0.25)) return true;
set_open_claw(true);
SendSuperstructureGoal();
+ set_intake_angle(-0.40);
+ LOG(INFO, "Dropped first box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(500));
- StartDrive(0.25, 0.0, kDrive, kTurn);
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(200));
+
+ LOG(INFO, "Starting second box drive %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ constexpr double kSecondBoxSwerveAngle = 0.35;
+ constexpr double kSecondBoxDrive = 1.43;
+ StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
+ if (!WaitForDriveNear(kSecondBoxDrive - 0.2, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, kSecondBoxSwerveAngle, kDrive, kFastTurn);
+ if (!WaitForDriveNear(0.5, M_PI / 2.0)) return true;
+
+ set_open_claw(true);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, -kSecondBoxSwerveAngle, kDrive, kFastTurn);
+
if (!WaitForDriveProfileDone()) return true;
+ set_intake_angle(0.10);
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ set_open_claw(false);
+
+ set_roller_voltage(10.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Grabbing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForBoxGrabed()) return true;
+
+ LOG(INFO, "Got second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ ::std::this_thread::sleep_for(chrono::milliseconds(600));
+
+ set_grab_box(false);
+ set_arm_goal_position(arm::UpIndex());
+ set_roller_voltage(0.0);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+ LOG(INFO, "Driving to place second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(-kSecondBoxDrive + 0.2, kSecondBoxSwerveAngle, kDrive,
+ kFastTurn);
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+
+ constexpr double kSecondBoxEndExtraAngle = 0.3;
+ StartDrive(0.0, -kSecondBoxSwerveAngle - kSecondBoxEndExtraAngle, kDrive,
+ kFastTurn);
+
+ LOG(INFO, "Starting throw %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ SendSuperstructureGoal();
+
+ // Throw the box.
+ if (!WaitForArmTrajectoryClose(0.03)) return true;
+
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ LOG(INFO, "Releasing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(700));
+ set_open_claw(false);
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Driving to third box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
+ kFastTurn);
+ if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
+
+ StartDrive(0.0, kThirdCubeTurn, kThirdBoxDrive, kFastTurn);
+ if (!WaitForDriveNear(0.3, M_PI / 4.0)) return true;
+
+ set_intake_angle(0.05);
+ set_roller_voltage(9.0);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.30, kThirdBoxEndTurnAngle, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ if (!WaitForBoxGrabed()) return true;
+ LOG(INFO, "Third box grabbed %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ const bool too_late =
+ monotonic_clock::now() > start_time + chrono::milliseconds(12000);
+ if (too_late) {
+ LOG(INFO, "Third box too long, going up. %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_grab_box(false);
+ set_arm_goal_position(arm::UpIndex());
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+ }
+ ::std::this_thread::sleep_for(chrono::milliseconds(600));
+
+ set_grab_box(false);
+ if (!too_late) {
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
+ }
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ StartDrive(-kThirdCubeDropDrive, 0.0,
+ kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForDriveNear(1.40, M_PI / 4.0)) return true;
+ StartDrive(0.0, -kThirdCubeTurn - 0.2 - kThirdBoxEndTurnAngle - 0.3,
+ kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForArmTrajectoryClose(0.005)) return true;
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+
+ if (!too_late) {
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Final open %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ }
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(14750) -
+ (monotonic_clock::now() - start_time));
+
+ set_arm_goal_position(arm::UpIndex());
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(15000) -
+ (monotonic_clock::now() - start_time));
+
+ set_close_claw(true);
+ SendSuperstructureGoal();
+
} break;
}
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index cf5766c..a292531 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -15,6 +15,7 @@
namespace y2018 {
namespace actors {
using ::y2018::control_loops::superstructure_queue;
+using ::frc971::control_loops::drivetrain_queue;
namespace arm = ::y2018::control_loops::superstructure::arm;
@@ -32,7 +33,9 @@
arm_goal_position_ = arm::NeutralIndex();
grab_box_ = false;
open_claw_ = false;
+ close_claw_ = false;
deploy_fork_ = false;
+ disable_box_correct_ = false;
InitializeEncoders();
ResetDrivetrain();
SendSuperstructureGoal();
@@ -44,11 +47,17 @@
uint32_t arm_goal_position_ = arm::NeutralIndex();
bool grab_box_ = false;
bool open_claw_ = false;
+ bool close_claw_ = false;
bool deploy_fork_ = false;
+ bool disable_box_correct_ = false;
void set_roller_voltage(double roller_voltage) {
roller_voltage_ = roller_voltage;
}
+ void set_intake_angle(double intake_angle) {
+ set_left_intake_angle(intake_angle);
+ set_right_intake_angle(intake_angle);
+ }
void set_left_intake_angle(double left_intake_angle) {
left_intake_angle_ = left_intake_angle;
}
@@ -60,8 +69,13 @@
}
void set_grab_box(bool grab_box) { grab_box_ = grab_box; }
void set_open_claw(bool open_claw) { open_claw_ = open_claw; }
+ void set_close_claw(bool close_claw) { close_claw_ = close_claw; }
void set_deploy_fork(bool deploy_fork) { deploy_fork_ = deploy_fork; }
+ void set_disable_box_correct(bool disable_box_correct) {
+ disable_box_correct_ = disable_box_correct;
+ }
+
void SendSuperstructureGoal() {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->intake.roller_voltage = roller_voltage_;
@@ -71,6 +85,7 @@
new_superstructure_goal->arm_goal_position = arm_goal_position_;
new_superstructure_goal->grab_box = grab_box_;
new_superstructure_goal->open_claw = open_claw_;
+ new_superstructure_goal->close_claw = close_claw_;
new_superstructure_goal->deploy_fork = deploy_fork_;
if (!new_superstructure_goal.Send()) {
@@ -78,6 +93,67 @@
}
}
+ bool WaitForArmTrajectoryOrDriveClose(double drive_threshold,
+ double arm_threshold) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+
+ constexpr double kPositionTolerance = 0.02;
+ constexpr double kProfileTolerance = 0.001;
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ superstructure_queue.status.FetchLatest();
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get() && superstructure_queue.status.get()) {
+ const double left_profile_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->profiled_left_position_goal);
+ const double right_profile_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->profiled_right_position_goal);
+
+ const double left_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->estimated_left_position);
+ const double right_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->estimated_right_position);
+
+ const double profile_distance_to_go =
+ (left_profile_error + right_profile_error) / 2.0;
+
+ const double distance_to_go = (left_error + right_error) / 2.0;
+
+ // Check superstructure first.
+ if (superstructure_queue.status->arm.current_node ==
+ arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go <
+ arm_threshold) {
+ LOG(INFO, "Arm finished first: %f, drivetrain %f distance\n",
+ superstructure_queue.status->arm.path_distance_to_go,
+ ::std::abs(distance_to_go));
+ return true;
+ }
+
+ // Now check drivetrain.
+ if (::std::abs(profile_distance_to_go) <
+ drive_threshold + kProfileTolerance &&
+ ::std::abs(distance_to_go) < drive_threshold + kPositionTolerance) {
+ LOG(INFO,
+ "Drivetrain finished first: arm %f, drivetrain %f distance\n",
+ superstructure_queue.status->arm.path_distance_to_go,
+ ::std::abs(distance_to_go));
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
+
bool WaitForArmTrajectoryClose(double threshold) {
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
@@ -88,7 +164,8 @@
superstructure_queue.status.FetchLatest();
if (superstructure_queue.status.get()) {
- if (superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ if (superstructure_queue.status->arm.current_node ==
+ arm_goal_position_ &&
superstructure_queue.status->arm.path_distance_to_go < threshold) {
return true;
}
@@ -96,6 +173,24 @@
phased_loop.SleepUntilNext();
}
}
+
+ bool WaitForBoxGrabed() {
+ ::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.grab_state == 4) {
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
};
} // namespace actors
diff --git a/y2018/constants.cc b/y2018/constants.cc
index cef8979..8180bdc 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -122,9 +122,9 @@
arm_proximal->potentiometer_offset = -1.242 - 0.03 - 0.1;
arm_distal->zeroing.measured_absolute_position =
- 1.102987 - kDistalZeroingPosition + 0.12;
+ 1.102987 - kDistalZeroingPosition + 0.12 + 0.0095;
arm_distal->potentiometer_offset =
- 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226;
+ 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226 + 0.862067;
break;
default:
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 5559f7c..abe5ed3 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -117,6 +117,26 @@
% (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
h_file.append("}")
+ front_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.front_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> FrontPoints() {" %
+ len(front_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(front_points), ", ".join(front_points)))
+ h_file.append("}")
+
+ back_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.back_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> BackPoints() {" %
+ len(back_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(back_points), ", ".join(back_points)))
+ h_file.append("}")
+
# Add the Make*Path functions.
h_file.append("")
cc_file.append("")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 0807c79..b6fd3bd 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -386,7 +386,7 @@
for alpha in subdivide_spline(start, control1, control2, end)
])
- cr.move_to(self.start[0] + xy_end_circle_size, start[1])
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
cr.move_to(end[0] + xy_end_circle_size, end[1])
cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
@@ -556,19 +556,71 @@
front_switch_c1 = numpy.array([1.903841, -0.622351])
front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+sparse_front_points = [
+ (front_high_box, "FrontHighBox"),
+ (front_middle2_box, "FrontMiddle2Box"),
+ (front_middle3_box, "FrontMiddle3Box"),
+ (front_middle1_box, "FrontMiddle1Box"),
+ (front_low_box, "FrontLowBox"),
+ (front_switch, "FrontSwitch"),
+] # yapf: disable
+
+sparse_back_points = [
+ (back_high_box, "BackHighBox"),
+ (back_middle2_box, "BackMiddle2Box"),
+ (back_middle1_box, "BackMiddle1Box"),
+ (back_low_box, "BackLowBox"),
+] # yapf: disable
+
+def expand_points(points, max_distance):
+ """Expands a list of points to be at most max_distance apart
+
+ Generates the paths to connect the new points to the closest input points,
+ and the paths connecting the points.
+
+ Args:
+ points, list of tuple of point, name, The points to start with and fill
+ in.
+ max_distance, float, The max distance between two points when expanding
+ the graph.
+
+ Return:
+ points, edges
+ """
+ result_points = [points[0]]
+ result_paths = []
+ for point, name in points[1:]:
+ previous_point = result_points[-1][0]
+ previous_point_xy = get_xy(previous_point)
+ circular_index = get_circular_index(previous_point)
+
+ point_xy = get_xy(point)
+ norm = numpy.linalg.norm(point_xy - previous_point_xy)
+ num_points = int(numpy.ceil(norm / max_distance))
+ last_iteration_point = previous_point
+ for subindex in range(1, num_points):
+ subpoint = to_theta(
+ alpha_blend(previous_point_xy, point_xy,
+ float(subindex) / num_points),
+ circular_index=circular_index)
+ result_points.append((subpoint, '%s%dof%d' % (name, subindex,
+ num_points)))
+ result_paths.append(XYSegment(previous_point, subpoint))
+ if (last_iteration_point != previous_point).any():
+ result_paths.append(XYSegment(last_iteration_point, subpoint))
+ result_paths.append(XYSegment(subpoint, point))
+ last_iteration_point = subpoint
+ result_points.append((point, name))
+
+ return result_points, result_paths
+
+front_points, front_paths = expand_points(sparse_front_points, 0.05)
+back_points, back_paths = expand_points(sparse_back_points, 0.05)
+
points = [(ready_above_box, "ReadyAboveBox"),
(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"),
- (back_high_box, "BackHighBox"),
- (back_middle2_box, "BackMiddle2Box"),
- (back_middle1_box, "BackMiddle1Box"),
- (back_low_box, "BackLowBox"),
- (front_switch, "FrontSwitch"),
(back_switch, "BackSwitch"),
(neutral, "Neutral"),
(up, "Up"),
@@ -580,7 +632,7 @@
(starting, "Starting"),
(duck, "Duck"),
(vertical_starting, "VerticalStarting"),
-] # yapf: disable
+] + front_points + back_points # yapf: disable
duck_c1 = numpy.array([1.337111, -1.721008])
duck_c2 = numpy.array([1.283701, -1.795519])
@@ -588,6 +640,9 @@
ready_to_up_c1 = numpy.array([1.792962, 0.198329])
ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
@@ -611,6 +666,7 @@
]
unnamed_segments = [
+ SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2, front_switch_auto),
SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
@@ -623,15 +679,11 @@
XYSegment(ready_above_box, front_middle1_box),
XYSegment(ready_above_box, front_middle2_box),
XYSegment(ready_above_box, front_middle3_box),
- XYSegment(ready_above_box, front_high_box),
- #XYSegment(ready_above_box, up),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, front_high_box),
AngleSegment(starting, vertical_starting),
AngleSegment(vertical_starting, neutral),
- # TODO(austin): Duck -> neutral with a theta spline.
- #AngleSegment(duck, vertical_starting),
-
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
@@ -641,8 +693,6 @@
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),
@@ -669,6 +719,6 @@
AngleSegment(up, below_hang),
AngleSegment(up, self_hang),
AngleSegment(up, partner_hang),
-]
+] + front_paths + back_paths
segments = named_segments + unnamed_segments
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d3997ca..d79a293 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -39,7 +39,7 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- const control_loops::ArmPosition *position,
+ bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
@@ -59,9 +59,20 @@
if (open_claw) {
claw_closed_ = false;
}
- if (outputs_disabled) {
+ if (close_claw) {
claw_closed_ = true;
}
+ if (outputs_disabled) {
+ if (claw_closed_count_ == 0) {
+ claw_closed_ = true;
+ } else {
+ --claw_closed_count_;
+ }
+ } else {
+ // Wait this many iterations before closing the claw. That prevents
+ // brownouts from closing the claw.
+ claw_closed_count_ = 50;
+ }
Y << position->proximal.encoder + proximal_offset_,
position->distal.encoder + distal_offset_;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d86b95b..6b2864a 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -35,7 +35,7 @@
static constexpr double kGotoPathVMax() { return 6.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- const control_loops::ArmPosition *position,
+ bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
const bool intake_clear_of_box, double *proximal_output,
@@ -114,6 +114,8 @@
// Start at the 0th index.
uint32_t current_node_ = 0;
+ uint32_t claw_closed_count_ = 0;
+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 2c36efa..e570c50 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -97,6 +97,8 @@
return controller_.output_position() < -0.1;
}
+ double output_position() const { return controller_.output_position(); }
+
private:
IntakeController controller_;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index e22219e..757b166 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -1,5 +1,7 @@
#include "y2018/control_loops/superstructure/superstructure.h"
+#include <chrono>
+
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
@@ -10,6 +12,10 @@
namespace control_loops {
namespace superstructure {
+using ::aos::monotonic_clock;
+
+namespace chrono = ::std::chrono;
+
namespace {
// The maximum voltage the intake roller will be allowed to use.
constexpr double kMaxIntakeRollerVoltage = 12.0;
@@ -34,12 +40,31 @@
arm_.Reset();
}
- 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));
+ const double clipped_box_distance =
+ ::std::min(1.0, ::std::max(0.0, position->box_distance));
+
+ const double box_velocity =
+ (clipped_box_distance - last_box_distance_) / 0.005;
+
+ constexpr double kFilteredBoxVelocityAlpha = 0.02;
+ filtered_box_velocity_ =
+ box_velocity * kFilteredBoxVelocityAlpha +
+ (1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
+ status->filtered_box_velocity = filtered_box_velocity_;
+
+ constexpr double kCenteringAngleGain = 0.0;
+ const double left_intake_goal =
+ ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0
+ : unsafe_goal->intake.left_intake_angle)) +
+ last_intake_center_error_ * kCenteringAngleGain;
+ const double right_intake_goal =
+ ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0
+ : unsafe_goal->intake.right_intake_angle)) -
+ last_intake_center_error_ * kCenteringAngleGain;
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
@@ -51,13 +76,18 @@
output != nullptr ? &(output->right_intake) : nullptr,
&(status->right_intake));
+ const double intake_center_error =
+ intake_right_.output_position() - intake_left_.output_position();
+ last_intake_center_error_ = intake_center_error;
+
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
arm_.Iterate(
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
- unsafe_goal != nullptr ? unsafe_goal->open_claw : false, &(position->arm),
- position->claw_beambreak_triggered,
+ unsafe_goal != nullptr ? unsafe_goal->open_claw : false,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
+ &(position->arm), position->claw_beambreak_triggered,
position->box_back_beambreak_triggered, intake_clear_of_box,
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
@@ -87,16 +117,37 @@
double roller_voltage = ::std::max(
-kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
kMaxIntakeRollerVoltage));
- constexpr int kReverseTime = 15;
- if (unsafe_goal->intake.roller_voltage < 0.0) {
+ constexpr int kReverseTime = 14;
+ if (unsafe_goal->intake.roller_voltage < 0.0 ||
+ unsafe_goal->disable_box_correct) {
output->left_intake.voltage_rollers = roller_voltage;
output->right_intake.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
+ stuck_count_ = 0;
} else {
+ monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ const bool stuck = position->box_distance < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered;
+ // Make sure we don't declare ourselves re-stuck too quickly. We want to
+ // wait 400 ms before triggering the stuck condition again.
+ if (!stuck) {
+ last_unstuck_time_ = monotonic_now;
+ }
+ if (monotonic_now < last_stuck_time_ + chrono::milliseconds(400)) {
+ last_unstuck_time_ = monotonic_now;
+ }
+
switch (rotation_state_) {
case RotationState::NOT_ROTATING:
- if (position->left_intake.beam_break) {
+ if (stuck &&
+ monotonic_now > last_stuck_time_ + chrono::milliseconds(400) &&
+ monotonic_now > last_unstuck_time_ + chrono::milliseconds(100)) {
+ rotation_state_ = RotationState::STUCK;
+ ++stuck_count_;
+ last_stuck_time_ = monotonic_now;
+ } else if (position->left_intake.beam_break) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
@@ -107,6 +158,13 @@
} else {
break;
}
+ case RotationState::STUCK: {
+ // Latch being stuck for 80 ms so we kick the box out far enough.
+ if (last_stuck_time_ + chrono::milliseconds(80) < monotonic_now) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ last_unstuck_time_ = monotonic_now;
+ }
+ } break;
case RotationState::ROTATING_LEFT:
if (position->right_intake.beam_break) {
rotation_count_ = kReverseTime;
@@ -129,25 +187,57 @@
break;
}
- if (position->box_back_beambreak_triggered && roller_voltage > 0.0) {
- roller_voltage = 0;
+ constexpr double kHoldVoltage = 1.0;
+ constexpr double kStuckVoltage = 10.0;
+
+ if (position->box_back_beambreak_triggered &&
+ roller_voltage > kHoldVoltage) {
+ roller_voltage = kHoldVoltage;
}
switch (rotation_state_) {
- case RotationState::NOT_ROTATING:
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
- break;
+ case RotationState::NOT_ROTATING: {
+ double centering_gain = 13.0;
+ if (stuck_count_ > 1) {
+ if ((stuck_count_ - 1) % 2 == 0) {
+ centering_gain = 0.0;
+ }
+ }
+ output->left_intake.voltage_rollers =
+ roller_voltage - intake_center_error * centering_gain;
+ output->right_intake.voltage_rollers =
+ roller_voltage + intake_center_error * centering_gain;
+ } break;
+ case RotationState::STUCK: {
+ if (roller_voltage > kHoldVoltage) {
+ output->left_intake.voltage_rollers = -kStuckVoltage;
+ output->right_intake.voltage_rollers = -kStuckVoltage;
+ }
+ } break;
case RotationState::ROTATING_LEFT:
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = -roller_voltage;
+ if (position->left_intake.beam_break) {
+ output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ } else {
+ output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ }
+ output->right_intake.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = -roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ output->left_intake.voltage_rollers = roller_voltage;
+ if (position->right_intake.beam_break) {
+ output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ } else {
+ output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ }
break;
}
}
+ } else {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ rotation_count_ = 0;
+ stuck_count_ = 0;
}
+ status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ last_box_distance_ = clipped_box_distance;
}
} // namespace superstructure
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 2081e6c..ec0c9e1 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -36,14 +36,28 @@
intake::IntakeSide intake_right_;
arm::Arm arm_;
+ // The last centering error. This is the distance that the center of the two
+ // intakes is away from 0.
+ double last_intake_center_error_ = 0.0;
+ // The last distance that the box distance lidar measured.
+ double last_box_distance_ = 0.0;
+ // State variable for the box velocity low pass filter.
+ double filtered_box_velocity_ = 0.0;
+
enum class RotationState {
NOT_ROTATING = 0,
ROTATING_LEFT = 1,
- ROTATING_RIGHT = 2
+ ROTATING_RIGHT = 2,
+ STUCK = 3
};
RotationState rotation_state_ = RotationState::NOT_ROTATING;
int rotation_count_ = 0;
+ int stuck_count_ = 0;
+ ::aos::monotonic_clock::time_point last_stuck_time_ =
+ ::aos::monotonic_clock::min_time;
+ ::aos::monotonic_clock::time_point last_unstuck_time_ =
+ ::aos::monotonic_clock::min_time;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 9bdabed..e2cad73 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -132,12 +132,15 @@
bool grab_box;
bool open_claw;
+ bool close_claw;
bool deploy_fork;
bool hook_release;
double voltage_winch;
+
+ bool disable_box_correct;
};
message Status {
@@ -152,6 +155,9 @@
IntakeSideStatus right_intake;
ArmStatus arm;
+
+ double filtered_box_velocity;
+ uint32_t rotation_state;
};
message Position {
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index a828048..4e3103b 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -28,43 +28,51 @@
using ::aos::input::driver_station::POVLocation;
using ::aos::input::DrivetrainInputReader;
+using ::y2018::control_loops::superstructure::arm::FrontPoints;
+using ::y2018::control_loops::superstructure::arm::BackPoints;
+
namespace y2018 {
namespace input {
namespace joysticks {
namespace arm = ::y2018::control_loops::superstructure::arm;
-const ButtonLocation kIntakeClosed(4, 1);
-const ButtonLocation kSmallBox(4, 4);
+const ButtonLocation kIntakeClosed(3, 2);
+const ButtonLocation kDuck(3, 9);
+const ButtonLocation kSmallBox(3, 1);
-const ButtonLocation kIntakeIn(3, 16);
-const ButtonLocation kIntakeOut(4, 3);
+const ButtonLocation kIntakeIn(3, 4);
+const ButtonLocation kIntakeOut(3, 3);
-const ButtonLocation kArmFrontHighBox(4, 5);
-const ButtonLocation kArmFrontExtraHighBox(3, 8);
-const ButtonLocation kArmFrontMiddle2Box(4, 7);
-const ButtonLocation kArmFrontMiddle1Box(4, 9);
-const ButtonLocation kArmFrontLowBox(4, 11);
-const ButtonLocation kArmFrontSwitch(3, 14);
+const ButtonLocation kArmFrontHighBox(4, 11);
+const ButtonLocation kArmFrontExtraHighBox(4, 1);
+const ButtonLocation kArmFrontMiddle2Box(4, 9);
+const ButtonLocation kArmFrontMiddle1Box(4, 7);
+const ButtonLocation kArmFrontLowBox(4, 5);
+const ButtonLocation kArmFrontSwitch(3, 7);
-const ButtonLocation kArmBackHighBox(4, 6);
-const ButtonLocation kArmBackExtraHighBox(3, 10);
-const ButtonLocation kArmBackMiddle2Box(4, 8);
-const ButtonLocation kArmBackMiddle1Box(4, 10);
-const ButtonLocation kArmBackLowBox(4, 12);
-const ButtonLocation kArmBackSwitch(3, 12);
+const ButtonLocation kArmBackHighBox(4, 12);
+const ButtonLocation kArmBackExtraHighBox(3, 14);
+const ButtonLocation kArmBackMiddle2Box(4, 10);
+const ButtonLocation kArmBackMiddle1Box(4, 8);
+const ButtonLocation kArmBackLowBox(4, 6);
+const ButtonLocation kArmBackSwitch(3, 10);
-const ButtonLocation kArmAboveHang(3, 7);
-const ButtonLocation kArmBelowHang(3, 2);
+const ButtonLocation kArmAboveHang(3, 15);
+const ButtonLocation kArmBelowHang(3, 16);
-const ButtonLocation kWinch(3, 4);
+const ButtonLocation kWinch(4, 2);
-const ButtonLocation kArmNeutral(3, 13);
-const ButtonLocation kArmUp(3, 9);
+const ButtonLocation kArmNeutral(3, 8);
+const ButtonLocation kArmUp(3, 11);
-const ButtonLocation kArmPickupBoxFromIntake(3, 6);
+const ButtonLocation kArmStepUp(3, 13);
+const ButtonLocation kArmStepDown(3, 12);
-const ButtonLocation kClawOpen(3, 1);
+const ButtonLocation kArmPickupBoxFromIntake(4, 3);
+
+const ButtonLocation kClawOpen(4, 4);
+const ButtonLocation kDriverClawOpen(2, 4);
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
@@ -134,7 +142,7 @@
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage = 7.5;
+ new_superstructure_goal->intake.roller_voltage = 8.0;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
new_superstructure_goal->intake.roller_voltage = -12.0;
@@ -169,16 +177,15 @@
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) {
+ if (superstructure_queue.position->box_distance < 0.10) {
+ new_superstructure_goal->intake.roller_voltage -= 3.0;
+ intake_goal_ = 0.22;
+ } else if (superstructure_queue.position->box_distance < 0.17) {
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;
+ intake_goal_ = 0.05;
} else {
- intake_goal_ = -0.40;
+ intake_goal_ = -0.10;
}
} else {
intake_goal_ = -0.60;
@@ -189,16 +196,64 @@
intake_goal_ = -3.3;
}
+ if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
+ intake_goal_ > 0.0) {
+ if (superstructure_queue.position->box_distance < 0.10) {
+ new_superstructure_goal->intake.roller_voltage -= 3.0;
+ }
+ new_superstructure_goal->intake.roller_voltage += 3.0;
+ }
+
// 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)) {
+ if (!data.GetControlBit(ControlBit::kEnabled) || never_disabled_) {
arm_goal_position_ = superstructure_queue.status->arm.current_node;
+ never_disabled_ = false;
}
bool grab_box = false;
if (data.IsPressed(kArmPickupBoxFromIntake)) {
- arm_goal_position_ = arm::NeutralIndex();
grab_box = true;
+ }
+ const bool near_goal =
+ superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go < 1e-3;
+ if (data.IsPressed(kArmStepDown) && near_goal) {
+ uint32_t *front_point = ::std::find(
+ front_points_.begin(), front_points_.end(), arm_goal_position_);
+ uint32_t *back_point = ::std::find(
+ back_points_.begin(), back_points_.end(), arm_goal_position_);
+ if (front_point != front_points_.end()) {
+ ++front_point;
+ if (front_point != front_points_.end()) {
+ arm_goal_position_ = *front_point;
+ }
+ } else if (back_point != back_points_.end()) {
+ ++back_point;
+ if (back_point != back_points_.end()) {
+ arm_goal_position_ = *back_point;
+ }
+ }
+ } else if (data.IsPressed(kArmStepUp) && near_goal) {
+ const uint32_t *front_point = ::std::find(
+ front_points_.begin(), front_points_.end(), arm_goal_position_);
+ const uint32_t *back_point = ::std::find(
+ back_points_.begin(), back_points_.end(), arm_goal_position_);
+ if (front_point != front_points_.end()) {
+ if (front_point != front_points_.begin()) {
+ --front_point;
+ arm_goal_position_ = *front_point;
+ }
+ } else if (back_point != back_points_.end()) {
+ if (back_point != back_points_.begin()) {
+ --back_point;
+ arm_goal_position_ = *back_point;
+ }
+ }
+ } else if (data.PosEdge(kArmPickupBoxFromIntake)) {
+ arm_goal_position_ = arm::NeutralIndex();
+ } else if (data.IsPressed(kDuck)) {
+ arm_goal_position_ = arm::DuckIndex();
} else if (data.IsPressed(kArmNeutral)) {
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kArmUp)) {
@@ -246,6 +301,7 @@
}
if (data.IsPressed(kWinch)) {
+ LOG(INFO, "Winching\n");
new_superstructure_goal->voltage_winch = 12.0;
} else {
new_superstructure_goal->voltage_winch = 0.0;
@@ -255,7 +311,8 @@
new_superstructure_goal->arm_goal_position = arm_goal_position_;
- if (data.IsPressed(kClawOpen) || data.PosEdge(kArmPickupBoxFromIntake)) {
+ if ((data.IsPressed(kClawOpen) && data.IsPressed(kDriverClawOpen)) ||
+ data.PosEdge(kArmPickupBoxFromIntake)) {
new_superstructure_goal->open_claw = true;
} else {
new_superstructure_goal->open_claw = false;
@@ -298,8 +355,12 @@
bool was_running_ = false;
bool auto_running_ = false;
+ bool never_disabled_ = true;
- int arm_goal_position_ = 0;
+ uint32_t arm_goal_position_ = 0;
+
+ decltype(FrontPoints()) front_points_ = FrontPoints();
+ decltype(BackPoints()) back_points_ = BackPoints();
::aos::common::actions::ActionQueue action_queue_;
};
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index b644ca1..f204605 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -725,8 +725,10 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right_victor_->SetSpeed(-queue->right_voltage / 12.0);
+ drivetrain_left_victor_->SetSpeed(
+ ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
+ drivetrain_right_victor_->SetSpeed(
+ ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
}
virtual void Stop() override {