merging in the new and improved USB code
It now uses isochronous like it should and gets packets every 10ms for the
control loops nicely. The gyro board code has also been reorganized so it is
much clearer and easier to maintain.
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index a5666a9..69c3121 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -9,16 +9,16 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::ZeroOutputs() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::Iterate() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::Iterate() {
// Temporary storage for printing out inputs and outputs.
char state[1024];
@@ -60,8 +60,10 @@
}
} else {
LOG(ERROR, "Never had a position.\n");
- ZeroOutputs();
- return;
+ if (fail_no_position) {
+ ZeroOutputs();
+ return;
+ }
}
}
if (position) {
@@ -112,8 +114,8 @@
status.Send();
}
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::Run() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::Run() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index 05f8902..6af7235 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -50,7 +50,7 @@
// If has_position is false, the control loop will always use NULL as the
// position and not check the queue. This is used for "loops" that control
// motors open loop.
-template <class T, bool has_position = true>
+template <class T, bool has_position = true, bool fail_no_position = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e7c48a5..13aeb51 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -421,8 +421,6 @@
SetDriveGoal(kDistanceToCenterMeters);
if (ShouldExitAuto()) return;
- return;
-
ShootNDiscs(4);
if (ShouldExitAuto()) return;
} else {
@@ -441,11 +439,11 @@
WaitForIndex(); // ready to pick up discs
// How long we're going to drive in total.
- static const double kDriveDistance = 2.8;
+ static const double kDriveDistance = 2.84;
// How long to drive slowly to pick up the 2 disks under the pyramid.
static const double kFirstDrive = 0.4;
// How long to drive slowly to pick up the last 2 disks.
- static const double kLastDrive = 0.3;
+ static const double kLastDrive = 0.34;
// How fast to drive when picking up disks.
static const double kPickupVelocity = 0.6;
// Where to take the second set of shots from.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index df822e2..3016db5 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -20,7 +20,7 @@
// For purposes of moving the end up/down by a certain amount, the wrist is 18
// inches long.
const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.168;
+const double kPracticeWristHallEffectStartAngle = 1.175;
const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 7b07247..244f3e5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -8,13 +8,13 @@
namespace control_loops {
class DrivetrainLoop
- : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ : public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+ : aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false>(
my_drivetrain) {}
protected:
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 779e6a9..6ac91b7 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -839,7 +839,7 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = false;
- disk_stuck_in_loader_ = false;
+ disc_stuck_in_loader_ = false;
if (loader_goal_ == LoaderGoal::GRAB ||
loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
if (goal->force_fire) {
@@ -899,7 +899,7 @@
break;
}
case LoaderState::LIFTING:
- LOG(DEBUG, "Loader LIFTING %d\n", loader_countdown_);
+ LOG(DEBUG, "Loader LIFTING %d %d\n", loader_countdown_, loader_timeout_);
// Lifting the disc.
loader_up_ = true;
disc_clamped_ = true;
@@ -918,8 +918,10 @@
++loader_timeout_;
if (loader_timeout_ > kLiftingTimeout) {
LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
- loader_state_ = LoaderState::LIFTED;
- disk_stuck_in_loader_ = true;
+ loader_state_ = LoaderState::LOWERING;
+ loader_countdown_ = kLoweringDelay;
+ loader_timeout_ = 0;
+ disc_stuck_in_loader_ = true;
} else {
break;
}
@@ -953,14 +955,14 @@
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
loader_timeout_ = 0;
- --hopper_disc_count_;
- ++shot_disc_count_;
case LoaderState::LOWERING:
- LOG(DEBUG, "Loader LOWERING %d\n", loader_countdown_);
+ LOG(DEBUG, "Loader LOWERING %d %d\n", loader_countdown_, loader_timeout_);
// Lowering the loader back down.
loader_up_ = false;
disc_clamped_ = false;
- disc_ejected_ = true;
+ // We don't want to eject if we're stuck because it will force the disc
+ // into the green loader wheel.
+ disc_ejected_ = disc_stuck_in_loader_ ? false : true;
if (position->loader_bottom) {
if (loader_countdown_ > 0) {
--loader_countdown_;
@@ -968,6 +970,8 @@
break;
} else {
loader_state_ = LoaderState::LOWERED;
+ --hopper_disc_count_;
+ ++shot_disc_count_;
}
} else {
// Restart the countdown if it bounces back up or something.
@@ -976,7 +980,7 @@
if (loader_timeout_ > kLoweringTimeout) {
LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
loader_state_ = LoaderState::LOWERED;
- disk_stuck_in_loader_ = true;
+ disc_stuck_in_loader_ = true;
} else {
break;
}
@@ -986,8 +990,8 @@
loader_up_ = false;
disc_ejected_ = false;
is_shooting_ = false;
- if (disk_stuck_in_loader_) {
- disk_stuck_in_loader_ = false;
+ if (disc_stuck_in_loader_) {
+ disc_stuck_in_loader_ = false;
disc_clamped_ = true;
loader_state_ = LoaderState::GRABBED;
} else {
@@ -1044,7 +1048,11 @@
if (output) {
output->intake_voltage = intake_voltage;
- output->transfer_voltage = transfer_voltage;
+ if (goal->override_transfer) {
+ output->transfer_voltage = goal->transfer_voltage;
+ } else {
+ output->transfer_voltage = transfer_voltage;
+ }
if (goal->override_index) {
output->index_voltage = goal->index_voltage;
} else {
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 0be4801..e6902d8 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -317,9 +317,9 @@
LoaderGoal loader_goal_;
LoaderState loader_state_;
int loader_countdown_, loader_timeout_;
- // Whether or not we (might have) failed to shoot a disk that's now (probably)
+ // Whether or not we (might have) failed to shoot a disc that's now (probably)
// still in the loader.
- bool disk_stuck_in_loader_;
+ bool disc_stuck_in_loader_;
// Current state of the pistons.
bool loader_up_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index b299b42..9530a4f 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -20,6 +20,8 @@
// If true, set the indexer voltage to index_voltage.
bool override_index;
double index_voltage;
+ bool override_transfer;
+ double transfer_voltage;
};
message Position {
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index d9db969..be4e188 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -52,6 +52,7 @@
const ButtonLocation kIntake(3, 10);
const ButtonLocation kForceFire(3, 12);
const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+const ButtonLocation kForceSpitOut(2, 11);
const ButtonLocation kDeployHangers(3, 1);
@@ -182,9 +183,10 @@
// pretend like no button is pressed
}
#endif
+ // This shot is from 30'.
shooter_goal->velocity = 360;
if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.596;
+ angle_adjust_goal = 0.630;
} else if (data.IsPressed(kMediumShot)) {
#if 0
shooter_goal->velocity = 375;
@@ -227,7 +229,9 @@
const bool index_up = data.IsPressed(kForceIndexUp);
const bool index_down = data.IsPressed(kForceIndexDown);
- index_goal->override_index = index_up || index_down;
+ const bool spit_out = data.IsPressed(kForceSpitOut);
+ index_goal->override_index = index_up || index_down || spit_out;
+ index_goal->override_transfer = spit_out;
if (index_up && index_down) {
index_goal->index_voltage = 0.0;
} else if (index_up) {
@@ -235,6 +239,10 @@
} else if (index_down) {
index_goal->index_voltage = -12.0;
}
+ if (spit_out) {
+ index_goal->index_voltage = -12.0;
+ index_goal->transfer_voltage = -12.0;
+ }
index_goal.Send();
shooter_goal.Send();