more changes from Davis
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index d7bdd2a..15a8c74 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -195,7 +195,7 @@
const double epsilon = 0.01;
profile.set_maximum_acceleration(2.0);
- profile.set_maximum_velocity(2.0);
+ profile.set_maximum_velocity(2.5);
while (true) {
::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
@@ -222,7 +222,7 @@
}
// Drives forward while we can pick up discs up to max_distance (in meters).
-void DriveForwardPickUp(double max_distance) {
+void DriveForwardPickUp(double max_distance, double wrist_angle) {
LOG(INFO, "going to pick up at a max distance of %f\n", max_distance);
max_distance *= kDrivetrainCorrectionFactor;
@@ -251,12 +251,16 @@
} else if (::std::abs(driveTrainState(0, 0) - max_distance) < epsilon) {
LOG(INFO, "went the max distance; driving back\n");
driving_back = true;
+ profile.set_maximum_velocity(2.5);
+ SetWristGoal(wrist_angle);
}
if (control_loops::index_loop.status.FetchLatest()) {
if (control_loops::index_loop.status->hopper_disc_count >= 4) {
LOG(INFO, "done intaking; driving back\n");
driving_back = true;
+ profile.set_maximum_velocity(2.5);
+ SetWristGoal(wrist_angle);
}
} else {
LOG(WARNING, "getting index status failed\n");
@@ -289,8 +293,8 @@
// in drivetrain "meters"
const double kRobotWidth = 0.4544;
- profile.set_maximum_acceleration(1);
- profile.set_maximum_velocity(0.6);
+ profile.set_maximum_acceleration(1.5);
+ profile.set_maximum_velocity(0.8);
const double side_offset = kRobotWidth * radians / 2.0;
@@ -302,19 +306,19 @@
if (ShouldExitAuto()) return;
LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_initial_position + driveTrainState(0, 0),
- right_initial_position - driveTrainState(0, 0));
+ left_initial_position - driveTrainState(0, 0),
+ right_initial_position + driveTrainState(0, 0));
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(false)
- .left_goal(left_initial_position + driveTrainState(0, 0))
- .right_goal(right_initial_position - driveTrainState(0, 0))
- .left_velocity_goal(driveTrainState(1, 0))
- .right_velocity_goal(-driveTrainState(1, 0))
+ .left_goal(left_initial_position - driveTrainState(0, 0))
+ .right_goal(right_initial_position + driveTrainState(0, 0))
+ .left_velocity_goal(-driveTrainState(1, 0))
+ .right_velocity_goal(driveTrainState(1, 0))
.Send();
}
- left_initial_position += side_offset;
- right_initial_position -= side_offset;
+ left_initial_position -= side_offset;
+ right_initial_position += side_offset;
LOG(INFO, "Done moving\n");
}
@@ -333,7 +337,7 @@
LOG(INFO, "Got constants\n");
const double WRIST_DOWN = -0.633;
const double ANGLE_ONE = 0.5101;
- //const double ANGLE_TWO = 0.685;
+ //const double ANGLE_ONE = 0.70;
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
@@ -351,6 +355,7 @@
SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
SetAngle_AdjustGoal(ANGLE_ONE);
SetShooterVelocity(410.0);
+ //SetShooterVelocity(131);
WaitForIndexReset();
if (ShouldExitAuto()) return;
PreloadIndex(); // spin to top and put 1 disc into loader
@@ -368,7 +373,7 @@
const double kDistanceToCenterMeters = 3.11023;
const double kMaxPickupDistance = 2.5;
- const double kTurnToCenterDegrees = 73.2;
+ const double kTurnToCenterDegrees = 78.2;
// Drive back to the center line.
SetDriveGoal(-kDistanceToCenterMeters);
@@ -382,7 +387,7 @@
if (ShouldExitAuto()) return;
// Pick up at most 4 discs and drive at most kMaxPickupDistance.
- DriveForwardPickUp(kMaxPickupDistance);
+ DriveForwardPickUp(kMaxPickupDistance, WRIST_UP);
SetWristGoal(WRIST_UP);
DriveSpin(-kTurnToCenterDegrees * M_PI / 180.0);
@@ -391,6 +396,8 @@
SetDriveGoal(kDistanceToCenterMeters);
if (ShouldExitAuto()) return;
+ return;
+
ShootNDiscs(4);
if (ShouldExitAuto()) return;
}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 620d784..51cd641 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -224,23 +224,23 @@
VerifyNearGoal();
}
-// Tests that loosing the encoder for a second triggers a re-zero.
+// Tests that losing the encoder for 11 seconds triggers a re-zero.
TEST_F(AngleAdjustTest, RezeroWithMissingPos) {
my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
- for (int i = 0; i < 800; ++i) {
+ for (int i = 0; i < 1800; ++i) {
// After 3 seconds, simulate the encoder going missing.
// This should trigger a re-zero. To make sure it works, change the goal as
// well.
- if (i < 300 || i > 400) {
+ if (i < 300 || i > 1400) {
angle_adjust_motor_plant_.SendPositionMessage();
} else {
- if (i > 310) {
+ if (i > 1310) {
// Should be re-zeroing now.
EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
}
my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.5).Send();
}
- if (i == 430) {
+ if (i == 1430) {
EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
angle_adjust_motor_.is_moving_off());
}
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index a756d35..468dc13 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -276,19 +276,21 @@
if (position == NULL) {
LOG(WARNING, "no new pos given\n");
error_count_++;
- } else {
- error_count_ = 0;
}
if (error_count_ >= 4) {
output_enabled = false;
LOG(WARNING, "err_count is %d so disabling\n", error_count_);
- } else if ((::aos::time::Time::Now() - last_good_time_) > kRezeroTime) {
- LOG(WARNING, "err_count is %d so forcing a re-zero (or 1st time)\n",
- error_count_);
- state_ = UNINITIALIZED;
+
+ if ((::aos::time::Time::Now() - last_good_time_) > kRezeroTime) {
+ LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
+ error_count_);
+ state_ = UNINITIALIZED;
+ loop_->Reset();
+ }
}
- if (position) {
+ if (position != NULL) {
last_good_time_ = ::aos::time::Time::Now();
+ error_count_ = 0;
}
// Compute the absolute position of the wrist.
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 092ae11..ac8017f 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -119,9 +119,10 @@
shooter_goal->velocity = 0;
static double angle_adjust_goal = 0.42;
if (Pressed(2, 5)) {
- // long shot
- shooter_goal->velocity = 131;
- angle_adjust_goal = 0.70;
+ // middle wheel on the back line (same as auto)
+ shooter_goal->velocity = 410;
+ wrist_up_position = 1.23 - 0.4;
+ angle_adjust_goal = 0.5101;
} else if (Pressed(2, 3)) {
// medium shot
#if 0
@@ -130,13 +131,17 @@
angle_adjust_goal = 0.564;
#endif
// middle wheel on the back line (same as auto)
- shooter_goal->velocity = 410;
+ shooter_goal->velocity = 400;
wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.5101;
+ angle_adjust_goal = 0.5434;
} else if (Pressed(2, 6)) {
// short shot
shooter_goal->velocity = 375;
angle_adjust_goal = 0.7267;
+ } else if (Pressed(2, 9)) {
+ // pit shot
+ shooter_goal->velocity = 131;
+ angle_adjust_goal = 0.70;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();