robot code work
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 1f3496d..823a8ed 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -170,12 +170,21 @@
LOG(INFO, "Stopping the drivetrain\n");
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(true)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .quickturn(false)
+ .Send();
+}
+
+void ResetDrivetrain() {
+ LOG(INFO, "resetting the drivetrain\n");
+ control_loops::drivetrain.goal.MakeWithBuilder()
+ .control_loop_driving(false)
.highgear(false)
.steering(0.0)
.throttle(0.0)
- .left_goal(left_initial_position)
- .right_goal(right_initial_position)
- .quickturn(false)
.Send();
}
@@ -318,8 +327,21 @@
// start with N discs in the indexer
void HandleAuto() {
LOG(INFO, "Handling auto mode\n");
+
double WRIST_UP;
+ const double WRIST_DOWN = -0.580;
+ const double WRIST_DOWN_TWO = WRIST_DOWN - 0.010;
+ const double ANGLE_ONE = 0.560;
+ const double ANGLE_TWO = 0.707;
+
+ ResetIndex();
+ SetWristGoal(1.0); // wrist must calibrate itself on power-up
+ SetAngle_AdjustGoal(ANGLE_TWO); // make it still move a bit
+ SetShooterVelocity(0.0); // or else it keeps spinning from last time
+ ResetDrivetrain();
+
::aos::time::SleepFor(::aos::time::Time::InSeconds(20));
+ if (ShouldExitAuto()) return;
::aos::robot_state.FetchLatest();
if (!::aos::robot_state.get() ||
@@ -329,9 +351,6 @@
}
WRIST_UP -= 0.4;
LOG(INFO, "Got constants\n");
- const double WRIST_DOWN = -0.633;
- const double ANGLE_ONE = 0.5434;
- const double ANGLE_TWO = 0.685;
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
@@ -343,7 +362,6 @@
right_initial_position =
control_loops::drivetrain.position->right_encoder;
- ResetIndex();
StopDrivetrain();
SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
@@ -407,16 +425,17 @@
if (ShouldExitAuto()) return;
WaitForIndex(); // ready to pick up discs
- static const double kDriveDistance = 3.2;
- static const double kFirstDrive = 0.3;
+ static const double kDriveDistance = 2.8;
+ static const double kFirstDrive = 0.27;
+ static const double kSecondShootDistance = 2.0;
SetDriveGoal(kFirstDrive, 0.6);
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
- SetDriveGoal(kDriveDistance - kFirstDrive, 0.6);
+ SetWristGoal(WRIST_DOWN_TWO);
+ SetDriveGoal(kDriveDistance - kFirstDrive, 2.0);
if (ShouldExitAuto()) return;
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ SetDriveGoal(kSecondShootDistance - kDriveDistance, 2.0);
PreloadIndex();
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.4));
- SetDriveGoal(-1.3);
if (ShouldExitAuto()) return;
WaitForAngle_Adjust();
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index df88a34..5514412 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -275,6 +275,7 @@
position->bottom_disc_negedge_wait_count;
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ last_top_disc_detect_ = position->top_disc_detect;
// The open positions for the upper is right here and isn't a hard edge.
upper_open_region_.Restart(wrist_loop_->Y(0, 0));
lower_open_region_.Restart(wrist_loop_->Y(0, 0));
@@ -289,6 +290,7 @@
position->bottom_disc_negedge_wait_count;
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ last_top_disc_detect_ = position->top_disc_detect;
// We can't really trust the open range any more if the crio rebooted.
upper_open_region_.Restart(wrist_loop_->Y(0, 0));
lower_open_region_.Restart(wrist_loop_->Y(0, 0));
@@ -302,6 +304,12 @@
wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
}
missing_position_count_ = 0;
+ if (last_top_disc_detect_ && position->top_disc_detect) {
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ }
+ if (!last_top_disc_detect_ && !position->top_disc_detect) {
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ }
} else {
++missing_position_count_;
}
@@ -554,7 +562,7 @@
if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
// It has been too long. The disc must be jammed.
LOG(ERROR, "Been way too long. Jammed disc?\n");
- intake_voltage = 0.0;
+ intake_voltage = -12.0;
transfer_voltage = -12.0;
}
}
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 697e129..4de09e0 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -139,9 +139,9 @@
angle_adjust_goal = 0.564;
#endif
// middle wheel on the back line (same as auto)
- shooter_goal->velocity = 400;
+ shooter_goal->velocity = 360;
wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.5434;
+ angle_adjust_goal = 0.586;
} else if (Pressed(2, 6)) {
// short shot
shooter_goal->velocity = 375;