various small tweaks/fixes from Madtown
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 97c2310..8b8159c 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -4,6 +4,7 @@
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/network/team_number.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
@@ -381,7 +382,7 @@
WaitForAngle_Adjust();
ShootIndex(); // tilt up, shoot, repeat until empty
// calls WaitForShooter
- ShootNDiscs(3); // ShootNDiscs returns if ShouldExitAuto
+ ShootNDiscs(4); // ShootNDiscs returns if ShouldExitAuto
if (ShouldExitAuto()) return;
StartIndex(); // take in up to 4 discs
@@ -437,20 +438,29 @@
static const double kLastDrive = 0.34;
// How fast to drive when picking up disks.
static const double kPickupVelocity = 0.6;
+ // How fast to drive when not picking up disks.
+ static const double kDriveVelocity =
+ constants::GetValues().clutch_transmission ? 0.8 : 1.75;
// Where to take the second set of shots from.
static const double kSecondShootDistance = 2.0;
// Go slowly to grab the 2 disks under the pyramid.
SetDriveGoal(kFirstDrive, kPickupVelocity);
::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
- SetDriveGoal(kDriveDistance - kFirstDrive - kLastDrive, 2.0);
- SetWristGoal(WRIST_DOWN_TWO);
- // Go slowly at the end to pick up the last 2 disks.
- SetDriveGoal(kLastDrive, kPickupVelocity);
- if (ShouldExitAuto()) return;
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
- SetDriveGoal(kSecondShootDistance - kDriveDistance, 2.0);
+ if (constants::GetValues().clutch_transmission) {
+ SetDriveGoal(kSecondShootDistance - kFirstDrive, kDriveVelocity);
+ } else {
+ SetDriveGoal(kDriveDistance - kFirstDrive - kLastDrive, kDriveVelocity);
+ SetWristGoal(WRIST_DOWN_TWO);
+ // Go slowly at the end to pick up the last 2 disks.
+ SetDriveGoal(kLastDrive, kPickupVelocity);
+ if (ShouldExitAuto()) return;
+
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
+ SetDriveGoal(kSecondShootDistance - kDriveDistance, kDriveVelocity);
+ }
+
PreloadIndex();
if (ShouldExitAuto()) return;
diff --git a/frc971/constants.cc b/frc971/constants.cc
index d062c77..da2d593 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -24,7 +24,7 @@
// It has about 0.029043 of gearbox slop.
// For purposes of moving the end up/down by a certain amount, the wrist is 18
// inches long.
-const double kCompWristHallEffectStartAngle = 1.27;
+const double kCompWristHallEffectStartAngle = 1.277;
const double kPracticeWristHallEffectStartAngle = 1.178;
const double kWristHallEffectStopAngle = 100 * M_PI / 180.0;
@@ -50,7 +50,7 @@
// wire guide is 0.773652098 radians.
const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
-const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305172594, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.297, 1.5};
const double kAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
@@ -70,7 +70,7 @@
const double kAngleAdjustZeroingOffSpeed = -0.5;
const double kPracticeAngleAdjustDeadband = 0.4;
-const double kCompAngleAdjustDeadband = 0.65;
+const double kCompAngleAdjustDeadband = 0.0;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 921fab0..71af487 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -106,7 +106,6 @@
_gyro = gyro;
_control_loop_driving = control_loop_driving;
SetRawPosition(left, right);
- //LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
}
void Update(bool update_observer, bool stop_motors) {
@@ -524,8 +523,6 @@
}
void SendMotors(Drivetrain::Output *output) {
- LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
- loop_->U(0, 0), loop_->U(1, 0), wheel_, throttle_);
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -738,6 +735,7 @@
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
if (gyro.FetchLatest()) {
+ LOG(DEBUG, "gyro %f\n", gyro->angle);
dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
control_loop_driving);
} else {
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index b36d517..9a03657 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -110,7 +110,7 @@
/*static*/ const int IndexMotor::kGrabbingDelay = 5;
/*static*/ const int IndexMotor::kLiftingDelay = 5;
/*static*/ const int IndexMotor::kLiftingTimeout = 100;
-/*static*/ const int IndexMotor::kShootingDelay = 10;
+/*static*/ const int IndexMotor::kShootingDelay = 25;
/*static*/ const int IndexMotor::kLoweringDelay = 4;
/*static*/ const int IndexMotor::kLoweringTimeout = 120;
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 0fbe33e..26dfc91 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -263,7 +263,7 @@
template <int kNumZeroSensors>
const ::aos::time::Time ZeroedJoint<kNumZeroSensors>::kRezeroTime =
- ::aos::time::Time::InSeconds(10);
+ ::aos::time::Time::InSeconds(2);
template <int kNumZeroSensors>
/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index fcfabb2..ffe538d 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -93,7 +93,7 @@
bad_gyro_ = true;
gyro.MakeWithBuilder().angle(0).Send();
} else if (data()->old_gyro_reading) {
- LOG(WARNING, "old gyro reading\n");
+ LOG(WARNING, "old/bad/uninitialized gyro reading\n");
bad_gyro_ = true;
} else {
bad_gyro_ = false;