deleted all of the 2013 robot-specific code
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 8b8159c..ea2a74b 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -9,10 +9,6 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
using ::aos::time::Time;
@@ -30,155 +26,6 @@
return ans;
}
-void SetShooterVelocity(double velocity) {
- LOG(INFO, "Setting shooter velocity to %f\n", velocity);
- control_loops::shooter.goal.MakeWithBuilder()
- .velocity(velocity).Send();
-}
-
-void SetWristGoal(double goal) {
- LOG(INFO, "Setting wrist to %f\n", goal);
- control_loops::wrist.goal.MakeWithBuilder()
- .goal(goal).Send();
-}
-
-void SetAngle_AdjustGoal(double goal) {
- LOG(INFO, "Setting angle adjust to %f\n", goal);
- control_loops::angle_adjust.goal.MakeWithBuilder()
- .goal(goal).Send();
-}
-
-void StartIndex() {
- LOG(INFO, "Starting index\n");
- control_loops::index_loop.goal.MakeWithBuilder()
- .goal_state(2)
- .force_fire(false)
- .override_index(false)
- .Send();
-}
-
-void PreloadIndex() {
- LOG(INFO, "Preloading index\n");
- control_loops::index_loop.goal.MakeWithBuilder()
- .goal_state(3)
- .force_fire(false)
- .override_index(false)
- .Send();
-}
-
-void ShootIndex() {
- LOG(INFO, "Shooting index\n");
- control_loops::index_loop.goal.MakeWithBuilder()
- .goal_state(4)
- .force_fire(false)
- .override_index(false)
- .Send();
-}
-
-void ResetIndex() {
- LOG(INFO, "Resetting index\n");
- control_loops::index_loop.goal.MakeWithBuilder()
- .goal_state(5)
- .force_fire(false)
- .override_index(false)
- .Send();
-}
-
-void WaitForIndexReset() {
- LOG(INFO, "Waiting for the indexer to reset\n");
- control_loops::index_loop.status.FetchLatest();
-
- // Fetch a couple index status packets to make sure that the indexer has run.
- for (int i = 0; i < 5; ++i) {
- LOG(DEBUG, "Fetching another index status packet\n");
- control_loops::index_loop.status.FetchNextBlocking();
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Indexer is now reset.\n");
-}
-
-void WaitForWrist() {
- LOG(INFO, "Waiting for the wrist\n");
- control_loops::wrist.status.FetchLatest();
-
- while (!control_loops::wrist.status.get()) {
- LOG(WARNING, "No previous wrist packet, trying to fetch again\n");
- control_loops::wrist.status.FetchNextBlocking();
- }
-
- while (!control_loops::wrist.status->done) {
- control_loops::wrist.status.FetchNextBlocking();
- LOG(DEBUG, "Got a new wrist status packet\n");
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Done waiting for the wrist\n");
-}
-void WaitForIndex() {
- LOG(INFO, "Waiting for the indexer to be ready to intake\n");
- control_loops::index_loop.status.FetchLatest();
-
- while (!control_loops::index_loop.status.get()) {
- LOG(WARNING, "No previous index packet, trying to fetch again\n");
- control_loops::index_loop.status.FetchNextBlocking();
- }
-
- while (!control_loops::index_loop.status->ready_to_intake) {
- control_loops::index_loop.status.FetchNextBlocking();
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Indexer ready to intake\n");
-}
-
-void WaitForAngle_Adjust() {
- LOG(INFO, "Waiting for the angle adjuster to finish\n");
- control_loops::angle_adjust.status.FetchLatest();
-
- while (!control_loops::angle_adjust.status.get()) {
- LOG(WARNING, "No previous angle adjust packet, trying to fetch again\n");
- control_loops::angle_adjust.status.FetchNextBlocking();
- }
-
- while (!control_loops::angle_adjust.status->done) {
- control_loops::angle_adjust.status.FetchNextBlocking();
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Angle adjuster done\n");
-}
-
-void WaitForShooter() {
- LOG(INFO, "Waiting for the shooter to spin up\n");
- control_loops::shooter.status.FetchLatest();
-
- while (!control_loops::shooter.status.get()) {
- LOG(WARNING, "No previous shooteracket, trying to fetch again\n");
- control_loops::shooter.status.FetchNextBlocking();
- }
-
- while (!control_loops::shooter.status->ready) {
- control_loops::shooter.status.FetchNextBlocking();
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Shooter ready to shoot\n");
-}
-
-void ShootNDiscs(int n) {
- LOG(INFO, "Waiting until %d discs have been shot\n", n);
- control_loops::index_loop.status.FetchLatest();
-
- while (!control_loops::index_loop.status.get()) {
- LOG(WARNING, "No previous index_loop packet, trying to fetch again\n");
- control_loops::index_loop.status.FetchNextBlocking();
- }
-
- int final_disc_count = control_loops::index_loop.status->shot_disc_count + n;
- LOG(DEBUG, "Disc count should be %d when done\n", final_disc_count);
- while (final_disc_count > control_loops::index_loop.status->shot_disc_count) {
- control_loops::index_loop.status.FetchNextBlocking();
- if (ShouldExitAuto()) return;
- }
- LOG(INFO, "Shot %d discs\n", n);
-}
-
void StopDrivetrain() {
LOG(INFO, "Stopping the drivetrain\n");
control_loops::drivetrain.goal.MakeWithBuilder()
@@ -237,67 +84,6 @@
LOG(INFO, "Done moving\n");
}
-// Drives forward while we can pick up discs up to max_distance (in meters).
-void DriveForwardPickUp(double max_distance, double wrist_angle) {
- LOG(INFO, "going to pick up at a max distance of %f\n", max_distance);
-
- static const ::aos::time::Time kPeriod = ::aos::time::Time::InMS(10);
- ::aos::util::TrapezoidProfile profile(kPeriod);
- ::Eigen::Matrix<double, 2, 1> driveTrainState;
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- static const double kMaximumAcceleration = 1.0;
-
- profile.set_maximum_acceleration(kMaximumAcceleration);
- profile.set_maximum_velocity(0.6);
-
- bool driving_back = false;
- const double kDestination = -0.20;
-
- while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
- driveTrainState = profile.Update(driving_back ? kDestination : max_distance,
- goal_velocity);
-
- if (ShouldExitAuto()) return;
-
- if (driving_back) {
- if (::std::abs(driveTrainState(0, 0)) < epsilon) break;
- } 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");
- }
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- driveTrainState(0, 0) + left_initial_position,
- driveTrainState(0, 0) + right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(driveTrainState(0, 0) + left_initial_position)
- .right_goal(driveTrainState(0, 0) + right_initial_position)
- .left_velocity_goal(driveTrainState(1, 0))
- .right_velocity_goal(driveTrainState(1, 0))
- .Send();
- }
- left_initial_position += kDestination;
- right_initial_position += kDestination;
- LOG(INFO, "Done moving\n");
-}
-
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
@@ -337,24 +123,11 @@
LOG(INFO, "Done moving\n");
}
-// start with N discs in the indexer
void HandleAuto() {
LOG(INFO, "Handling auto mode\n");
- const double WRIST_UP =
- constants::GetValues().wrist_hall_effect_start_angle - 0.4;
- const double WRIST_DOWN = -0.580;
- const double WRIST_DOWN_TWO = WRIST_DOWN - 0.012;
- const double ANGLE_ONE = 0.509;
- const double ANGLE_TWO = 0.673;
-
- 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;
control_loops::drivetrain.position.FetchLatest();
@@ -368,107 +141,6 @@
control_loops::drivetrain.position->right_encoder;
StopDrivetrain();
-
- SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
- SetAngle_AdjustGoal(ANGLE_ONE);
- SetShooterVelocity(395.0);
- WaitForIndexReset();
- if (ShouldExitAuto()) return;
- PreloadIndex(); // spin to top and put 1 disc into loader
-
- if (ShouldExitAuto()) return;
- WaitForWrist();
- if (ShouldExitAuto()) return;
- WaitForAngle_Adjust();
- ShootIndex(); // tilt up, shoot, repeat until empty
- // calls WaitForShooter
- ShootNDiscs(4); // ShootNDiscs returns if ShouldExitAuto
- if (ShouldExitAuto()) return;
-
- StartIndex(); // take in up to 4 discs
-
- if (false) {
- const double kDistanceToCenterMeters = 3.11023;
- const double kMaxPickupDistance = 2.5;
- const double kTurnToCenterDegrees = 78.2;
-
- // Drive back to the center line.
- SetDriveGoal(-kDistanceToCenterMeters);
- if (ShouldExitAuto()) return;
-
- SetWristGoal(WRIST_DOWN);
- // Turn towards the center.
- DriveSpin(kTurnToCenterDegrees * M_PI / 180.0);
- if (ShouldExitAuto()) return;
- WaitForWrist();
- if (ShouldExitAuto()) return;
-
- // Pick up at most 4 discs and drive at most kMaxPickupDistance.
- DriveForwardPickUp(kMaxPickupDistance, WRIST_UP);
-
- SetWristGoal(WRIST_UP);
- DriveSpin(-kTurnToCenterDegrees * M_PI / 180.0);
- if (ShouldExitAuto()) return;
- // Drive back to where we were.
- SetDriveGoal(kDistanceToCenterMeters);
- if (ShouldExitAuto()) return;
-
- ShootNDiscs(4);
- if (ShouldExitAuto()) return;
- } else {
- // Delay to let the disc out of the shooter.
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
- SetWristGoal(WRIST_DOWN);
- StartIndex(); // take in up to 4 discs
-
- if (ShouldExitAuto()) return;
- WaitForWrist(); // wrist must be down before moving
- ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
- SetAngle_AdjustGoal(ANGLE_TWO);
- SetShooterVelocity(375.0);
-
- if (ShouldExitAuto()) return;
- WaitForIndex(); // ready to pick up discs
-
- // How long we're going to drive in total.
- 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.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));
-
- 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;
- WaitForAngle_Adjust();
- if (ShouldExitAuto()) return;
- ShootIndex();
- if (ShouldExitAuto()) return;
- }
}
} // namespace autonomous
diff --git a/frc971/autonomous/auto.h b/frc971/autonomous/auto.h
index 00d2d72..21122bc 100644
--- a/frc971/autonomous/auto.h
+++ b/frc971/autonomous/auto.h
@@ -8,4 +8,5 @@
} // namespace autonomous
} // namespace frc971
+
#endif // FRC971_AUTONOMOUS_AUTO_H_
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index c581198..91b1b2f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -24,10 +24,6 @@
'dependencies': [
'auto_queue',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
diff --git a/frc971/constants.cc b/frc971/constants.cc
index da2d593..ad5e191 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -21,60 +21,6 @@
namespace constants {
namespace {
-// 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.277;
-const double kPracticeWristHallEffectStartAngle = 1.178;
-
-const double kWristHallEffectStopAngle = 100 * M_PI / 180.0;
-
-const double kPracticeWristUpperPhysicalLimit = 1.677562;
-const double kCompWristUpperPhysicalLimit = 1.677562;
-
-const double kPracticeWristLowerPhysicalLimit = -0.746128;
-const double kCompWristLowerPhysicalLimit = -0.746128;
-
-const double kPracticeWristUpperLimit = 1.615385;
-const double kCompWristUpperLimit = 1.615385;
-
-const double kPracticeWristLowerLimit = -0.746128;
-const double kCompWristLowerLimit = -0.746128;
-
-const double kWristZeroingSpeed = 0.125;
-const double kWristZeroingOffSpeed = 0.35;
-
-const int kAngleAdjustHallEffect = 2;
-
-// Angle measured from CAD with the top of the angle adjust at the top of the
-// wire guide is 0.773652098 radians.
-
-const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
-const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.297, 1.5};
-
-const double kAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
-
-const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
-const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
-
-const double kPracticeAngleAdjustLowerPhysicalLimit = 0.270;
-const double kCompAngleAdjustLowerPhysicalLimit = 0.302;
-
-const double kPracticeAngleAdjustUpperLimit = 0.87;
-const double kCompAngleAdjustUpperLimit = 0.87;
-
-const double kPracticeAngleAdjustLowerLimit = 0.31;
-const double kCompAngleAdjustLowerLimit = 0.28;
-
-const double kAngleAdjustZeroingSpeed = -0.2;
-const double kAngleAdjustZeroingOffSpeed = -0.5;
-
-const double kPracticeAngleAdjustDeadband = 0.4;
-const double kCompAngleAdjustDeadband = 0.0;
-
-const int kCompCameraCenter = -2;
-const int kPracticeCameraCenter = -5;
-
const double kCompDrivetrainEncoderRatio =
(15.0 / 50.0) /*output reduction*/ * (36.0 / 24.0) /*encoder gears*/;
const double kCompLowGearRatio = 14.0 / 60.0 * 15.0 / 50.0;
@@ -98,60 +44,28 @@
LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
switch (team) {
case kCompTeamNumber:
- return new Values{kCompWristHallEffectStartAngle,
- kWristHallEffectStopAngle,
- kCompWristUpperLimit,
- kCompWristLowerLimit,
- kCompWristUpperPhysicalLimit,
- kCompWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kCompAngleAdjustHallEffectStartAngle,
- kAngleAdjustHallEffectStopAngle,
- kCompAngleAdjustUpperLimit,
- kCompAngleAdjustLowerLimit,
- kCompAngleAdjustUpperPhysicalLimit,
- kCompAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kCompAngleAdjustDeadband,
- kCompDrivetrainEncoderRatio,
- kCompLowGearRatio,
- kCompHighGearRatio,
- kCompLeftDriveShifter,
- kCompRightDriveShifter,
- true,
- control_loops::MakeVClutchDrivetrainLoop,
- control_loops::MakeClutchDrivetrainLoop,
- kCompCameraCenter};
+ return new Values{
+ kCompDrivetrainEncoderRatio,
+ kCompLowGearRatio,
+ kCompHighGearRatio,
+ kCompLeftDriveShifter,
+ kCompRightDriveShifter,
+ true,
+ control_loops::MakeVClutchDrivetrainLoop,
+ control_loops::MakeClutchDrivetrainLoop,
+ };
break;
case kPracticeTeamNumber:
- return new Values{kPracticeWristHallEffectStartAngle,
- kWristHallEffectStopAngle,
- kPracticeWristUpperLimit,
- kPracticeWristLowerLimit,
- kPracticeWristUpperPhysicalLimit,
- kPracticeWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kPracticeAngleAdjustHallEffectStartAngle,
- kAngleAdjustHallEffectStopAngle,
- kPracticeAngleAdjustUpperLimit,
- kPracticeAngleAdjustLowerLimit,
- kPracticeAngleAdjustUpperPhysicalLimit,
- kPracticeAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kPracticeAngleAdjustDeadband,
- kPracticeDrivetrainEncoderRatio,
- kPracticeLowGearRatio,
- kPracticeHighGearRatio,
- kPracticeLeftDriveShifter,
- kPracticeRightDriveShifter,
- false,
- control_loops::MakeVDogDrivetrainLoop,
- control_loops::MakeDogDrivetrainLoop,
- kPracticeCameraCenter};
+ return new Values{
+ kPracticeDrivetrainEncoderRatio,
+ kPracticeLowGearRatio,
+ kPracticeHighGearRatio,
+ kPracticeLeftDriveShifter,
+ kPracticeRightDriveShifter,
+ false,
+ control_loops::MakeVDogDrivetrainLoop,
+ control_loops::MakeDogDrivetrainLoop,
+ };
break;
default:
LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
diff --git a/frc971/constants.h b/frc971/constants.h
index 838bb35..2d12bfa 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -25,46 +25,6 @@
// This structure contains current values for all of the things that change.
struct Values {
- // Wrist hall effect positive and negative edges.
- // How many radians from horizontal to the location of interest.
- double wrist_hall_effect_start_angle;
- double wrist_hall_effect_stop_angle;
-
- // Upper and lower extreme limits of travel for the wrist.
- // These are the soft stops for up and down.
- double wrist_upper_limit;
- double wrist_lower_limit;
-
- // Physical limits. These are here for testing.
- double wrist_upper_physical_limit;
- double wrist_lower_physical_limit;
-
- // Zeroing speed.
- // The speed to move the wrist at when zeroing in rad/sec
- double wrist_zeroing_speed;
- // Zeroing off speed (in rad/sec).
- double wrist_zeroing_off_speed;
-
- // AngleAdjust hall effect positive and negative edges.
- // These are the soft stops for up and down.
- const double (&angle_adjust_hall_effect_start_angle)[2];
- const double (&angle_adjust_hall_effect_stop_angle)[2];
-
- // Upper and lower extreme limits of travel for the angle adjust.
- double angle_adjust_upper_limit;
- double angle_adjust_lower_limit;
- // Physical limits. These are here for testing.
- double angle_adjust_upper_physical_limit;
- double angle_adjust_lower_physical_limit;
-
- // The speed to move the angle adjust when zeroing, in rad/sec
- double angle_adjust_zeroing_speed;
- // Zeroing off speed.
- double angle_adjust_zeroing_off_speed;
-
- // Deadband voltage.
- double angle_adjust_deadband;
-
// The ratio from the encoder shaft to the drivetrain wheels.
double drivetrain_encoder_ratio;
@@ -79,10 +39,6 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
-
- // How many pixels off center the vertical line
- // on the camera view is.
- int camera_center;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
deleted file mode 100644
index ffb98f2..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ /dev/null
@@ -1,85 +0,0 @@
-#include "frc971/control_loops/angle_adjust/angle_adjust.h"
-
-#include <algorithm>
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/constants.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-
-AngleAdjustMotor::AngleAdjustMotor(
- control_loops::AngleAdjustLoop *my_angle_adjust)
- : aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
- my_angle_adjust),
- zeroed_joint_(MakeAngleAdjustLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<2>::ConfigurationData config_data;
-
- config_data.lower_limit = GetValues().angle_adjust_lower_limit;
- config_data.upper_limit = GetValues().angle_adjust_upper_limit;
- memcpy(config_data.hall_effect_start_angle,
- GetValues().angle_adjust_hall_effect_start_angle,
- sizeof(config_data.hall_effect_start_angle));
- config_data.zeroing_off_speed = GetValues().angle_adjust_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().angle_adjust_zeroing_speed;
-
- config_data.max_zeroing_voltage = 4.0;
- config_data.deadband_voltage = GetValues().angle_adjust_deadband;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
-
-// Positive angle is up, and positive power is up.
-void AngleAdjustMotor::RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::AngleAdjustLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status *status) {
-
- // Disable the motors now so that all early returns will return with the
- // motors disabled.
- if (output) {
- output->voltage = 0;
- }
-
- ZeroedJoint<2>::PositionData transformed_position;
- ZeroedJoint<2>::PositionData *transformed_position_ptr =
- &transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- transformed_position.position = position->angle;
- transformed_position.hall_effects[0] = position->bottom_hall_effect;
- transformed_position.hall_effect_positions[0] =
- position->bottom_calibration;
- transformed_position.hall_effects[1] = position->middle_hall_effect;
- transformed_position.hall_effect_positions[1] =
- position->middle_calibration;
- }
-
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
- output != NULL,
- goal->goal, 0.0);
-
- if (position) {
- LOG(DEBUG, "pos: %f bottom_hall: %s middle_hall: %s absolute: %f\n",
- position->angle,
- position->bottom_hall_effect ? "true" : "false",
- position->middle_hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
- }
-
- if (output) {
- output->voltage = voltage;
- }
- status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.002;
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
deleted file mode 100644
index afdabb2..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust.gyp
+++ /dev/null
@@ -1,78 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'angle_adjust_loop',
- 'type': 'static_library',
- 'sources': ['angle_adjust_motor.q'],
- 'variables': {
- 'header_path': 'frc971/control_loops/angle_adjust',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'angle_adjust_lib',
- 'type': 'static_library',
- 'sources': [
- 'angle_adjust.cc',
- 'angle_adjust_motor_plant.cc',
- 'unaugmented_angle_adjust_motor_plant.cc',
- ],
- 'dependencies': [
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- 'angle_adjust_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(AOS)/common/common.gyp:controls',
- 'angle_adjust_loop',
- ],
- },
- {
- 'target_name': 'angle_adjust_lib_test',
- 'type': 'executable',
- 'sources': [
- 'angle_adjust_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'angle_adjust_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- 'angle_adjust_loop',
- ],
- },
- {
- 'target_name': 'angle_adjust_csv',
- 'type': 'executable',
- 'sources': [
- 'angle_adjust_csv.cc',
- ],
- 'dependencies': [
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:timing',
- 'angle_adjust_loop',
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/common/common.gyp:queues',
- ],
- },
- {
- 'target_name': 'angle_adjust',
- 'type': 'executable',
- 'sources': [
- 'angle_adjust_main.cc',
- ],
- 'dependencies': [
- 'angle_adjust_lib',
- 'angle_adjust_loop',
- '<(AOS)/linux_code/linux_code.gyp:init',
- ],
- },
- ],
-}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
deleted file mode 100644
index 319e293..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
-#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
-
-#include <array>
-#include <memory>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
-#include "frc971/control_loops/zeroed_joint.h"
-
-namespace frc971 {
-namespace control_loops {
-
-// Allows the control loop to add the tests to access private members.
-namespace testing {
-class AngleAdjustTest_RezeroWithMissingPos_Test;
-class AngleAdjustTest_DisableGoesUninitialized_Test;
-}
-
-class AngleAdjustMotor
- : public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
- public:
- explicit AngleAdjustMotor(
- control_loops::AngleAdjustLoop *my_angle_adjust =
- &control_loops::angle_adjust);
- protected:
- virtual void RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::AngleAdjustLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status *status);
-
- // True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
-
- // True if the wrist is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
-
- // True if the wrist is zeroing.
- bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
-
- // True if the state machine is uninitialized.
- bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
-
- // True if the state machine is ready.
- bool is_ready() const { return zeroed_joint_.is_ready(); }
-
- private:
- // Allows the testing code to access some of private members.
- friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
- friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;
-
- // The zeroed joint to use.
- ZeroedJoint<2> zeroed_joint_;
-
- DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
-};
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
deleted file mode 100644
index 5da6ca9..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "stdio.h"
-
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/time.h"
-#include "aos/linux_code/init.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-
-using ::frc971::control_loops::angle_adjust;
-using ::aos::time::Time;
-
-// Records data from the queue and stores it in a .csv file which can then
-// be plotted/processed with relative ease.
-int main(int argc, char * argv[]) {
- FILE *data_file = NULL;
- FILE *output_file = NULL;
-
- if (argc == 2) {
- data_file = fopen(argv[1], "w");
- output_file = data_file;
- } else {
- printf("Not saving to a CSV file.\n");
- output_file = stdout;
- }
-
- fprintf(data_file, "time, power, position");
-
- ::aos::Init();
-
- Time start_time = Time::Now();
-
- while (true) {
- ::aos::time::PhasedLoop10MS(2000);
- angle_adjust.goal.FetchLatest();
- angle_adjust.status.FetchLatest();
- angle_adjust.position.FetchLatest();
- angle_adjust.output.FetchLatest();
- if (angle_adjust.output.get() &&
- angle_adjust.position.get()) {
- fprintf(output_file, "\n%f, %f, %f",
- (angle_adjust.position->sent_time - start_time).ToSeconds(),
- angle_adjust.output->voltage,
- angle_adjust.position->angle);
- }
- }
-
- if (data_file) {
- fclose(data_file);
- }
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_data.csv b/frc971/control_loops/angle_adjust/angle_adjust_data.csv
deleted file mode 100644
index 3e3a9fb..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_data.csv
+++ /dev/null
@@ -1,273 +0,0 @@
-0.007086, 0.696383, -0.000596
-0.016561, 0.696383, -0.000596
-0.026648, 0.696383, -0.000596
-0.036501, 0.696383, -0.000596
-0.046475, 0.696383, -0.000596
-0.056429, 0.696383, -0.000596
-0.066661, 0.696383, -0.000596
-0.076538, 0.696383, -0.000596
-0.086562, 0.696383, -0.000596
-0.096496, 0.696383, -0.000596
-0.106453, 0.696383, -0.000596
-0.116673, 0.696383, -0.000596
-0.126529, 0.696383, -0.000596
-0.136500, 0.696383, -0.000596
-0.146462, 0.696383, -0.000596
-0.156442, 0.696383, -0.000596
-0.166658, 0.696383, -0.000596
-0.176508, 0.696383, -0.000596
-0.186466, 0.696383, -0.000596
-0.196514, 0.696383, -0.000596
-0.206469, 0.696383, -0.000596
-0.216679, 0.696383, -0.000596
-0.226464, 0.696383, -0.000596
-0.236401, 0.696383, -0.000596
-0.246476, 0.696383, -0.000596
-0.256391, 0.696383, -0.000596
-0.266650, 0.696383, -0.000596
-0.276517, 0.696383, -0.000596
-0.286565, 0.696383, -0.000596
-0.296526, 0.696383, -0.000596
-0.306469, 0.696383, -0.000596
-0.316703, 0.696383, -0.000596
-0.326546, 0.696383, -0.000596
-0.336500, 0.696383, -0.000596
-0.346466, 0.696383, -0.000596
-0.356397, 0.696383, -0.000596
-0.366658, 0.696383, -0.000596
-0.376517, 0.696383, -0.000596
-0.386461, 0.696383, -0.000596
-0.396497, 0.696383, -0.000596
-0.406516, 0.696383, -0.000596
-0.416662, 0.696383, -0.000596
-0.426498, 0.696383, -0.000596
-0.436506, 0.696383, -0.000596
-0.446560, 0.696383, -0.000596
-0.456494, 0.696383, -0.000596
-0.466635, 0.696383, -0.000596
-0.476505, 0.696383, -0.000596
-0.486529, 0.696383, -0.000596
-0.496494, 0.696383, -0.000596
-0.506479, 0.696383, -0.000596
-0.516679, 0.696383, -0.000596
-0.526550, 0.696383, -0.000596
-0.536505, 0.696383, -0.000596
-0.546452, 0.696383, -0.000596
-0.556481, 0.696383, -0.000596
-0.566531, 0.696383, -0.000596
-0.576513, 0.696383, -0.000596
-0.586463, 0.696383, -0.000596
-0.596771, 0.696383, -0.000596
-0.606503, 0.696383, -0.000596
-0.616679, 0.696383, -0.000596
-0.626483, 0.696383, -0.000596
-0.636496, 0.696383, -0.000596
-0.646654, 0.696383, -0.000596
-0.656423, 0.696383, -0.000596
-0.666661, 0.696383, -0.000596
-0.676531, 0.696383, -0.000596
-0.686526, 0.696383, -0.000596
-0.696670, 0.696383, -0.000596
-0.706628, 0.696383, -0.000596
-0.716675, 0.696383, -0.000596
-0.726719, 0.696383, -0.000596
-0.736498, 0.696383, -0.000596
-0.746484, 0.696383, -0.000596
-0.756468, 0.696383, -0.000596
-0.766526, 0.696383, -0.000596
-0.776498, 0.696383, -0.000596
-0.786469, 0.696383, -0.000596
-0.796251, 0.696383, -0.000596
-0.806312, 0.696383, -0.000596
-0.816579, 0.696383, -0.000596
-0.826352, 0.696383, -0.000596
-0.836395, 0.696383, -0.000596
-0.846460, 0.696383, -0.000596
-0.856333, 0.696383, -0.000596
-0.866312, 0.696383, -0.000596
-0.876541, 0.696383, -0.000596
-0.886231, 0.696383, -0.000596
-0.896378, 0.696383, -0.000596
-0.906549, 0.696383, -0.000596
-0.916219, 0.696383, -0.000596
-0.926300, 0.696383, -0.000596
-0.936212, 0.696383, -0.000596
-0.946392, 12.000000, -0.000596
-0.956205, 12.000000, -0.000596
-0.966382, 12.000000, 0.001192
-0.976384, 12.000000, 0.005962
-0.988371, 12.000000, 0.014906
-0.996207, 12.000000, 0.022657
-1.006429, 12.000000, 0.032794
-1.016788, 12.000000, 0.043526
-1.026528, 12.000000, 0.055451
-1.036616, 12.000000, 0.067376
-1.049418, 12.000000, 0.084071
-1.056874, 12.000000, 0.093015
-1.066581, 12.000000, 0.105536
-1.076708, 12.000000, 0.118653
-1.086697, 12.000000, 0.131771
-1.096754, 12.000000, 0.144888
-1.106694, 12.000000, 0.158006
-1.116795, 12.000000, 0.171123
-1.126945, 12.000000, 0.184241
-1.136491, 12.000000, 0.197358
-1.146490, 12.000000, 0.211072
-1.156720, 8.838210, 0.224189
-1.166497, 4.659384, 0.237307
-1.176550, 1.566925, 0.250424
-1.186490, -1.463977, 0.262945
-1.196706, -4.497083, 0.273081
-1.206413, -6.664959, 0.280833
-1.216504, -8.191987, 0.285603
-1.226718, -8.899323, 0.285603
-1.238720, -8.265740, 0.282025
-1.246502, -7.344013, 0.277851
-1.256515, -5.283271, 0.270696
-1.266504, -3.292020, 0.262945
-1.276639, -1.703019, 0.254598
-1.286435, 0.722641, 0.247443
-1.296401, 2.376863, 0.240884
-1.306274, 3.669941, 0.235518
-1.316431, 4.798566, 0.231940
-1.326280, 5.585790, 0.230748
-1.336257, 5.131469, 0.231344
-1.346265, 5.052962, 0.233133
-1.356246, 4.237080, 0.236710
-1.366233, 3.588899, 0.240288
-1.376252, 2.132660, 0.244462
-1.386339, 1.550053, 0.248039
-1.396224, 0.328756, 0.252213
-1.406339, -0.379540, 0.255194
-1.416340, -1.273041, 0.256983
-1.426200, -1.551323, 0.258175
-1.436332, -1.593706, 0.258772
-1.446348, -1.695210, 0.258175
-1.456324, -1.569522, 0.257579
-1.466365, -0.979101, 0.256387
-1.476513, -0.794073, 0.255194
-1.486723, -0.508791, 0.254002
-1.496495, -0.189683, 0.252809
-1.506430, 0.093002, 0.251617
-1.516565, 0.386294, 0.250424
-1.526518, 0.681860, 0.249828
-1.536477, 0.659148, 0.249828
-1.546440, 0.541822, 0.249828
-1.556459, 0.602564, 0.249828
-1.566511, 0.597167, 0.249828
-1.576515, 0.587492, 0.249828
-1.586463, 0.593212, 0.249828
-1.596495, 0.592427, 0.249828
-1.606519, 0.591648, 0.249828
-1.616647, 0.592179, 0.249828
-1.626482, 0.592082, 0.249828
-1.636498, 0.592021, 0.249828
-1.646521, 0.592070, 0.249828
-1.656480, 0.592059, 0.249828
-1.666552, 0.592055, 0.249828
-1.676484, 0.592059, 0.249828
-1.686525, 0.592058, 0.249828
-1.696670, 0.592057, 0.249828
-1.706459, 0.592058, 0.249828
-1.716735, 0.592058, 0.249828
-1.726493, 0.592058, 0.249828
-1.736508, 0.592058, 0.249828
-1.746452, 0.592058, 0.249828
-1.756477, 0.592058, 0.249828
-1.766815, 0.592058, 0.249828
-1.776500, 0.592058, 0.249828
-1.786441, 0.592058, 0.249828
-1.796462, 0.592058, 0.249828
-1.806512, 0.592058, 0.249828
-1.816550, 0.592058, 0.249828
-1.826504, 0.592058, 0.249828
-1.836496, 0.592058, 0.249828
-1.846519, 0.592058, 0.249828
-1.856477, 0.592058, 0.249828
-1.866533, 0.592058, 0.249828
-1.876513, 0.592058, 0.249828
-1.886541, 0.592058, 0.249828
-1.896490, 0.592058, 0.249828
-1.906479, -12.000000, 0.249828
-1.916446, -12.000000, 0.249828
-1.926530, -12.000000, 0.248635
-1.936520, -12.000000, 0.244462
-1.946463, -12.000000, 0.236710
-1.956469, -12.000000, 0.226574
-1.966515, -12.000000, 0.215842
-1.976560, -12.000000, 0.203917
-1.986476, -12.000000, 0.191396
-1.996480, -12.000000, 0.177682
-2.006460, -12.000000, 0.163968
-2.016545, -12.000000, 0.150254
-2.026495, -12.000000, 0.135944
-2.036471, -12.000000, 0.121635
-2.046473, -12.000000, 0.107325
-2.056469, -12.000000, 0.093015
-2.066524, -12.000000, 0.078705
-2.076516, -12.000000, 0.064395
-2.086512, -12.000000, 0.049489
-2.096561, -11.491993, 0.034582
-2.106457, -4.953303, 0.020272
-2.116661, -2.354450, 0.005366
-2.126490, 1.313071, -0.008347
-2.136461, 4.880571, -0.020272
-2.146526, 7.088633, -0.029216
-2.156469, 9.296628, -0.034582
-2.166524, 9.779309, -0.036371
-2.176504, 9.677380, -0.033986
-2.186457, 8.409415, -0.029216
-2.196463, 7.037205, -0.022657
-2.206499, 4.964211, -0.014906
-2.216552, 3.206835, -0.007155
-2.226456, 0.878265, 0.000596
-2.236471, -0.788450, 0.007751
-2.246517, -2.095142, 0.012521
-2.256472, -3.529778, 0.016099
-2.266416, -3.784184, 0.017887
-2.276485, -3.906740, 0.017291
-2.286520, -3.591089, 0.015502
-2.296497, -2.772296, 0.012521
-2.306461, -2.173367, 0.008944
-2.316561, -1.324239, 0.005962
-2.326547, -0.303999, 0.002385
-2.336490, 0.515297, -0.000596
-2.346453, 1.081194, -0.002981
-2.356491, 1.877482, -0.005366
-2.366526, 1.984534, -0.005962
-2.376468, 2.197044, -0.005962
-2.386411, 1.936623, -0.005962
-2.396484, 2.018387, -0.005366
-2.406517, 1.716757, -0.004770
-2.416569, 1.598133, -0.003577
-2.426463, 1.469285, -0.002981
-2.436457, 0.993349, -0.001789
-2.446504, 0.754160, -0.001192
-2.456498, 0.786747, -0.000596
-2.466418, 0.572920, -0.000596
-2.476482, 0.737242, -0.000596
-2.486521, 0.701741, -0.000596
-2.496489, 0.685572, -0.000596
-2.506441, 0.700476, -0.000596
-2.516718, 0.696605, -0.000596
-2.526498, 0.695450, -0.000596
-2.536473, 0.696784, -0.000596
-2.546448, 0.696379, -0.000596
-2.556479, 0.696304, -0.000596
-2.566517, 0.696421, -0.000596
-2.576491, 0.696380, -0.000596
-2.586447, 0.696376, -0.000596
-2.596495, 0.696386, -0.000596
-2.606515, 0.696382, -0.000596
-2.616555, 0.696382, -0.000596
-2.626465, 0.696383, -0.000596
-2.636463, 0.696383, -0.000596
-2.646513, 0.696383, -0.000596
-2.656499, 0.696383, -0.000596
-2.666488, 0.696383, -0.000596
-2.676556, 0.696383, -0.000596
-2.686505, 0.696383, -0.000596
-2.696457, 0.696383, -0.000596
-2.706448, 0.696383, -0.000596
-2.716562, 0.696383, -0.000596
-2.726498, 0.696383, -0.000596
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
deleted file mode 100644
index 0ea7201..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ /dev/null
@@ -1,307 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-#include <array>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust.h"
-#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
-#include "frc971/constants.h"
-
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-
-
-// Class which simulates the angle_adjust and
-// sends out queue messages containing the position.
-class AngleAdjustMotorSimulation {
- public:
- // Constructs a motor simulation. initial_position is the inital angle of the
- // angle_adjust, which will be treated as 0 by the encoder.
- explicit AngleAdjustMotorSimulation(double initial_position)
- : angle_adjust_plant_(
- new StateFeedbackPlant<2, 1, 1>(MakeRawAngleAdjustPlant())),
- my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
- 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
- ".frc971.control_loops.angle_adjust.position",
- ".frc971.control_loops.angle_adjust.output",
- ".frc971.control_loops.angle_adjust.status") {
- Reinitialize(initial_position);
- }
-
- // Resets the plant so that it starts at initial_position.
- void Reinitialize(double initial_position) {
- initial_position_ = initial_position;
- angle_adjust_plant_->X(0, 0) = initial_position_;
- angle_adjust_plant_->X(1, 0) = 0.0;
- angle_adjust_plant_->Y = angle_adjust_plant_->C() * angle_adjust_plant_->X;
- last_position_ = angle_adjust_plant_->Y(0, 0);
- last_voltage_ = 0.0;
- calibration_value_[0] = 0.0;
- calibration_value_[1] = 0.0;
- }
-
- // Returns the absolute angle of the angle_adjust.
- double GetAbsolutePosition() const {
- return angle_adjust_plant_->Y(0, 0);
- }
-
- // Returns the adjusted angle of the angle_adjust.
- double GetPosition() const {
- return GetAbsolutePosition() - initial_position_;
- }
-
- // Sends out the position queue messages.
- void SendPositionMessage() {
- const double angle = GetPosition();
-
- ::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
- my_angle_adjust_loop_.position.MakeMessage();
- position->angle = angle;
-
- const double (&hall_effect_start_angle)[2] =
- constants::GetValues().angle_adjust_hall_effect_start_angle;
- const double (&hall_effect_stop_angle)[2] =
- constants::GetValues().angle_adjust_hall_effect_stop_angle;
-
- // Signal that the hall effect sensor has been triggered if it is within
- // the correct range.
- double abs_position = GetAbsolutePosition();
- if (abs_position <= hall_effect_start_angle[0] &&
- abs_position >= hall_effect_stop_angle[0]) {
- position->bottom_hall_effect = true;
- } else {
- position->bottom_hall_effect = false;
- }
- if (abs_position <= hall_effect_start_angle[1] &&
- abs_position >= hall_effect_stop_angle[1]) {
- position->middle_hall_effect = true;
- } else {
- position->middle_hall_effect = false;
- }
-
- // Only set calibration if it changed last cycle. Calibration starts out
- // with a value of 0.
- // TODO(aschuh): This won't deal with both edges correctly.
- if ((last_position_ < hall_effect_start_angle[0] ||
- last_position_ > hall_effect_stop_angle[0]) &&
- (position->bottom_hall_effect)) {
- calibration_value_[0] = hall_effect_start_angle[0] - initial_position_;
- }
- if ((last_position_ < hall_effect_start_angle[1] ||
- last_position_ > hall_effect_stop_angle[1]) &&
- (position->middle_hall_effect)) {
- calibration_value_[1] = hall_effect_start_angle[1] - initial_position_;
- }
-
- position->bottom_calibration = calibration_value_[0];
- position->middle_calibration = calibration_value_[1];
- position.Send();
- }
-
- // Simulates the angle_adjust moving for one timestep.
- void Simulate() {
- last_position_ = angle_adjust_plant_->Y(0, 0);
- EXPECT_TRUE(my_angle_adjust_loop_.output.FetchLatest());
- angle_adjust_plant_->U << last_voltage_;
- angle_adjust_plant_->Update();
-
- EXPECT_GE(constants::GetValues().angle_adjust_upper_physical_limit,
- angle_adjust_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().angle_adjust_lower_physical_limit,
- angle_adjust_plant_->Y(0, 0));
- last_voltage_ = my_angle_adjust_loop_.output->voltage;
- }
-
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
-
- private:
- AngleAdjustLoop my_angle_adjust_loop_;
- double initial_position_;
- double last_position_;
- double last_voltage_;
- double calibration_value_[2];
-};
-
-class AngleAdjustTest : public ::testing::Test {
- protected:
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- AngleAdjustLoop my_angle_adjust_loop_;
-
- // Create a loop and simulation plant.
- AngleAdjustMotor angle_adjust_motor_;
- AngleAdjustMotorSimulation angle_adjust_motor_plant_;
-
- AngleAdjustTest() :
- my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
- 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
- ".frc971.control_loops.angle_adjust.position",
- ".frc971.control_loops.angle_adjust.output",
- ".frc971.control_loops.angle_adjust.status"),
- angle_adjust_motor_(&my_angle_adjust_loop_),
- angle_adjust_motor_plant_(0.75) {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- SendDSPacket(true);
- }
-
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
- }
-
- void VerifyNearGoal() {
- my_angle_adjust_loop_.goal.FetchLatest();
- my_angle_adjust_loop_.position.FetchLatest();
- EXPECT_NEAR(my_angle_adjust_loop_.goal->goal,
- angle_adjust_motor_plant_.GetAbsolutePosition(),
- 1e-4);
- }
-
- virtual ~AngleAdjustTest() {
- ::aos::robot_state.Clear();
- }
-};
-
-// Tests that the angle_adjust zeros correctly and goes to a position.
-TEST_F(AngleAdjustTest, ZerosCorrectly) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
- for (int i = 0; i < 400; ++i) {
- angle_adjust_motor_plant_.SendPositionMessage();
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that the angle_adjust zeros correctly starting on the sensor.
-TEST_F(AngleAdjustTest, ZerosStartingOn) {
- angle_adjust_motor_plant_.Reinitialize(0.30);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
- for (int i = 0; i < 500; ++i) {
- angle_adjust_motor_plant_.SendPositionMessage();
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that missing positions are correctly handled.
-TEST_F(AngleAdjustTest, HandleMissingPosition) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
- for (int i = 0; i < 400; ++i) {
- if (i % 23) {
- angle_adjust_motor_plant_.SendPositionMessage();
- }
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// 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 < 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 > 1400) {
- angle_adjust_motor_plant_.SendPositionMessage();
- } else {
- 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 == 1430) {
- EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
- angle_adjust_motor_.is_moving_off());
- }
-
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that disabling while zeroing sends the state machine into the
-// uninitialized state.
-TEST_F(AngleAdjustTest, DisableGoesUninitialized) {
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
- for (int i = 0; i < 800; ++i) {
- angle_adjust_motor_plant_.SendPositionMessage();
- // After 0.5 seconds, disable the robot.
- if (i > 50 && i < 200) {
- SendDSPacket(false);
- if (i > 100) {
- // Give the loop a couple cycled to get the message and then verify that
- // it is in the correct state.
- EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
- }
- } else {
- SendDSPacket(true);
- }
- if (i == 202) {
- // Verify that we are zeroing after the bot gets enabled again.
- EXPECT_TRUE(angle_adjust_motor_.is_zeroing());
- }
-
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- }
- VerifyNearGoal();
-}
-
-/*
-// TODO(aschuh): Enable these tests if we install a second hall effect sensor.
-// Tests that the angle_adjust zeros correctly from above the second sensor.
-TEST_F(AngleAdjustTest, ZerosCorrectlyAboveSecond) {
- angle_adjust_motor_plant_.Reinitialize(1.75);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
- for (int i = 0; i < 400; ++i) {
- angle_adjust_motor_plant_.SendPositionMessage();
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that the angle_adjust zeros correctly starting on
-// the second hall effect sensor.
-TEST_F(AngleAdjustTest, ZerosStartingOnSecond) {
- angle_adjust_motor_plant_.Reinitialize(1.25);
- my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
- for (int i = 0; i < 500; ++i) {
- angle_adjust_motor_plant_.SendPositionMessage();
- angle_adjust_motor_.Iterate();
- angle_adjust_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-*/
-
-} // namespace testing
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_main.cc b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
deleted file mode 100644
index 1a90749..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/angle_adjust/angle_adjust.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- ::frc971::control_loops::AngleAdjustMotor angle_adjust;
- angle_adjust.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor.q b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
deleted file mode 100644
index a98419a..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor.q
+++ /dev/null
@@ -1,24 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-queue_group AngleAdjustLoop {
- implements aos.control_loops.ControlLoop;
-
- message Position {
- // Angle of the height adjust.
- double angle;
- bool bottom_hall_effect;
- bool middle_hall_effect;
- // The exact position when the corresponding hall_effect changed.
- double bottom_calibration;
- double middle_calibration;
- };
-
- queue aos.control_loops.Goal goal;
- queue Position position;
- queue aos.control_loops.Output output;
- queue aos.control_loops.Status status;
-};
-
-queue_group AngleAdjustLoop angle_adjust;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
deleted file mode 100644
index 5ae9e0b..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00844804908295, 0.000186726546509, 0.0, 0.706562970689, 0.0353055515475, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeAngleAdjustController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.82656297069, 80.440440048, 562.416026082;
- Eigen::Matrix<double, 1, 3> K;
- K << 171.984283883, 5.25098015082, 1.01656297069;
- return StateFeedbackController<3, 1, 1>(L, K, MakeAngleAdjustPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeAngleAdjustPlantCoefficients());
- return StateFeedbackPlant<3, 1, 1>(plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeAngleAdjustController());
- return StateFeedbackLoop<3, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
deleted file mode 100644
index 7141fa9..0000000
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeAngleAdjustController();
-
-StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
deleted file mode 100644
index 9efffc3..0000000
--- a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000186726546509, 0.0353055515475;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.60656297069, 51.0341417582;
- Eigen::Matrix<double, 1, 2> K;
- K << 264.830871921, 10.681380124;
- return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustRawPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustRawPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustRawController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
deleted file mode 100644
index c066e0a..0000000
--- a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
deleted file mode 100644
index 9a03657..0000000
--- a/frc971/control_loops/index/index.cc
+++ /dev/null
@@ -1,1075 +0,0 @@
-#include "frc971/control_loops/index/index.h"
-
-#include <stdio.h>
-
-#include <algorithm>
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/inttypes.h"
-
-#include "frc971/constants.h"
-#include "frc971/control_loops/index/index_motor_plant.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-
-double IndexMotor::Frisbee::ObserveNoTopDiscSensor(double index_position) {
- // The absolute disc position in meters.
- double disc_position = absolute_position(index_position);
- if (IndexMotor::kTopDiscDetectStart <= disc_position &&
- disc_position <= IndexMotor::kTopDiscDetectStop) {
- // Whoops, this shouldn't be happening.
- // Move the disc off the way that makes most sense.
- double distance_to_above = IndexMotor::ConvertDiscPositionToIndex(
- ::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
- double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
- ::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
- if (distance_to_above < distance_to_below) {
- LOG(INFO, "Moving disc to top slow.\n");
- // Move it up.
- index_start_position_ -= distance_to_above;
- return -distance_to_above;
- } else {
- LOG(INFO, "Moving disc to bottom slow.\n");
- index_start_position_ += distance_to_below;
- return distance_to_below;
- }
- }
- return 0.0;
-}
-
-IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
- : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
- wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
- hopper_disc_count_(0),
- total_disc_count_(0),
- shot_disc_count_(0),
- safe_goal_(Goal::HOLD),
- loader_goal_(LoaderGoal::READY),
- loader_state_(LoaderState::READY),
- loader_up_(false),
- disc_clamped_(false),
- disc_ejected_(false),
- is_shooting_(false),
- last_bottom_disc_detect_(false),
- last_top_disc_detect_(false),
- hopper_clear_(true),
- no_prior_position_(true),
- missing_position_count_(0) {
-}
-
-/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
-/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
-/*static*/ const double IndexMotor::kIndexFreeLength =
- IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
-/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
- kIndexStartPosition + kIndexFreeLength;
-/*static*/ const double IndexMotor::kReadyToPreload =
- kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
-/*static*/ const double IndexMotor::kReadyToLiftPosition =
- kLoaderFreeStopPosition + 0.2921;
-/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
-/*static*/ const double IndexMotor::kGrabberStartPosition =
- kReadyToLiftPosition - kGrabberLength;
-/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.7;
-/*static*/ const double IndexMotor::kLifterStopPosition =
- kReadyToLiftPosition + 0.161925;
-/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
-/*static*/ const double IndexMotor::kEjectorStopPosition =
- kLifterStopPosition + 0.01;
-/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
-/*static*/ const double IndexMotor::kBottomDiscDetectStart = 0.00;
-/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.13;
-/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.032;
-/*static*/ const ::aos::time::Time IndexMotor::kTransferOffDelay =
- ::aos::time::Time::InSeconds(0.3);
-
-// TODO(aschuh): Verify these with the sensor actually on.
-/*static*/ const double IndexMotor::kTopDiscDetectStart =
- (IndexMotor::kLoaderFreeStopPosition -
- IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
-/*static*/ const double IndexMotor::kTopDiscDetectStop =
- (IndexMotor::kLoaderFreeStopPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
-
-// I measured the angle between 2 discs. That then gives me the distance
-// between 2 posedges (or negedges). Then subtract off the width of the
-// positive pulse, and that gives the width of the negative pulse.
-/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
- (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
- (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
-
-const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
-const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
-const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
-
-/*static*/ const int IndexMotor::kGrabbingDelay = 5;
-/*static*/ const int IndexMotor::kLiftingDelay = 5;
-/*static*/ const int IndexMotor::kLiftingTimeout = 100;
-/*static*/ const int IndexMotor::kShootingDelay = 25;
-/*static*/ const int IndexMotor::kLoweringDelay = 4;
-/*static*/ const int IndexMotor::kLoweringTimeout = 120;
-
-// TODO(aschuh): Tune these.
-/*static*/ const double
- IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 11.0;
-/*static*/ const double
- IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 20;
-
-/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
- return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
-}
-
-/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
- const double angle) {
- return angle * (kDiscRadius + kRollerRadius);
-}
-
-/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
- const double position) {
- return position / (kDiscRadius + kRollerRadius);
-}
-
-/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
- return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
-}
-
-/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
- return IndexMotor::ConvertDiscAngleToDiscPosition(
- ConvertIndexToDiscAngle(angle));
-}
-
-/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
- const double angle) {
- const double gear_ratio = (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
- kTransferRollerRadius);
- return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
-}
-
-/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
- const double position) {
- return IndexMotor::ConvertDiscAngleToIndex(
- ConvertDiscPositionToDiscAngle(position));
-}
-
-bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
- bool found_start = false;
- for (unsigned int i = 0; i < frisbees_.size(); ++i) {
- Frisbee &frisbee = frisbees_[i];
- if (!found_start) {
- if (frisbee.has_position()) {
- *disc_position = frisbee.position();
- if (found_disc) {
- *found_disc = &frisbee;
- }
- found_start = true;
- }
- } else {
- if (frisbee.position() <= *disc_position) {
- *disc_position = frisbee.position();
- if (found_disc) {
- *found_disc = &frisbee;
- }
- }
- }
- }
- return found_start;
-}
-
-bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
- bool found_start = false;
- for (unsigned int i = 0; i < frisbees_.size(); ++i) {
- Frisbee &frisbee = frisbees_[i];
- if (!found_start) {
- if (frisbee.has_position()) {
- *disc_position = frisbee.position();
- if (found_disc) {
- *found_disc = &frisbee;
- }
- found_start = true;
- }
- } else {
- if (frisbee.position() > *disc_position) {
- *disc_position = frisbee.position();
- if (found_disc) {
- *found_disc = &frisbee;
- }
- }
- }
- }
- return found_start;
-}
-
-void IndexMotor::IndexStateFeedbackLoop::CapU() {
- // If the voltage has been low for a large number of cycles, cut the motor
- // power. This is generally very bad controls practice since this isn't LTI,
- // but we don't really care about tracking anything other than large step
- // inputs, and the loader doesn't need to be that accurate.
- if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
- ++low_voltage_count_;
- if (low_voltage_count_ > kNoMotionCuttoffCount) {
- U(0, 0) = 0.0;
- }
- } else {
- low_voltage_count_ = 0;
- }
-
- for (int i = 0; i < kNumOutputs; ++i) {
- if (U(i, 0) > U_max(i, 0)) {
- U(i, 0) = U_max(i, 0);
- } else if (U(i, 0) < U_min(i, 0)) {
- U(i, 0) = U_min(i, 0);
- }
- }
-}
-
-
-// Positive angle is towards the shooter, and positive power is towards the
-// shooter.
-void IndexMotor::RunIteration(
- const control_loops::IndexLoop::Goal *goal,
- const control_loops::IndexLoop::Position *position,
- control_loops::IndexLoop::Output *output,
- control_loops::IndexLoop::Status *status) {
- Time now = Time::Now();
- // Make goal easy to work with and sanity check it.
- Goal goal_enum = static_cast<Goal>(goal->goal_state);
- if (goal->goal_state < 0 || goal->goal_state > 5) {
- LOG(ERROR,
- "Goal state is %" PRId32 " which is out of range. Going to HOLD.\n",
- goal->goal_state);
- goal_enum = Goal::HOLD;
- }
-
- // Disable the motors now so that all early returns will return with the
- // motors disabled.
- double intake_voltage = 0.0;
- double transfer_voltage = 0.0;
- if (output) {
- output->intake_voltage = 0.0;
- output->transfer_voltage = 0.0;
- output->index_voltage = 0.0;
- }
-
- status->ready_to_intake = false;
-
- // Set the controller to use to be the one designed for the current number of
- // discs in the hopper. This is safe since the controller prevents the index
- // from being set out of bounds and picks the closest controller.
- wrist_loop_->set_controller_index(frisbees_.size());
-
- // Compute a safe index position that we can use.
- if (position) {
- wrist_loop_->Y << position->index_position;
- // Set the goal to be the current position if this is the first time through
- // so we don't always spin the indexer to the 0 position before starting.
- if (no_prior_position_) {
- LOG(INFO, "no prior position; resetting\n");
- wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
- wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
- no_prior_position_ = false;
- last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
- last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
- last_bottom_disc_negedge_wait_count_ =
- 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));
- }
-
- // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
- if (missing_position_count_ > 50) {
- LOG(INFO, "assuming cRIO rebooted\n");
- last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
- last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
- last_bottom_disc_negedge_wait_count_ =
- 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));
- // Adjust the disc positions so that they don't have to move.
- const double disc_offset =
- position->index_position - wrist_loop_->X_hat(0, 0);
- for (auto frisbee = frisbees_.begin();
- frisbee != frisbees_.end(); ++frisbee) {
- frisbee->OffsetDisc(disc_offset);
- }
- wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
- }
- missing_position_count_ = 0;
- if (last_top_disc_detect_) {
- if (last_top_disc_posedge_count_ != position->top_disc_posedge_count) {
- LOG(INFO, "Ignoring a top disc posedge\n");
- }
- last_top_disc_posedge_count_ = position->top_disc_posedge_count;
- }
- if (!last_top_disc_detect_) {
- if (last_top_disc_negedge_count_ != position->top_disc_negedge_count) {
- LOG(INFO, "Ignoring a top disc negedge\n");
- }
- last_top_disc_negedge_count_ = position->top_disc_negedge_count;
- }
- } else {
- ++missing_position_count_;
- }
- const double index_position = wrist_loop_->X_hat(0, 0);
-
- if (position) {
- // Reset the open region if we saw a negedge.
- if (position->bottom_disc_negedge_wait_count !=
- last_bottom_disc_negedge_wait_count_) {
- // Saw a negedge, must be a new region.
- lower_open_region_.Restart(position->bottom_disc_negedge_wait_position);
- }
- // Reset the open region if we saw a negedge.
- if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
- // Saw a negedge, must be a new region.
- upper_open_region_.Restart(position->top_disc_negedge_position);
- }
-
- // No disc. Expand the open region.
- if (!position->bottom_disc_detect) {
- lower_open_region_.Expand(index_position);
- }
-
- // No disc. Expand the open region.
- if (!position->top_disc_detect) {
- upper_open_region_.Expand(index_position);
- }
-
- if (!position->top_disc_detect) {
- // We don't see a disc. Verify that there are no discs that we should be
- // seeing.
- // Assume that discs will move slow enough that we won't miss one as it
- // goes by. They will either pile up above or below the sensor.
-
- double cumulative_offset = 0.0;
- for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
- frisbee != rend; ++frisbee) {
- frisbee->OffsetDisc(cumulative_offset);
- double amount_moved = frisbee->ObserveNoTopDiscSensor(
- wrist_loop_->X_hat(0, 0));
- cumulative_offset += amount_moved;
- }
- }
-
- if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
- LOG(INFO, "Saw a top posedge\n");
- const double index_position = wrist_loop_->X_hat(0, 0) -
- position->index_position + position->top_disc_posedge_position;
- // TODO(aschuh): Sanity check this number...
- // Requires storing when the disc was last seen with the sensor off, and
- // figuring out what to do if things go south.
-
- // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
- // down.
- int disc_direction = 0;
- if (wrist_loop_->X_hat(1, 0) > 100.0) {
- disc_direction = 1;
- } else if (wrist_loop_->X_hat(1, 0) < -100.0) {
- disc_direction = -1;
- } else {
- // Save the upper and lower positions that we last saw a disc at.
- // If there is a big buffer above, must be a disc from below.
- // If there is a big buffer below, must be a disc from above.
- // This should work to replace the velocity threshold above.
-
- const double open_width = upper_open_region_.width();
- const double relative_upper_open_precentage =
- (upper_open_region_.upper_bound() - index_position) / open_width;
- const double relative_lower_open_precentage =
- (index_position - upper_open_region_.lower_bound()) / open_width;
-
- if (ConvertIndexToDiscPosition(open_width) <
- kTopDiscDetectMinSeperation * 0.9) {
- LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
- } else if (relative_upper_open_precentage > 0.75) {
- // Looks like it is a disc going down from above since we are near
- // the upper edge.
- disc_direction = -1;
- LOG(INFO, "Disc edge going down\n");
- } else if (relative_lower_open_precentage > 0.75) {
- // Looks like it is a disc going up from below since we are near
- // the lower edge.
- disc_direction = 1;
- LOG(INFO, "Disc edge going up\n");
- } else {
- LOG(ERROR,
- "Got an edge in the middle of what should be an open region.\n");
- LOG(ERROR, "Open width: %f upper precentage %f %%\n",
- open_width, relative_upper_open_precentage);
- }
- }
-
- if (disc_direction > 0) {
- // Moving up at a reasonable clip.
- // Find the highest disc that is below the top disc sensor.
- // While we are at it, count the number above and log an error if there
- // are too many.
- if (frisbees_.size() == 0) {
- Frisbee new_frisbee;
- new_frisbee.has_been_indexed_ = true;
- new_frisbee.index_start_position_ = index_position -
- ConvertDiscPositionToIndex(kTopDiscDetectStart -
- kIndexStartPosition);
- ++hopper_disc_count_;
- ++total_disc_count_;
- frisbees_.push_front(new_frisbee);
- LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
- }
-
- int above_disc_count = 0;
- double highest_position = 0;
- Frisbee *highest_frisbee_below_sensor = NULL;
- for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
- frisbee != rend; ++frisbee) {
- const double disc_position = frisbee->absolute_position(
- index_position);
- // It is save to use the top position for the cuttoff, since the
- // sensor being low will result in discs being pushed off of it.
- if (disc_position >= kTopDiscDetectStop) {
- ++above_disc_count;
- } else if (!highest_frisbee_below_sensor ||
- disc_position > highest_position) {
- highest_frisbee_below_sensor = &*frisbee;
- highest_position = disc_position;
- }
- }
-
- if (!highest_frisbee_below_sensor) {
- Frisbee new_frisbee;
- new_frisbee.has_been_indexed_ = true;
- new_frisbee.index_start_position_ = index_position -
- ConvertDiscPositionToIndex(kTopDiscDetectStart -
- kIndexStartPosition);
- highest_position = kTopDiscDetectStart;
- ++hopper_disc_count_;
- ++total_disc_count_;
- frisbees_.push_front(new_frisbee);
- LOG(WARNING, "Added a disc to the hopper at the top sensor because the one we know about is up top\n");
- }
-
- if (above_disc_count > 1) {
- LOG(ERROR, "We have 2 discs above the top sensor.\n");
- }
- // We now have the disc. Shift all the ones below the sensor up by the
- // computed delta.
- const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
- highest_position - kTopDiscDetectStart);
- for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
- frisbee != rend; ++frisbee) {
- const double disc_position = frisbee->absolute_position(
- index_position);
- if (disc_position < kTopDiscDetectStop) {
- LOG(INFO, "Moving disc down by %f meters, since it is at %f and top is [%f, %f]\n",
- ConvertIndexToDiscPosition(disc_delta),
- disc_position, kTopDiscDetectStart,
- kTopDiscDetectStop);
- frisbee->OffsetDisc(disc_delta);
- }
- }
- if (highest_frisbee_below_sensor) {
- LOG(INFO, "Currently have %d discs, saw posedge moving up. "
- "Moving down by %f to %f\n", frisbees_.size(),
- ConvertIndexToDiscPosition(disc_delta),
- highest_frisbee_below_sensor->absolute_position(
- wrist_loop_->X_hat(0, 0)));
- } else {
- LOG(INFO, "Currently have %d discs, saw posedge moving up. "
- "Moving down by %f\n", frisbees_.size(),
- ConvertIndexToDiscPosition(disc_delta));
- }
- } else if (disc_direction < 0) {
- // Moving down at a reasonable clip.
- // There can only be 1 disc up top that would give us a posedge.
- // Find it and place it at the one spot that it can be.
- double min_disc_position = 0;
- Frisbee *min_frisbee = NULL;
- MinDiscPosition(&min_disc_position, &min_frisbee);
- if (!min_frisbee) {
- // Uh, oh, we see a disc but there isn't one...
- LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
- } else {
- const double disc_position = min_frisbee->absolute_position(
- index_position);
-
- const double disc_delta_meters = disc_position - kTopDiscDetectStop;
- const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
- disc_delta_meters);
- LOG(INFO, "Posedge going down. Moving top disc down by %f\n",
- disc_delta_meters);
- for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
- frisbee != end; ++frisbee) {
- frisbee->OffsetDisc(disc_delta);
- }
- }
- } else {
- LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
- }
- }
- }
-
- // Bool to track if it is safe for the goal to change yet.
- bool safe_to_change_state = true;
- if (!position) {
- // This fixes a nasty indexer bug.
- // If we didn't get a position this cycle, we don't run the code below which
- // checks the state of the disc detect sensor and whether all the discs are
- // indexed. It is therefore not safe to change state and loose track of
- // that disc.
- safe_to_change_state = false;
- }
- switch (safe_goal_) {
- case Goal::HOLD:
- // The goal should already be good, so sit tight with everything the same
- // as it was.
- break;
- case Goal::READY_LOWER:
- case Goal::INTAKE:
- hopper_clear_ = false;
- {
- if (position) {
- // Posedge of the disc entering the beam break.
- if (position->bottom_disc_posedge_count !=
- last_bottom_disc_posedge_count_) {
- transfer_frisbee_.Reset();
- transfer_frisbee_.bottom_posedge_time_ = now;
- LOG(INFO, "Posedge of bottom disc %f\n",
- transfer_frisbee_.bottom_posedge_time_.ToSeconds());
- ++hopper_disc_count_;
- ++total_disc_count_;
- }
-
- // Disc exited the beam break now.
- if (position->bottom_disc_negedge_count !=
- last_bottom_disc_negedge_count_) {
- transfer_frisbee_.bottom_negedge_time_ = now;
- LOG(INFO, "Negedge of bottom disc %f\n",
- transfer_frisbee_.bottom_negedge_time_.ToSeconds());
- frisbees_.push_front(transfer_frisbee_);
- }
-
- if (position->bottom_disc_detect) {
- intake_voltage = 0.0;
- transfer_voltage = 12.0;
- // Must wait until the disc gets out before we can change state.
- safe_to_change_state = false;
-
- // TODO(aschuh): A disc on the way through needs to start moving
- // the indexer if it isn't already moving. Maybe?
-
- Time elapsed_posedge_time = now -
- transfer_frisbee_.bottom_posedge_time_;
- 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 = -12.0;
- transfer_voltage = -12.0;
- }
- }
-
- // Check all non-indexed discs and see if they should be indexed.
- for (auto frisbee = frisbees_.begin();
- frisbee != frisbees_.end(); ++frisbee) {
- if (!frisbee->has_been_indexed_) {
- if (last_bottom_disc_negedge_wait_count_ !=
- position->bottom_disc_negedge_wait_count) {
- // We have an index difference.
- // Save the indexer position, and the time.
- if (last_bottom_disc_negedge_wait_count_ + 1 !=
- position->bottom_disc_negedge_wait_count) {
- LOG(ERROR, "Funny, we got 2 edges since we last checked.\n");
- }
-
- // Save the captured position as the position at which the disc
- // touched the indexer.
- LOG(INFO, "Grabbed on the index now at %f\n", index_position);
- frisbee->has_been_indexed_ = true;
- frisbee->index_start_position_ =
- position->bottom_disc_negedge_wait_position;
- }
- }
- }
- }
- for (auto frisbee = frisbees_.begin();
- frisbee != frisbees_.end(); ++frisbee) {
- if (!frisbee->has_been_indexed_) {
- intake_voltage = 0.0;
- transfer_voltage = 12.0;
-
- // All discs must be indexed before it is safe to stop indexing.
- safe_to_change_state = false;
- }
- }
-
- // Figure out where the indexer should be to move the discs down to
- // the right position.
- double max_disc_position = 0;
- if (MaxDiscPosition(&max_disc_position, NULL)) {
- LOG(DEBUG, "There is a disc down here!\n");
- // TODO(aschuh): Figure out what to do if grabbing the next one
- // would cause things to jam into the loader.
- // Say we aren't ready any more. Undefined behavior will result if
- // that isn't observed.
- double bottom_disc_position =
- max_disc_position + ConvertDiscAngleToIndex(M_PI);
- wrist_loop_->R << bottom_disc_position, 0.0;
-
- // Verify that we are close enough to the goal so that we should be
- // fine accepting the next disc.
- double disc_error_meters = ConvertIndexToDiscPosition(
- wrist_loop_->X_hat(0, 0) - bottom_disc_position);
- // We are ready for the next disc if the first one is in the first
- // half circle of the indexer. It will take time for the disc to
- // come into the indexer, so we will be able to move it out of the
- // way in time.
- // This choice also makes sure that we don't claim that we aren't
- // ready between full speed intaking.
- if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
- disc_error_meters < 0.04) {
- // We are only ready if we aren't being asked to change state or
- // are full.
- status->ready_to_intake =
- (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
- } else {
- status->ready_to_intake = false;
- }
- } else {
- // No discs! We are always ready for more if we aren't being
- // asked to change state.
- status->ready_to_intake = (safe_goal_ == goal_enum);
- }
-
- // Turn on the transfer roller if we are ready.
- if (status->ready_to_intake && hopper_disc_count_ < 4 &&
- safe_goal_ == Goal::INTAKE) {
- intake_voltage = transfer_voltage = 12.0;
- }
- }
- LOG(DEBUG, "INTAKE\n");
- break;
- case Goal::REINITIALIZE:
- LOG(WARNING, "Reinitializing the indexer\n");
- break;
- case Goal::READY_SHOOTER:
- case Goal::SHOOT:
- // Don't let us leave the shoot or preload state if there are 4 discs in
- // the hopper.
- if (hopper_disc_count_ >= 4 && goal_enum != Goal::SHOOT) {
- safe_to_change_state = false;
- }
- // Check if we have any discs to shoot or load and handle them.
- double min_disc_position = 0;
- if (MinDiscPosition(&min_disc_position, NULL)) {
- const double ready_disc_position = min_disc_position +
- ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
-
- const double grabbed_disc_position =
- min_disc_position +
- ConvertDiscPositionToIndex(kReadyToLiftPosition -
- kIndexStartPosition + 0.07);
-
- // Check the state of the loader FSM.
- // If it is ready to load discs, position the disc so that it is ready
- // to be grabbed.
- // If it isn't ready, there is a disc in there. It needs to finish it's
- // cycle first.
- if (loader_state_ != LoaderState::READY) {
- // We already have a disc in the loader.
- // Stage the discs back a bit.
- wrist_loop_->R << ready_disc_position, 0.0;
-
- // Shoot if we are grabbed and being asked to shoot.
- if (loader_state_ == LoaderState::GRABBED &&
- safe_goal_ == Goal::SHOOT) {
- loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
- is_shooting_ = true;
- }
-
- // Must wait until it has been grabbed to continue.
- if (loader_state_ == LoaderState::GRABBING) {
- safe_to_change_state = false;
- }
- } else {
- // No disc up top right now.
- wrist_loop_->R << grabbed_disc_position, 0.0;
-
- // See if the disc has gotten pretty far up yet.
- if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
- // Point of no return. We are committing to grabbing it now.
- safe_to_change_state = false;
- const double robust_grabbed_disc_position =
- (grabbed_disc_position -
- ConvertDiscPositionToIndex(kGrabberLength));
-
- // If close, start grabbing and/or shooting.
- if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
- // Start the state machine.
- if (safe_goal_ == Goal::SHOOT) {
- loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
- } else {
- loader_goal_ = LoaderGoal::GRAB;
- }
- // This frisbee is now gone. Take it out of the queue.
- frisbees_.pop_back();
- }
- }
- }
- } else {
- if (loader_state_ != LoaderState::READY) {
- // Shoot if we are grabbed and being asked to shoot.
- if (loader_state_ == LoaderState::GRABBED &&
- safe_goal_ == Goal::SHOOT) {
- loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
- }
- } else {
- // Ok, no discs in sight. Spin the hopper up by 150% of it's full
- // range and verify that we don't see anything.
- const double hopper_clear_verification_position =
- ::std::max(upper_open_region_.lower_bound(),
- lower_open_region_.lower_bound()) +
- ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
-
- wrist_loop_->R << hopper_clear_verification_position, 0.0;
- if (::std::abs(wrist_loop_->X_hat(0, 0) -
- hopper_clear_verification_position) <
- ConvertDiscPositionToIndex(0.05)) {
- // We are at the end of the range. There are no more discs here.
- while (frisbees_.size() > 0) {
- LOG(ERROR, "Dropping an extra disc since it can't exist\n");
- LOG(ERROR, "Upper is [%f %f]\n",
- upper_open_region_.upper_bound(),
- upper_open_region_.lower_bound());
- LOG(ERROR, "Lower is [%f %f]\n",
- lower_open_region_.upper_bound(),
- lower_open_region_.lower_bound());
- frisbees_.pop_back();
- --hopper_disc_count_;
- --total_disc_count_;
- }
- if (hopper_disc_count_ != 0) {
- LOG(ERROR,
- "Emptied the hopper out but there are still discs there\n");
- hopper_disc_count_ = 0;
- }
- hopper_clear_ = true;
- }
- }
- }
-
- {
- const double hopper_clear_verification_position =
- ::std::max(upper_open_region_.lower_bound(),
- lower_open_region_.lower_bound()) +
- ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
-
- if (wrist_loop_->X_hat(0, 0) >
- hopper_clear_verification_position +
- ConvertDiscPositionToIndex(0.05)) {
- // We are at the end of the range. There are no more discs here.
- while (frisbees_.size() > 0) {
- LOG(ERROR, "Dropping an extra disc since it can't exist\n");
- LOG(ERROR, "Upper is [%f %f]\n",
- upper_open_region_.upper_bound(),
- upper_open_region_.lower_bound());
- LOG(ERROR, "Lower is [%f %f]\n",
- lower_open_region_.upper_bound(),
- lower_open_region_.lower_bound());
- frisbees_.pop_back();
- --hopper_disc_count_;
- --total_disc_count_;
- }
- if (hopper_disc_count_ != 0) {
- LOG(ERROR,
- "Emptied the hopper out but there are still %" PRId32 " discs there\n",
- hopper_disc_count_);
- hopper_disc_count_ = 0;
- }
- hopper_clear_ = true;
- }
- }
-
- LOG(DEBUG, "READY_SHOOTER or SHOOT\n");
- break;
- }
-
- // Wait for a period of time to make sure that the disc gets sucked
- // in properly. We need to do this regardless of what the indexer is doing.
- for (auto frisbee = frisbees_.begin();
- frisbee != frisbees_.end(); ++frisbee) {
- if (now - frisbee->bottom_negedge_time_ < kTransferOffDelay) {
- transfer_voltage = 12.0;
- }
- }
-
- // If we have 4 discs, it is time to preload.
- if (safe_to_change_state && hopper_disc_count_ >= 4) {
- switch (safe_goal_) {
- case Goal::HOLD:
- case Goal::READY_LOWER:
- case Goal::INTAKE:
- safe_goal_ = Goal::READY_SHOOTER;
- safe_to_change_state = false;
- LOG(INFO, "We have %" PRId32 " discs, time to preload automatically\n",
- hopper_disc_count_);
- break;
- case Goal::READY_SHOOTER:
- case Goal::SHOOT:
- case Goal::REINITIALIZE:
- break;
- }
- }
-
- // The only way out of the loader is to shoot the disc. The FSM can only go
- // forwards.
- switch (loader_state_) {
- case LoaderState::READY:
- LOG(DEBUG, "Loader READY\n");
- // Open and down, ready to accept a disc.
- loader_up_ = false;
- disc_clamped_ = false;
- disc_ejected_ = false;
- disc_stuck_in_loader_ = false;
- if (loader_goal_ == LoaderGoal::GRAB ||
- loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
- if (goal->force_fire) {
- LOG(INFO, "Told to force fire, moving on\n");
- } else if (loader_goal_ == LoaderGoal::GRAB) {
- LOG(INFO, "Told to GRAB, moving on\n");
- } else {
- LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
- }
- loader_state_ = LoaderState::GRABBING;
- loader_countdown_ = kGrabbingDelay;
- } else {
- break;
- }
- case LoaderState::GRABBING:
- LOG(DEBUG, "Loader GRABBING %d\n", loader_countdown_);
- // Closing the grabber.
- loader_up_ = false;
- disc_clamped_ = true;
- disc_ejected_ = false;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
- } else {
- loader_state_ = LoaderState::GRABBED;
- }
- case LoaderState::GRABBED:
- LOG(DEBUG, "Loader GRABBED\n");
- // Grabber closed.
- loader_up_ = false;
- disc_clamped_ = true;
- disc_ejected_ = false;
- if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
- if (shooter.status.FetchLatest() || shooter.status.get()) {
- // TODO(aschuh): If we aren't shooting nicely, wait until the shooter
- // is up to speed rather than just spinning.
- if (shooter.status->average_velocity > 130 && shooter.status->ready) {
- loader_state_ = LoaderState::LIFTING;
- loader_countdown_ = kLiftingDelay;
- loader_timeout_ = 0;
- LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
- } else {
- LOG(WARNING, "Told to SHOOT_AND_RESET, shooter too slow at %f\n",
- shooter.status->average_velocity);
- break;
- }
- } else {
- LOG(ERROR, "Told to SHOOT_AND_RESET, no shooter data, moving on.\n");
- loader_state_ = LoaderState::LIFTING;
- loader_countdown_ = kLiftingDelay;
- loader_timeout_ = 0;
- }
- } else if (loader_goal_ == LoaderGoal::READY) {
- LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
- break;
- } else {
- break;
- }
- case LoaderState::LIFTING:
- LOG(DEBUG, "Loader LIFTING %d %d\n", loader_countdown_, loader_timeout_);
- // Lifting the disc.
- loader_up_ = true;
- disc_clamped_ = true;
- disc_ejected_ = false;
- if (position->loader_top) {
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- loader_timeout_ = 0;
- break;
- } else {
- loader_state_ = LoaderState::LIFTED;
- }
- } else {
- // Restart the countdown if it bounces back down or whatever.
- loader_countdown_ = kLiftingDelay;
- ++loader_timeout_;
- if (loader_timeout_ > kLiftingTimeout) {
- LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
- loader_state_ = LoaderState::LOWERING;
- loader_countdown_ = kLoweringDelay;
- loader_timeout_ = 0;
- disc_stuck_in_loader_ = true;
- } else {
- break;
- }
- }
- case LoaderState::LIFTED:
- LOG(DEBUG, "Loader LIFTED\n");
- // Disc lifted. Time to eject it out.
- loader_up_ = true;
- disc_clamped_ = true;
- disc_ejected_ = false;
- loader_state_ = LoaderState::SHOOTING;
- loader_countdown_ = kShootingDelay;
- case LoaderState::SHOOTING:
- LOG(DEBUG, "Loader SHOOTING %d\n", loader_countdown_);
- // Ejecting the disc into the shooter.
- loader_up_ = true;
- disc_clamped_ = false;
- disc_ejected_ = true;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
- } else {
- loader_state_ = LoaderState::SHOOT;
- }
- case LoaderState::SHOOT:
- LOG(DEBUG, "Loader SHOOT\n");
- // The disc has been shot.
- loader_up_ = true;
- disc_clamped_ = false;
- disc_ejected_ = true;
- loader_state_ = LoaderState::LOWERING;
- loader_countdown_ = kLoweringDelay;
- loader_timeout_ = 0;
- case LoaderState::LOWERING:
- LOG(DEBUG, "Loader LOWERING %d %d\n", loader_countdown_, loader_timeout_);
- // Lowering the loader back down.
- loader_up_ = false;
- disc_clamped_ = false;
- // 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_;
- loader_timeout_ = 0;
- break;
- } else {
- loader_state_ = LoaderState::LOWERED;
- --hopper_disc_count_;
- ++shot_disc_count_;
- }
- } else {
- // Restart the countdown if it bounces back up or something.
- loader_countdown_ = kLoweringDelay;
- ++loader_timeout_;
- if (loader_timeout_ > kLoweringTimeout) {
- LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
- loader_state_ = LoaderState::LOWERED;
- disc_stuck_in_loader_ = true;
- } else {
- break;
- }
- }
- case LoaderState::LOWERED:
- LOG(DEBUG, "Loader LOWERED\n");
- loader_up_ = false;
- disc_ejected_ = false;
- is_shooting_ = false;
- if (disc_stuck_in_loader_) {
- disc_stuck_in_loader_ = false;
- disc_clamped_ = true;
- loader_state_ = LoaderState::GRABBED;
- } else {
- disc_clamped_ = false;
- loader_state_ = LoaderState::READY;
- // Once we have shot, we need to hang out in READY until otherwise
- // notified.
- loader_goal_ = LoaderGoal::READY;
- }
- break;
- }
-
- // Update the observer.
- wrist_loop_->Update(position != NULL, output == NULL);
-
- if (position) {
- LOG(DEBUG, "pos=%f\n", position->index_position);
- last_bottom_disc_detect_ = position->bottom_disc_detect;
- last_top_disc_detect_ = position->top_disc_detect;
- last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
- last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
- last_bottom_disc_negedge_wait_count_ =
- 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;
- }
-
- // Clear everything if we are supposed to re-initialize.
- if (goal_enum == Goal::REINITIALIZE) {
- safe_goal_ = Goal::REINITIALIZE;
- no_prior_position_ = true;
- hopper_disc_count_ = 0;
- total_disc_count_ = 0;
- shot_disc_count_ = 0;
- loader_state_ = LoaderState::READY;
- loader_goal_ = LoaderGoal::READY;
- loader_countdown_ = 0;
- loader_up_ = false;
- disc_clamped_ = false;
- disc_ejected_ = false;
-
- intake_voltage = 0.0;
- transfer_voltage = 0.0;
- wrist_loop_->U(0, 0) = 0.0;
- frisbees_.clear();
- }
-
- status->hopper_disc_count = hopper_disc_count_;
- status->total_disc_count = total_disc_count_;
- status->shot_disc_count = shot_disc_count_;
- status->preloaded = (loader_state_ != LoaderState::READY);
- status->is_shooting = is_shooting_;
- status->hopper_clear = hopper_clear_;
-
- if (output) {
- output->intake_voltage = intake_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 {
- output->index_voltage = wrist_loop_->U(0, 0);
- }
- output->loader_up = loader_up_;
- output->disc_clamped = disc_clamped_;
- output->disc_ejected = disc_ejected_;
- }
-
- if (safe_to_change_state) {
- safe_goal_ = goal_enum;
- }
- if (hopper_disc_count_ < 0) {
- LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
- }
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
deleted file mode 100644
index dc1c06d..0000000
--- a/frc971/control_loops/index/index.gyp
+++ /dev/null
@@ -1,69 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'index_loop',
- 'type': 'static_library',
- 'sources': ['index_motor.q'],
- 'variables': {
- 'header_path': 'frc971/control_loops/index',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'index_lib',
- 'type': 'static_library',
- 'sources': [
- 'index.cc',
- 'index_motor_plant.cc',
- ],
- 'dependencies': [
- 'index_loop',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(AOS)/common/common.gyp:controls',
- 'index_loop',
- ],
- },
- {
- 'target_name': 'index_lib_test',
- 'type': 'executable',
- 'sources': [
- 'index_lib_test.cc',
- 'transfer_motor_plant.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'index_loop',
- 'index_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- ],
- },
- {
- 'target_name': 'index',
- 'type': 'executable',
- 'sources': [
- 'index_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'index_lib',
- 'index_loop',
- ],
- },
- ],
-}
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
deleted file mode 100644
index e6902d8..0000000
--- a/frc971/control_loops/index/index.h
+++ /dev/null
@@ -1,361 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_H_
-#define FRC971_CONTROL_LOOPS_INDEX_INDEX_H_
-
-#include <deque>
-#include "aos/common/libstdc++/memory"
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "aos/common/time.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/index/index_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-class IndexTest_InvalidStateTest_Test;
-class IndexTest_LostDisc_Test;
-}
-
-// This class represents a region of space.
-class Region {
- public:
- Region () : upper_bound_(0.0), lower_bound_(0.0) {}
-
- // Restarts the region tracking by starting over with a 0 width region with
- // the bounds at [edge, edge].
- void Restart(double edge) {
- upper_bound_ = edge;
- lower_bound_ = edge;
- }
-
- // Expands the region to include the new point.
- void Expand(double new_point) {
- if (new_point > upper_bound_) {
- upper_bound_ = new_point;
- } else if (new_point < lower_bound_) {
- lower_bound_ = new_point;
- }
- }
-
- // Returns the width of the region.
- double width() const { return upper_bound_ - lower_bound_; }
- // Returns the upper and lower bounds.
- double upper_bound() const { return upper_bound_; }
- double lower_bound() const { return lower_bound_; }
-
- private:
- // Upper bound of the region.
- double upper_bound_;
- // Lower bound of the region.
- double lower_bound_;
-};
-
-class IndexMotor
- : public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
- public:
- explicit IndexMotor(
- control_loops::IndexLoop *my_index = &control_loops::index_loop);
-
- static const double kTransferStartPosition;
- static const double kIndexStartPosition;
- // The distance from where the disc first grabs on the indexer to where it
- // just bairly clears the loader.
- static const double kIndexFreeLength;
- // The distance to where the disc just starts to enter the loader.
- static const double kLoaderFreeStopPosition;
- // The distance to where the next disc gets positioned while the current disc
- // is shooting.
- static const double kReadyToPreload;
-
- // Distance that the grabber pulls the disc in by.
- static const double kGrabberLength;
- // Distance to where the grabber takes over.
- static const double kGrabberStartPosition;
-
- // The distance to where the disc hits the back of the loader and is ready to
- // lift.
- static const double kReadyToLiftPosition;
-
- static const double kGrabberMovementVelocity;
- // TODO(aschuh): This depends on the shooter angle...
- // Distance to where the shooter is up and ready to shoot.
- static const double kLifterStopPosition;
- static const double kLifterMovementVelocity;
-
- // Distance to where the disc has been launched.
- // TODO(aschuh): This depends on the shooter angle...
- static const double kEjectorStopPosition;
- static const double kEjectorMovementVelocity;
-
- // Start and stop position of the bottom disc detect sensor in meters.
- static const double kBottomDiscDetectStart;
- static const double kBottomDiscDetectStop;
- // Delay between the negedge of the disc detect and when it engages on the
- // indexer.
- static const double kBottomDiscIndexDelay;
-
- // Time after seeing the fourth disc that we need to wait before turning the
- // transfer roller off.
- static const ::aos::time::Time kTransferOffDelay;
-
- static const double kTopDiscDetectStart;
- static const double kTopDiscDetectStop;
-
- // Minimum distance between 2 frisbees as seen by the top disc detect sensor.
- static const double kTopDiscDetectMinSeperation;
-
- // Converts the angle of the indexer to the angle of the disc.
- static double ConvertIndexToDiscAngle(const double angle);
- // Converts the angle of the indexer to the position that the center of the
- // disc has traveled.
- static double ConvertIndexToDiscPosition(const double angle);
-
- // Converts the angle of the transfer roller to the position that the center
- // of the disc has traveled.
- static double ConvertTransferToDiscPosition(const double angle);
-
- // Converts the distance around the indexer to the position of
- // the index roller.
- static double ConvertDiscPositionToIndex(const double position);
- // Converts the angle around the indexer to the position of the index roller.
- static double ConvertDiscAngleToIndex(const double angle);
- // Converts the angle around the indexer to the position of the disc in the
- // indexer.
- static double ConvertDiscAngleToDiscPosition(const double angle);
- // Converts the distance around the indexer to the angle of the disc around
- // the indexer.
- static double ConvertDiscPositionToDiscAngle(const double position);
-
- // Disc radius in meters.
- static const double kDiscRadius;
- // Roller radius in meters.
- static const double kRollerRadius;
- // Transfer roller radius in meters.
- static const double kTransferRollerRadius;
-
- // Time that it takes to grab the disc in cycles.
- static const int kGrabbingDelay;
- // Time that it takes to finish lifting the loader after the sensor is
- // triggered in cycles.
- static const int kLiftingDelay;
- // Time until we give up lifting and move on in cycles.
- static const int kLiftingTimeout;
- // Time that it takes to shoot the disc in cycles.
- static const int kShootingDelay;
- // Time that it takes to finish lowering the loader after the sensor is
- // triggered in cycles.
- static const int kLoweringDelay;
- // Time until we give up lowering and move on in cycles.
- // It's a long time because we really don't want to ever hit this.
- static const int kLoweringTimeout;
-
- // Object representing a Frisbee tracked by the indexer.
- class Frisbee {
- public:
- Frisbee()
- : bottom_posedge_time_(0, 0),
- bottom_negedge_time_(0, 0) {
- Reset();
- }
-
- // Resets a Frisbee so it can be reused.
- void Reset() {
- bottom_posedge_time_ = ::aos::time::Time(0, 0);
- bottom_negedge_time_ = ::aos::time::Time(0, 0);
- has_been_indexed_ = false;
- index_start_position_ = 0.0;
- }
-
- // Returns true if the position is valid.
- bool has_position() const {
- return has_been_indexed_;
- }
-
- // Returns the most up to date and accurate position that we have for the
- // disc. This is the indexer position that the disc grabbed at.
- double position() const {
- return index_start_position_;
- }
-
- // Returns the absolute position of the disc in meters in the hopper given
- // that the indexer is at the provided position.
- double absolute_position(const double index_position) const {
- return IndexMotor::ConvertIndexToDiscPosition(
- index_position - index_start_position_) +
- IndexMotor::kIndexStartPosition;
- }
-
- // Shifts the disc down the indexer by the provided offset. This is to
- // handle when the cRIO reboots.
- void OffsetDisc(double offset) {
- index_start_position_ += offset;
- }
-
- // Potentially offsets the position with the knowledge that no discs are
- // currently blocking the top sensor. This knowledge can be used to move
- // this disc if it is believed to be blocking the top sensor.
- // Returns the amount that the disc moved due to this observation.
- double ObserveNoTopDiscSensor(double index_position);
-
- // Posedge and negedge disc times.
- ::aos::time::Time bottom_posedge_time_;
- ::aos::time::Time bottom_negedge_time_;
-
- // True if the disc has a valid index position.
- bool has_been_indexed_;
- // Location of the index when the disc first contacted it.
- double index_start_position_;
- };
-
- // Returns where the indexer thinks the frisbees are.
- const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
-
- protected:
- virtual void RunIteration(
- const control_loops::IndexLoop::Goal *goal,
- const control_loops::IndexLoop::Position *position,
- control_loops::IndexLoop::Output *output,
- control_loops::IndexLoop::Status *status);
-
- private:
- friend class testing::IndexTest_InvalidStateTest_Test;
- friend class testing::IndexTest_LostDisc_Test;
-
- // This class implements the CapU function correctly given all the extra
- // information that we know about from the wrist motor.
- class IndexStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
- public:
- IndexStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop)
- : StateFeedbackLoop<2, 1, 1>(loop),
- low_voltage_count_(0) {
- }
-
- // Voltage below which the indexer won't move with a disc in it.
- static const double kMinMotionVoltage;
- // Maximum number of cycles to apply a low voltage to the motor.
- static const double kNoMotionCuttoffCount;
-
- // Caps U, but disables the motor after a number of cycles of inactivity.
- virtual void CapU();
- private:
- // Number of cycles that we have seen a small voltage being applied.
- uint32_t low_voltage_count_;
- };
-
- // Sets disc_position to the minimum or maximum disc position.
- // Sets found_disc to point to the frisbee that was found, and ignores it if
- // found_disc is NULL.
- // Returns true if there were discs, and false if there weren't.
- // On false, disc_position is left unmodified.
- bool MinDiscPosition(double *disc_position, Frisbee **found_disc);
- bool MaxDiscPosition(double *disc_position, Frisbee **found_disc);
-
- // The state feedback control loop to talk to for the index.
- ::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
-
- // Count of the number of discs that we have collected.
- int32_t hopper_disc_count_;
- int32_t total_disc_count_;
- int32_t shot_disc_count_;
-
- enum class Goal {
- // Hold position, in a low power state.
- HOLD = 0,
- // Get ready to load discs by shifting the discs down.
- READY_LOWER = 1,
- // Ready the discs, spin up the transfer roller, and accept discs.
- INTAKE = 2,
- // Get ready to shoot, and place a disc in the loader.
- READY_SHOOTER = 3,
- // Shoot at will.
- SHOOT = 4,
- // Reinitialize.
- REINITIALIZE = 5
- };
-
- // These two enums command and track the loader loading discs into the
- // shooter.
- enum class LoaderState {
- // Open and down, ready to accept a disc.
- READY,
- // Closing the grabber.
- GRABBING,
- // Grabber closed.
- GRABBED,
- // Lifting the disc.
- LIFTING,
- // Disc lifted.
- LIFTED,
- // Ejecting the disc into the shooter.
- SHOOTING,
- // The disc has been shot.
- SHOOT,
- // Lowering the loader back down.
- LOWERING,
- // The indexer is lowered.
- LOWERED
- };
-
- // TODO(aschuh): If we are grabbed and asked to be ready, now what?
- // LOG ?
- enum class LoaderGoal {
- // Get the loader ready to accept another disc.
- READY,
- // Grab a disc now.
- GRAB,
- // Lift it up, shoot, and reset.
- // Waits to shoot until the shooter is stable.
- // Resets the goal to READY once one disc has been shot.
- SHOOT_AND_RESET
- };
-
- // The current goal
- Goal safe_goal_;
-
- // Loader goal, state, and counter.
- LoaderGoal loader_goal_;
- LoaderState loader_state_;
- int loader_countdown_, loader_timeout_;
- // Whether or not we (might have) failed to shoot a disc that's now (probably)
- // still in the loader.
- bool disc_stuck_in_loader_;
-
- // Current state of the pistons.
- bool loader_up_;
- bool disc_clamped_;
- bool disc_ejected_;
-
- bool is_shooting_;
-
- // The frisbee that is flying through the transfer rollers.
- Frisbee transfer_frisbee_;
-
- // Bottom disc detect from the last valid packet for detecting edges.
- bool last_bottom_disc_detect_;
- bool last_top_disc_detect_;
- bool hopper_clear_;
- int32_t last_bottom_disc_posedge_count_;
- int32_t last_bottom_disc_negedge_count_;
- int32_t last_bottom_disc_negedge_wait_count_;
- int32_t last_top_disc_posedge_count_;
- int32_t last_top_disc_negedge_count_;
-
- // Frisbees are in order such that the newest frisbee is on the front.
- ::std::deque<Frisbee> frisbees_;
-
- // True if we haven't seen a position before.
- bool no_prior_position_;
- // Number of position messages that we have missed in a row.
- uint32_t missing_position_count_;
-
- // The no-disc regions for both the bottom and top beam break sensors.
- Region upper_open_region_;
- Region lower_open_region_;
-
- DISALLOW_COPY_AND_ASSIGN(IndexMotor);
-};
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_H_
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
deleted file mode 100644
index 035589d..0000000
--- a/frc971/control_loops/index/index_lib_test.cc
+++ /dev/null
@@ -1,1340 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/index/index.h"
-#include "frc971/control_loops/index/index_motor_plant.h"
-#include "frc971/control_loops/index/transfer_motor_plant.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/constants.h"
-
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-
-class Frisbee {
- public:
- // Creates a frisbee starting at the specified position in the frisbee path,
- // and with the transfer and index rollers at the specified positions.
- Frisbee(double transfer_roller_position,
- double index_roller_position,
- double position = IndexMotor::kBottomDiscDetectStart - 0.001)
- : transfer_roller_position_(transfer_roller_position),
- index_roller_position_(index_roller_position),
- position_(position),
- has_been_shot_(false),
- has_bottom_disc_negedge_wait_position_(false),
- bottom_disc_negedge_wait_position_(0.0),
- after_negedge_time_left_(IndexMotor::kBottomDiscIndexDelay),
- counted_negedge_wait_(false),
- has_top_disc_posedge_position_(false),
- top_disc_posedge_position_(0.0),
- has_top_disc_negedge_position_(false),
- top_disc_negedge_position_(0.0) {
- }
-
- // Returns true if the frisbee is controlled by the transfer roller.
- bool IsTouchingTransfer(double position) const {
- return (position >= IndexMotor::kBottomDiscDetectStart &&
- position <= IndexMotor::kIndexStartPosition);
- }
- bool IsTouchingTransfer() const { return IsTouchingTransfer(position_); }
-
- // Returns true if the frisbee is in a place where it is unsafe to grab.
- bool IsUnsafeToGrab() const {
- return (position_ > (IndexMotor::kLoaderFreeStopPosition) &&
- position_ < IndexMotor::kGrabberStartPosition);
- }
-
- // Returns true if the frisbee is controlled by the indexing roller.
- bool IsTouchingIndex(double position) const {
- return (position >= IndexMotor::kIndexStartPosition &&
- position < IndexMotor::kGrabberStartPosition);
- }
- bool IsTouchingIndex() const { return IsTouchingIndex(position_); }
-
- // Returns true if the frisbee is in a position such that the disc can be
- // lifted.
- bool IsUnsafeToLift() const {
- return (position_ >= IndexMotor::kLoaderFreeStopPosition &&
- position_ <= IndexMotor::kReadyToLiftPosition);
- }
-
- // Returns true if the frisbee is in a position such that the grabber will
- // pull it into the loader.
- bool IsTouchingGrabber() const {
- return (position_ >= IndexMotor::kGrabberStartPosition &&
- position_ < IndexMotor::kReadyToLiftPosition);
- }
-
- // Returns true if the frisbee is in a position such that the disc can be
- // lifted.
- bool IsTouchingLoader() const {
- return (position_ >= IndexMotor::kReadyToLiftPosition &&
- position_ < IndexMotor::kLifterStopPosition);
- }
-
- // Returns true if the frisbee is touching the ejector.
- bool IsTouchingEjector() const {
- return (position_ >= IndexMotor::kLifterStopPosition &&
- position_ < IndexMotor::kEjectorStopPosition);
- }
-
- // Returns true if the disc is triggering the bottom disc detect sensor.
- bool bottom_disc_detect(double position) const {
- return (position >= IndexMotor::kBottomDiscDetectStart &&
- position <= IndexMotor::kBottomDiscDetectStop);
- }
- bool bottom_disc_detect() const { return bottom_disc_detect(position_); }
-
- // Returns true if the disc is triggering the top disc detect sensor.
- bool top_disc_detect(double position) const {
- return (position >= IndexMotor::kTopDiscDetectStart &&
- position <= IndexMotor::kTopDiscDetectStop);
- }
- bool top_disc_detect() const { return top_disc_detect(position_); }
-
- // Returns true if the bottom disc sensor will negedge after the disc moves
- // by dx.
- bool will_negedge_bottom_disc_detect(double disc_dx) {
- if (bottom_disc_detect()) {
- return !bottom_disc_detect(position_ + disc_dx);
- }
- return false;
- }
-
- // Returns true if the bottom disc sensor will posedge after the disc moves
- // by dx.
- bool will_posedge_top_disc_detect(double disc_dx) {
- if (!top_disc_detect()) {
- return top_disc_detect(position_ + disc_dx);
- }
- return false;
- }
-
- // Returns true if the bottom disc sensor will negedge after the disc moves
- // by dx.
- bool will_negedge_top_disc_detect(double disc_dx) {
- if (top_disc_detect()) {
- return !top_disc_detect(position_ + disc_dx);
- }
- return false;
- }
-
- // Handles potentially dealing with the delayed negedge.
- // Computes the index position when time expires using the cached old indexer
- // position, the elapsed time, and the average velocity.
- void HandleAfterNegedge(
- double index_velocity, double elapsed_time, double time_left) {
- if (!has_bottom_disc_negedge_wait_position_) {
- if (after_negedge_time_left_ < time_left) {
- // Assume constant velocity and compute the position.
- bottom_disc_negedge_wait_position_ =
- index_roller_position_ +
- index_velocity * (elapsed_time + after_negedge_time_left_);
- after_negedge_time_left_ = 0.0;
- has_bottom_disc_negedge_wait_position_ = true;
- } else {
- after_negedge_time_left_ -= time_left;
- }
- }
- }
-
- // Updates the position of the disc assuming that it has started on the
- // transfer. The elapsed time is the simulated amount of time that has
- // elapsed since the simulation timestep started and this method was called.
- // time_left is the amount of time left to spend during this timestep.
- double UpdateTransferPositionForTime(double transfer_roller_velocity,
- double index_roller_velocity,
- double elapsed_time,
- double time_left) {
- double disc_dx = IndexMotor::ConvertTransferToDiscPosition(
- transfer_roller_velocity * time_left);
- bool shrunk_time = false;
- if (!IsTouchingTransfer(position_ + disc_dx)) {
- shrunk_time = true;
- time_left = (IndexMotor::kIndexStartPosition - position_) /
- transfer_roller_velocity;
- disc_dx = IndexMotor::ConvertTransferToDiscPosition(
- transfer_roller_velocity * time_left);
- }
-
- if (will_negedge_bottom_disc_detect(disc_dx)) {
- // Compute the time from the negedge to the end of the cycle assuming
- // constant velocity.
- const double elapsed_time =
- (position_ + disc_dx - IndexMotor::kBottomDiscDetectStop) /
- disc_dx * time_left;
-
- // I am not implementing very short delays until this fails.
- assert(elapsed_time <= after_negedge_time_left_);
- after_negedge_time_left_ -= elapsed_time;
- } else if (position_ >= IndexMotor::kBottomDiscDetectStop) {
- HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
- }
-
- if (shrunk_time) {
- EXPECT_LT(0, transfer_roller_velocity);
- position_ = IndexMotor::kIndexStartPosition;
- } else {
- position_ += disc_dx;
- }
- LOG(DEBUG, "Transfer Roller: Disc is at %f\n", position_);
- return time_left;
- }
-
- // Updates the position of the disc assuming that it has started on the
- // indexer. The elapsed time is the simulated amount of time that has
- // elapsed since the simulation timestep started and this method was called.
- // time_left is the amount of time left to spend during this timestep.
- double UpdateIndexPositionForTime(double index_roller_velocity,
- double elapsed_time,
- double time_left) {
- double index_dx = IndexMotor::ConvertIndexToDiscPosition(
- index_roller_velocity * time_left);
- bool shrunk_time = false;
- if (!IsTouchingIndex(position_ + index_dx)) {
- shrunk_time = true;
- time_left = (IndexMotor::kGrabberStartPosition - position_) /
- index_roller_velocity;
- index_dx = IndexMotor::ConvertTransferToDiscPosition(
- index_roller_velocity * time_left);
- }
-
- if (position_ >= IndexMotor::kBottomDiscDetectStop) {
- HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
- }
-
- if (will_posedge_top_disc_detect(index_dx)) {
- // Wohoo! Find the edge.
- // Assume constant velocity and compute the position.
- double edge_position = index_roller_velocity > 0 ?
- IndexMotor::kTopDiscDetectStart : IndexMotor::kTopDiscDetectStop;
- const double disc_time =
- (edge_position - position_) / index_roller_velocity;
- top_disc_posedge_position_ = index_roller_position_ +
- IndexMotor::ConvertDiscPositionToIndex(
- index_roller_velocity * (elapsed_time + disc_time));
- has_top_disc_posedge_position_ = true;
- LOG(INFO, "Posedge on top sensor at %f\n", top_disc_posedge_position_);
- }
-
- if (will_negedge_top_disc_detect(index_dx)) {
- // Wohoo! Find the edge.
- // Assume constant velocity and compute the position.
- double edge_position = index_roller_velocity > 0 ?
- IndexMotor::kTopDiscDetectStop : IndexMotor::kTopDiscDetectStart;
- const double disc_time =
- (edge_position - position_) / index_roller_velocity;
- top_disc_negedge_position_ = index_roller_position_ +
- IndexMotor::ConvertDiscPositionToIndex(
- index_roller_velocity * (elapsed_time + disc_time));
- has_top_disc_negedge_position_ = true;
- LOG(INFO, "Negedge on top sensor at %f\n", top_disc_negedge_position_);
- }
-
- if (shrunk_time) {
- if (index_roller_velocity > 0) {
- position_ = IndexMotor::kGrabberStartPosition;
- } else {
- position_ = IndexMotor::kIndexStartPosition;
- }
- } else {
- position_ += index_dx;
- }
- LOG(DEBUG, "Index: Disc is at %f\n", position_);
- return time_left;
- }
-
- // Updates the position given velocities, piston comands, and the time left in
- // the simulation cycle.
- void UpdatePositionForTime(double transfer_roller_velocity,
- double index_roller_velocity,
- bool clamped,
- bool lifted,
- bool ejected,
- double time_left) {
- double elapsed_time = 0.0;
- // We are making this assumption below
- ASSERT_LE(IndexMotor::kBottomDiscDetectStop,
- IndexMotor::kIndexStartPosition);
- if (IsTouchingTransfer() || position() < 0.0) {
- double deltat = UpdateTransferPositionForTime(
- transfer_roller_velocity, index_roller_velocity,
- elapsed_time, time_left);
- time_left -= deltat;
- elapsed_time += deltat;
- }
-
- if (IsTouchingIndex() && time_left >= 0) {
- // Verify that we aren't trying to grab or lift when it isn't safe.
- EXPECT_FALSE(clamped && IsUnsafeToGrab());
- EXPECT_FALSE(lifted && IsUnsafeToLift());
-
- double deltat = UpdateIndexPositionForTime(
- index_roller_velocity, elapsed_time, time_left);
- time_left -= deltat;
- elapsed_time += deltat;
- }
- if (IsTouchingGrabber()) {
- if (clamped) {
- const double grabber_dx =
- IndexMotor::kGrabberMovementVelocity * time_left;
- position_ = ::std::min(position_ + grabber_dx,
- IndexMotor::kReadyToLiftPosition);
- }
- EXPECT_FALSE(lifted) << "Can't lift while in grabber";
- EXPECT_FALSE(ejected) << "Can't eject while in grabber";
- LOG(DEBUG, "Grabber: Disc is at %f\n", position_);
- } else if (IsTouchingLoader()) {
- if (lifted) {
- const double lifter_dx =
- IndexMotor::kLifterMovementVelocity * time_left;
- position_ = ::std::min(position_ + lifter_dx,
- IndexMotor::kLifterStopPosition);
- }
- EXPECT_TRUE(clamped);
- EXPECT_FALSE(ejected);
- LOG(DEBUG, "Loader: Disc is at %f\n", position_);
- } else if (IsTouchingEjector()) {
- EXPECT_TRUE(lifted);
- if (ejected) {
- const double ejector_dx =
- IndexMotor::kEjectorMovementVelocity * time_left;
- position_ = ::std::min(position_ + ejector_dx,
- IndexMotor::kEjectorStopPosition);
- EXPECT_FALSE(clamped);
- }
- LOG(DEBUG, "Ejector: Disc is at %f\n", position_);
- } else if (position_ == IndexMotor::kEjectorStopPosition) {
- LOG(DEBUG, "Shot: Disc is at %f\n", position_);
- has_been_shot_ = true;
- }
- }
-
- // Updates the position of the frisbee in the frisbee path.
- void UpdatePosition(double transfer_roller_position,
- double index_roller_position,
- bool clamped,
- bool lifted,
- bool ejected) {
- const double transfer_roller_velocity =
- (transfer_roller_position - transfer_roller_position_) / 0.01;
- const double index_roller_velocity =
- (index_roller_position - index_roller_position_) / 0.01;
- UpdatePositionForTime(transfer_roller_velocity,
- index_roller_velocity,
- clamped,
- lifted,
- ejected,
- 0.01);
- transfer_roller_position_ = transfer_roller_position;
- index_roller_position_ = index_roller_position;
- }
-
- // Returns if the disc has been shot and can be removed from the robot.
- bool has_been_shot() const {
- return has_been_shot_;
- }
-
- // Returns the position of the disc in the system.
- double position() const {
- return position_;
- }
-
- // Sets whether or not we have counted the delayed negedge.
- void set_counted_negedge_wait(bool counted_negedge_wait) {
- counted_negedge_wait_ = counted_negedge_wait;
- }
-
- // Returns if we have counted the delayed negedge.
- bool counted_negedge_wait() { return counted_negedge_wait_; }
-
- // Returns true if the negedge wait position is valid.
- bool has_bottom_disc_negedge_wait_position() {
- return has_bottom_disc_negedge_wait_position_;
- }
-
- // Returns the negedge wait position.
- double bottom_disc_negedge_wait_position() {
- return bottom_disc_negedge_wait_position_;
- }
-
- // Returns the last position where a posedge was seen.
- double top_disc_posedge_position() { return top_disc_posedge_position_; }
-
- // Returns the last position where a negedge was seen.
- double top_disc_negedge_position() { return top_disc_negedge_position_; }
-
- // True if the top disc has seen a posedge.
- // Reading this flag clears it.
- bool has_top_disc_posedge_position() {
- bool prev = has_top_disc_posedge_position_;
- has_top_disc_posedge_position_ = false;
- return prev;
- }
-
- // True if the top disc has seen a negedge.
- // Reading this flag clears it.
- bool has_top_disc_negedge_position() {
- bool prev = has_top_disc_negedge_position_;
- has_top_disc_negedge_position_ = false;
- return prev;
- }
-
- // Simulates the index roller moving without the disc moving.
- void OffsetIndex(double offset) {
- index_roller_position_ += offset;
- }
-
- private:
- // Previous transfer roller position for computing deltas.
- double transfer_roller_position_;
- // Previous index roller position for computing deltas.
- double index_roller_position_;
- // Position in the robot.
- double position_;
- // True if the disc has been shot.
- bool has_been_shot_;
- // True if the delay after the negedge of the beam break has occured.
- bool has_bottom_disc_negedge_wait_position_;
- // Posiiton of the indexer when the delayed negedge occures.
- double bottom_disc_negedge_wait_position_;
- // Time left after the negedge before we need to sample the indexer position.
- double after_negedge_time_left_;
- // Bool for the user to record if they have counted the negedge from this
- // disc.
- bool counted_negedge_wait_;
- // True if the top disc sensor posedge has occured and
- // hasn't been counted yet.
- bool has_top_disc_posedge_position_;
- // The position at which the posedge occured.
- double top_disc_posedge_position_;
- // True if the top disc sensor negedge has occured and
- // hasn't been counted yet.
- bool has_top_disc_negedge_position_;
- // The position at which the negedge occured.
- double top_disc_negedge_position_;
-};
-
-
-// Class which simulates the index and sends out queue messages containing the
-// position.
-class IndexMotorSimulation {
- public:
- // Constructs a motor simulation. initial_position is the inital angle of the
- // index, which will be treated as 0 by the encoder.
- IndexMotorSimulation()
- : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
- transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
- bottom_disc_posedge_count_(0),
- bottom_disc_negedge_count_(0),
- bottom_disc_negedge_wait_count_(0),
- bottom_disc_negedge_wait_position_(0),
- top_disc_posedge_count_(0),
- top_disc_posedge_position_(0.0),
- top_disc_negedge_count_(0),
- top_disc_negedge_position_(0.0),
- my_index_loop_(".frc971.control_loops.index",
- 0x1a7b7094, ".frc971.control_loops.index.goal",
- ".frc971.control_loops.index.position",
- ".frc971.control_loops.index.output",
- ".frc971.control_loops.index.status") {
- }
-
- // Starts a disc offset from the start of the index.
- void InsertDisc(double offset = IndexMotor::kBottomDiscDetectStart - 0.001) {
- Frisbee new_frisbee(transfer_roller_position(),
- index_roller_position(),
- offset);
- ASSERT_FALSE(new_frisbee.bottom_disc_detect());
- ASSERT_FALSE(new_frisbee.top_disc_detect());
- frisbees.push_back(new_frisbee);
- }
-
- // Returns true if the bottom disc sensor is triggered.
- bool BottomDiscDetect() const {
- bool bottom_disc_detect = false;
- for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
- bottom_disc_detect |= frisbee->bottom_disc_detect();
- }
- return bottom_disc_detect;
- }
-
- // Returns true if the top disc sensor is triggered.
- bool TopDiscDetect() const {
- bool top_disc_detect = false;
- for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
- top_disc_detect |= frisbee->top_disc_detect();
- }
- return top_disc_detect;
- }
-
- // Updates all discs, and verifies that the state of the system is sane.
- void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
- for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
- const bool old_bottom_disc_detect = frisbee->bottom_disc_detect();
- frisbee->UpdatePosition(transfer_roller_position(),
- index_roller_position(),
- clamped,
- lifted,
- ejected);
-
- // Look for disc detect edges and report them.
- const bool bottom_disc_detect = frisbee->bottom_disc_detect();
- if (old_bottom_disc_detect && !bottom_disc_detect) {
- LOG(INFO, "Negedge of disc\n");
- ++bottom_disc_negedge_count_;
- }
-
- if (!old_bottom_disc_detect && frisbee->bottom_disc_detect()) {
- LOG(INFO, "Posedge of disc\n");
- ++bottom_disc_posedge_count_;
- }
-
- // See if the frisbee has a delayed negedge and encoder value to report
- // back.
- if (frisbee->has_bottom_disc_negedge_wait_position()) {
- if (!frisbee->counted_negedge_wait()) {
- bottom_disc_negedge_wait_position_ =
- frisbee->bottom_disc_negedge_wait_position();
- ++bottom_disc_negedge_wait_count_;
- frisbee->set_counted_negedge_wait(true);
- }
- }
- if (frisbee->has_top_disc_posedge_position()) {
- ++top_disc_posedge_count_;
- top_disc_posedge_position_ = frisbee->top_disc_posedge_position();
- }
- if (frisbee->has_top_disc_negedge_position()) {
- ++top_disc_negedge_count_;
- top_disc_negedge_position_ = frisbee->top_disc_negedge_position();
- }
- }
-
- // Make sure nobody is too close to anybody else.
- Frisbee *last_frisbee = NULL;
- for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
- if (last_frisbee) {
- const double distance = frisbee->position() - last_frisbee->position();
- double min_distance;
- if (frisbee->IsTouchingTransfer() ||
- last_frisbee->IsTouchingTransfer()) {
- min_distance = 0.3;
- } else {
- min_distance =
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI * 2.0 / 3.0);
- }
-
- EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close";
- }
- last_frisbee = &*frisbee;
- }
-
- // Remove any shot frisbees.
- for (int i = 0; i < static_cast<int>(frisbees.size()); ++i) {
- if (frisbees[i].has_been_shot()) {
- shot_frisbees.push_back(frisbees[i]);
- frisbees.erase(frisbees.begin() + i);
- --i;
- }
- }
- }
-
- // Sends out the position queue messages.
- void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
- my_index_loop_.position.MakeMessage();
- position->index_position = index_roller_position();
- position->bottom_disc_detect = BottomDiscDetect();
- position->top_disc_detect = TopDiscDetect();
- position->bottom_disc_posedge_count = bottom_disc_posedge_count_;
- position->bottom_disc_negedge_count = bottom_disc_negedge_count_;
- position->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
- position->bottom_disc_negedge_wait_position =
- bottom_disc_negedge_wait_position_;
- position->top_disc_posedge_count = top_disc_posedge_count_;
- position->top_disc_posedge_position = top_disc_posedge_position_;
- position->top_disc_negedge_count = top_disc_negedge_count_;
- position->top_disc_negedge_position = top_disc_negedge_position_;
- LOG(DEBUG,
- "bdd: %x tdd: %x posedge %d negedge %d "
- "delaycount %d delaypos %f topcount %d toppos %f "
- "topcount %d toppos %f\n",
- position->bottom_disc_detect,
- position->top_disc_detect,
- position->bottom_disc_posedge_count,
- position->bottom_disc_negedge_count,
- position->bottom_disc_negedge_wait_count,
- position->bottom_disc_negedge_wait_position,
- position->top_disc_posedge_count,
- position->top_disc_posedge_position,
- position->top_disc_negedge_count,
- position->top_disc_negedge_position);
- position.Send();
- }
-
- // Simulates the index moving for one timestep.
- void Simulate() {
- EXPECT_TRUE(my_index_loop_.output.FetchLatest());
-
- index_plant_->set_plant_index(frisbees.size());
- index_plant_->U << my_index_loop_.output->index_voltage;
- index_plant_->Update();
-
- transfer_plant_->U << my_index_loop_.output->transfer_voltage;
- transfer_plant_->Update();
- LOG(DEBUG, "tv: %f iv: %f tp : %f ip: %f\n",
- my_index_loop_.output->transfer_voltage,
- my_index_loop_.output->index_voltage,
- transfer_roller_position(), index_roller_position());
-
- UpdateDiscs(my_index_loop_.output->disc_clamped,
- my_index_loop_.output->loader_up,
- my_index_loop_.output->disc_ejected);
- }
-
- // Simulates the index roller moving without the disc moving.
- void OffsetIndices(double offset) {
- for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
- frisbee->OffsetIndex(offset);
- }
- }
-
- // Plants for the index and transfer rollers.
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
-
- // Posedge and negedge counts for the beam break.
- int32_t bottom_disc_posedge_count_;
- int32_t bottom_disc_negedge_count_;
-
- // Delayed negedge count and corrisponding position.
- int32_t bottom_disc_negedge_wait_count_;
- int32_t bottom_disc_negedge_wait_position_;
-
- // Posedge count and position for the upper disc sensor.
- int32_t top_disc_posedge_count_;
- double top_disc_posedge_position_;
-
- // Negedge count and position for the upper disc sensor.
- int32_t top_disc_negedge_count_;
- double top_disc_negedge_position_;
-
- // Returns the absolute angle of the index.
- double index_roller_position() const {
- return index_plant_->Y(0, 0);
- }
-
- // Returns the absolute angle of the index.
- double transfer_roller_position() const {
- return transfer_plant_->Y(0, 0);
- }
-
- // Frisbees being tracked in the robot.
- ::std::vector<Frisbee> frisbees;
- // Frisbees that have been shot.
- ::std::vector<Frisbee> shot_frisbees;
-
- private:
- // Control loop for the indexer.
- IndexLoop my_index_loop_;
-};
-
-
-class IndexTest : public ::testing::Test {
- protected:
- IndexTest() : my_index_loop_(".frc971.control_loops.index",
- 0x1a7b7094, ".frc971.control_loops.index.goal",
- ".frc971.control_loops.index.position",
- ".frc971.control_loops.index.output",
- ".frc971.control_loops.index.status"),
- index_motor_(&my_index_loop_),
- index_motor_plant_(),
- loop_count_(0) {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- ::frc971::control_loops::shooter.goal.Clear();
- ::frc971::control_loops::shooter.status.Clear();
- ::frc971::control_loops::shooter.output.Clear();
- ::frc971::control_loops::shooter.position.Clear();
- SendDSPacket(true);
- Time::EnableMockTime(Time(0, 0));
- }
-
- virtual ~IndexTest() {
- ::aos::robot_state.Clear();
- ::frc971::control_loops::shooter.goal.Clear();
- ::frc971::control_loops::shooter.status.Clear();
- ::frc971::control_loops::shooter.output.Clear();
- ::frc971::control_loops::shooter.position.Clear();
- Time::DisableMockTime();
- }
-
- // Sends a DS packet with the enable bit set to enabled.
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
- }
-
- // Updates the current mock time.
- void UpdateTime() {
- loop_count_ += 1;
- Time::SetMockTime(Time::InMS(10 * loop_count_));
- }
-
- // Lets N cycles of time pass.
- void SimulateNCycles(int cycles) {
- for (int i = 0; i < cycles; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
- }
-
- // Loads n discs into the indexer at the bottom.
- void LoadNDiscs(int n) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- // Spin it up.
- SimulateNCycles(100);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
-
- // Stuff N discs in, waiting between each one for a tiny bit of time so they
- // don't get too close.
- int num_grabbed = 0;
- int wait_counter = 0;
- while (num_grabbed < n) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- if (!index_motor_plant_.BottomDiscDetect()) {
- if (wait_counter > 0) {
- --wait_counter;
- } else {
- index_motor_plant_.InsertDisc();
- ++num_grabbed;
- wait_counter = 10;
- }
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-
- // Settle.
- int settling_time =
- static_cast<int>(IndexMotor::kTransferOffDelay.ToSeconds() * 100.0) + 2;
- for (int i = 0; i < 100; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- my_index_loop_.output.FetchLatest();
- my_index_loop_.status.FetchLatest();
- if (i <= settling_time || my_index_loop_.status->hopper_disc_count < 4) {
- EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
- } else {
- EXPECT_EQ(0.0, my_index_loop_.output->transfer_voltage);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
- }
-
- // Loads 2 discs, and then offsets them. We then send the first disc to the
- // grabber, and the second disc back down to the bottom. Verify that both
- // discs get found correctly. Positive numbers shift the discs up.
- void TestDualLostDiscs(double top_disc_offset, double bottom_disc_offset) {
- LoadNDiscs(2);
-
- // Move them in the indexer so they need to be re-found.
- // The top one is moved further than the bottom one so that both edges need to
- // be inspected.
- index_motor_plant_.frisbees[0].OffsetIndex(
- IndexMotor::ConvertDiscPositionToIndex(top_disc_offset));
- index_motor_plant_.frisbees[1].OffsetIndex(
- IndexMotor::ConvertDiscPositionToIndex(bottom_disc_offset));
-
- // Lift the discs up to the top. Wait a while to let the system settle and
- // verify that they don't collide.
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(300);
-
- // Verify that the disc has been grabbed.
- my_index_loop_.output.FetchLatest();
- EXPECT_TRUE(my_index_loop_.output->disc_clamped);
- // And that we are preloaded.
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->preloaded);
-
- // Pull the disc back down.
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- SimulateNCycles(300);
-
- EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
- index_motor_plant_.frisbees[0].position(), 0.01);
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.02);
-
- // Verify that we found the disc as accurately as the FPGA allows.
- my_index_loop_.position.FetchLatest();
- EXPECT_NEAR(
- index_motor_.frisbees()[0].absolute_position(
- my_index_loop_.position->index_position),
- index_motor_plant_.frisbees[1].position(), 0.0001);
- }
-
- // Copy of core that works in this process only.
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- IndexLoop my_index_loop_;
-
- // Create a loop and simulation plant.
- IndexMotor index_motor_;
- IndexMotorSimulation index_motor_plant_;
-
- // Number of loop cycles that have been executed for tracking the current
- // time.
- int loop_count_;
-};
-
-// Tests that the index grabs 1 disc and places it at the correct position.
-TEST_F(IndexTest, GrabSingleDisc) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 250; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- if (i == 100) {
- EXPECT_EQ(0, index_motor_plant_.index_roller_position());
- index_motor_plant_.InsertDisc();
- }
- if (i > 0) {
- EXPECT_TRUE(my_index_loop_.status.FetchLatest());
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
- EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
- index_motor_plant_.frisbees[0].position(), 0.05);
-}
-
-// Tests that the index grabs 1 disc and places it at the correct position when
-// told to hold immediately after the disc starts into the bot.
-TEST_F(IndexTest, GrabAndHold) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 200; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- if (i == 100) {
- EXPECT_EQ(0, index_motor_plant_.index_roller_position());
- index_motor_plant_.InsertDisc();
- } else if (i == 102) {
- // The disc has been seen. Tell the indexer to now hold.
- my_index_loop_.goal.MakeWithBuilder().goal_state(0).Send();
- } else if (i > 102) {
- my_index_loop_.status.FetchLatest();
- EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
- EXPECT_EQ(0, my_index_loop_.status->shot_disc_count);
- EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[0].position(), 0.04);
-}
-
-// Tests that the index grabs two discs and places them at the correct
-// positions.
-TEST_F(IndexTest, GrabTwoDiscs) {
- LoadNDiscs(2);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
- EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.10);
- EXPECT_NEAR(
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
- (index_motor_plant_.frisbees[0].position() -
- index_motor_plant_.frisbees[1].position()), 0.10);
-}
-
-// Tests that the index grabs 2 discs, and loads one up into the loader to get
-// ready to shoot. It then pulls the second disc back down to be ready to
-// intake more.
-TEST_F(IndexTest, ReadyGrabsOneDisc) {
- LoadNDiscs(2);
-
- // Lift the discs up to the top. Wait a while to let the system settle and
- // verify that they don't collide.
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(300);
-
- // Verify that the disc has been grabbed.
- my_index_loop_.output.FetchLatest();
- EXPECT_TRUE(my_index_loop_.output->disc_clamped);
- // And that we are preloaded.
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->preloaded);
-
- // Pull the disc back down and verify that the transfer roller doesn't turn on
- // until we are ready.
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 100; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
-
- EXPECT_TRUE(my_index_loop_.status.FetchLatest());
- EXPECT_TRUE(my_index_loop_.output.FetchLatest());
- if (!my_index_loop_.status->ready_to_intake) {
- EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0)
- << "Transfer should be off until indexer is ready";
- }
-
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
- EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
- my_index_loop_.output.FetchLatest();
- EXPECT_TRUE(my_index_loop_.output->disc_clamped);
-
- EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
- index_motor_plant_.frisbees[0].position(), 0.01);
- LOG(INFO, "Top disc error is %f\n",
- IndexMotor::kReadyToLiftPosition -
- index_motor_plant_.frisbees[0].position());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.02);
- LOG(INFO, "Bottom disc error is %f\n",
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI))-
- index_motor_plant_.frisbees[1].position());
-}
-
-// Tests that the index grabs 1 disc and continues to pull it in correctly when
-// in the READY_LOWER state. The transfer roller should be disabled then.
-TEST_F(IndexTest, GrabAndReady) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 200; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- if (i == 100) {
- EXPECT_EQ(0, index_motor_plant_.index_roller_position());
- index_motor_plant_.InsertDisc();
- } else if (i == 102) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
- } else if (i > 150) {
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
- EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[0].position(), 0.04);
-}
-
-// Tests that grabbing 4 discs ends up with 4 discs in the bot and us no longer
-// ready.
-TEST_F(IndexTest, GrabFourDiscs) {
- LoadNDiscs(4);
-
- my_index_loop_.output.FetchLatest();
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 4);
- EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
- EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- IndexMotor::kReadyToLiftPosition,
- index_motor_plant_.frisbees[0].position(), 0.001);
-
- EXPECT_NEAR(
- IndexMotor::kReadyToPreload,
- index_motor_plant_.frisbees[1].position(), 0.01);
-
- EXPECT_NEAR(
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
- (index_motor_plant_.frisbees[1].position() -
- index_motor_plant_.frisbees[2].position()), 0.10);
- EXPECT_NEAR(
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
- (index_motor_plant_.frisbees[2].position() -
- index_motor_plant_.frisbees[3].position()), 0.10);
-}
-
-// Tests that shooting 4 discs works.
-TEST_F(IndexTest, ShootFourDiscs) {
- LoadNDiscs(4);
-
- EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
-
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
-
- // Lifting and shooting takes a while...
- SimulateNCycles(300);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
- EXPECT_EQ(my_index_loop_.status->total_disc_count, 4);
- my_index_loop_.output.FetchLatest();
- EXPECT_FALSE(my_index_loop_.output->disc_clamped);
- EXPECT_FALSE(my_index_loop_.output->loader_up);
- EXPECT_FALSE(my_index_loop_.output->disc_ejected);
-
- EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.shot_frisbees.size());
-}
-
-// Tests that discs aren't pulled out of the loader half way through being
-// grabbed when being asked to index.
-TEST_F(IndexTest, PreloadToIndexEarlyTransition) {
- LoadNDiscs(2);
-
- // Lift the discs up to the top. Wait a while to let the system settle and
- // verify that they don't collide.
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- for (int i = 0; i < 300; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- // Drop out of the loop as soon as it enters the loader.
- // This will require it to finish the job before intaking more.
- my_index_loop_.status.FetchLatest();
- if (index_motor_plant_.frisbees[0].position() >
- IndexMotor::kLoaderFreeStopPosition) {
- break;
- }
- }
-
- // Pull the disc back down and verify that the transfer roller doesn't turn on
- // until we are ready.
- my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
- SimulateNCycles(100);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
- EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
- my_index_loop_.output.FetchLatest();
- EXPECT_TRUE(my_index_loop_.output->disc_clamped);
-
- EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
- index_motor_plant_.frisbees[0].position(), 0.01);
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.10);
-}
-
-// Tests that disabling while grabbing a disc doesn't cause problems.
-TEST_F(IndexTest, HandleDisable) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 200; ++i) {
- index_motor_plant_.SendPositionMessage();
- index_motor_.Iterate();
- if (i == 100) {
- EXPECT_EQ(0, index_motor_plant_.index_roller_position());
- index_motor_plant_.InsertDisc();
- } else if (i == 102) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
- } else if (i > 150) {
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(i < 102 || i > 110);
- UpdateTime();
- }
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
- EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[0].position(), 0.04);
-}
-
-// Tests that we can shoot after grabbing in the loader.
-TEST_F(IndexTest, GrabbedToShoot) {
- LoadNDiscs(2);
-
- // Lift the discs up to the top. Wait a while to let the system settle and
- // verify that they don't collide.
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(300);
-
- // Verify that it is preloaded.
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->preloaded);
-
- // Shoot them all.
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
- SimulateNCycles(200);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
- EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
- EXPECT_FALSE(my_index_loop_.status->preloaded);
-}
-
-// Tests that the cRIO can reboot and we don't loose discs.
-TEST_F(IndexTest, cRIOReboot) {
- LoadNDiscs(2);
-
- SimulateNCycles(100);
- for (int i = 0; i < 100; ++i) {
- // No position for a while is a cRIO reboot.
- index_motor_.Iterate();
- index_motor_plant_.Simulate();
- SendDSPacket(false);
- UpdateTime();
- }
-
- // Shift the plant.
- const double kPlantOffset = 5000.0;
- index_motor_plant_.index_plant_->Y(0, 0) += kPlantOffset;
- index_motor_plant_.index_plant_->X(0, 0) += kPlantOffset;
- index_motor_plant_.bottom_disc_posedge_count_ = 971;
- index_motor_plant_.bottom_disc_negedge_count_ = 971;
- index_motor_plant_.bottom_disc_negedge_wait_count_ = 971;
- index_motor_plant_.bottom_disc_negedge_wait_position_ = -1502;
-
- // Shift the discs
- index_motor_plant_.OffsetIndices(kPlantOffset);
- // Let time elapse to see if the loop wants to move the discs or not.
- SimulateNCycles(1000);
-
- // Verify that 2 discs are at the bottom of the hopper.
- EXPECT_TRUE(my_index_loop_.status.FetchLatest());
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
- EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
- EXPECT_NEAR(
- (IndexMotor::kIndexStartPosition +
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.10);
- EXPECT_NEAR(
- IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
- (index_motor_plant_.frisbees[0].position() -
- index_motor_plant_.frisbees[1].position()), 0.10);
-}
-
-// Tests that invalid states are rejected.
-TEST_F(IndexTest, InvalidStateTest) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(10).Send();
- SimulateNCycles(2);
- // Verify that the goal is correct.
- EXPECT_GE(4, static_cast<int>(index_motor_.safe_goal_));
- EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
-}
-
-// Tests that the motor is turned off after a number of cycles of low power.
-TEST_F(IndexTest, ZeroPowerAfterTimeout) {
- LoadNDiscs(4);
- SimulateNCycles(100);
-
- // Verify that the motor is hard off. This relies on floating point math
- // never really getting to 0 unless you set it explicitly.
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
-}
-
-// Tests that preloading 2 discs relocates the discs if they shift on the
-// indexer. Test shifting all 4 ways.
-TEST_F(IndexTest, ShiftedDiscsAreRefound) {
- TestDualLostDiscs(0.10, 0.15);
-}
-
-TEST_F(IndexTest, ShiftedDiscsAreRefoundOtherSeperation) {
- TestDualLostDiscs(0.15, 0.10);
-}
-
-TEST_F(IndexTest, ShiftedDownDiscsAreRefound) {
- TestDualLostDiscs(-0.15, -0.10);
-}
-
-TEST_F(IndexTest, ShiftedDownDiscsAreRefoundOtherSeperation) {
- TestDualLostDiscs(-0.10, -0.15);
-}
-
-// Verifies that the indexer is ready to intake imediately after loading.
-TEST_F(IndexTest, IntakingAfterLoading) {
- LoadNDiscs(1);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- SimulateNCycles(10);
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
-}
-
-// Verifies that the indexer is ready to intake imediately after loading.
-TEST_F(IndexTest, CanShootOneDiscAfterReady) {
- LoadNDiscs(1);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
- SimulateNCycles(100);
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(1, my_index_loop_.status->total_disc_count);
- EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
-}
-
-// Verifies that the indexer is ready to intake imediately after loading.
-TEST_F(IndexTest, GotExtraDisc) {
- LoadNDiscs(1);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
-
- double index_roller_position = index_motor_plant_.index_roller_position();
- index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.1);
- index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.6);
- SimulateNCycles(100);
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
- SimulateNCycles(300);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(3, my_index_loop_.status->total_disc_count);
- EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
- EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
- index_motor_plant_.index_roller_position() - index_roller_position);
-}
-
-// Verifies that the indexer is ready to intake imediately after loading.
-TEST_F(IndexTest, LostDisc) {
- LoadNDiscs(3);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
-
- index_motor_plant_.frisbees.erase(
- index_motor_plant_.frisbees.begin() + 1);
-
- double index_roller_position = index_motor_plant_.index_roller_position();
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
- SimulateNCycles(300);
-
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(2, my_index_loop_.status->total_disc_count);
- EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
- EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(3 * M_PI),
- index_motor_plant_.index_roller_position() - index_roller_position);
- EXPECT_EQ(0u, index_motor_.frisbees_.size());
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(0.0, my_index_loop_.output->index_voltage);
-}
-
-// Verifies that the indexer is ready to intake imediately after loading.
-TEST_F(IndexTest, CRIOReboot) {
- index_motor_plant_.index_plant_->Y(0, 0) = 5000.0;
- index_motor_plant_.index_plant_->X(0, 0) = 5000.0;
- LoadNDiscs(1);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- SimulateNCycles(10);
- my_index_loop_.output.FetchLatest();
- EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
- my_index_loop_.status.FetchLatest();
- EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
- EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
-}
-
-// Verifies that the indexer can shoot a disc and then intake and shoot another
-// one. This verifies that the code that forgets discs works correctly.
-TEST_F(IndexTest, CanShootIntakeAndShoot) {
- for (int i = 1; i < 4; ++i) {
- LoadNDiscs(1);
- my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
- SimulateNCycles(200);
- my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
- SimulateNCycles(500);
- my_index_loop_.status.FetchLatest();
- EXPECT_EQ(i, my_index_loop_.status->total_disc_count);
- EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
- EXPECT_EQ(i, my_index_loop_.status->shot_disc_count);
- }
-}
-
-// Tests that missing position packets don't cause the transfer motor
-// to turn off.
-TEST_F(IndexTest, NoPositionDoesNotTurnOff) {
- my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
- for (int i = 0; i < 250; ++i) {
- if (i % 10) {
- index_motor_plant_.SendPositionMessage();
- }
- index_motor_.Iterate();
-
- if (i > 1 && i % 10) {
- EXPECT_TRUE(my_index_loop_.output.FetchLatest());
- EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
- }
- index_motor_plant_.Simulate();
- SendDSPacket(true);
- UpdateTime();
- }
-}
-
-} // namespace testing
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/index/index_main.cc b/frc971/control_loops/index/index_main.cc
deleted file mode 100644
index 321d74c..0000000
--- a/frc971/control_loops/index/index_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/index/index.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- frc971::control_loops::IndexMotor indexer;
- indexer.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
deleted file mode 100644
index 9530a4f..0000000
--- a/frc971/control_loops/index/index_motor.q
+++ /dev/null
@@ -1,96 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-queue_group IndexLoop {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The state for the indexer to be in.
- // 0 means hold position, in a low power state.
- // 1 means get ready to load discs by shifting the discs down.
- // 2 means ready the discs, spin up the transfer roller, and accept discs.
- // 3 means get ready to shoot, and place a disc grabbed in the loader.
- // 4 means shoot at will.
- // 5 means re-initialize
- int32_t goal_state;
- // Forces the loader to fire.
- bool force_fire;
-
- // If true, set the indexer voltage to index_voltage.
- bool override_index;
- double index_voltage;
- bool override_transfer;
- double transfer_voltage;
- };
-
- message Position {
- // Index position
- double index_position;
-
- // Current values of both sensors.
- bool top_disc_detect;
- bool bottom_disc_detect;
- // Counts for positive and negative edges on the bottom sensor.
- int32_t bottom_disc_posedge_count;
- int32_t bottom_disc_negedge_count;
- // The most recent encoder position read after the most recent
- // negedge and a count of how many times it's been updated.
- double bottom_disc_negedge_wait_position;
- int32_t bottom_disc_negedge_wait_count;
- // The most recent index position at the posedge of the top disc detect
- // and a count of how many edges have been seen.
- int32_t top_disc_posedge_count;
- double top_disc_posedge_position;
- // The most recent index position at the negedge of the top disc detect
- // and a count of how many edges have been seen.
- int32_t top_disc_negedge_count;
- double top_disc_negedge_position;
-
- // Whether the hall effects for the loader are triggered (have a magnet in
- // front of them).
- bool loader_top;
- bool loader_bottom;
- };
-
- message Output {
- // Intake roller(s) output voltage.
- // Positive means into the robot.
- double intake_voltage;
- // Transfer roller output voltage.
- // Positive means into the robot.
- double transfer_voltage;
- // Index roller. Positive means up towards the shooter.
- double index_voltage;
- // Loader pistons.
- bool disc_clamped;
- bool loader_up;
- bool disc_ejected;
- };
-
- message Status {
- // Number of discs in the hopper
- int32_t hopper_disc_count;
- // Number of discs seen by the hopper.
- int32_t total_disc_count;
- // Number of discs shot by the shooter.
- int32_t shot_disc_count;
- // Disc ready in the loader.
- bool preloaded;
- // Indexer ready to accept more discs.
- bool ready_to_intake;
- // True from when we're committed to shooting util after the disk is clear
- // of the robot.
- bool is_shooting;
- // Goes false when we first get a disk and back true after we finish
- // clearing.
- bool hopper_clear;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-queue_group IndexLoop index_loop;
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
deleted file mode 100644
index 10fa60c..0000000
--- a/frc971/control_loops/index/index_motor_plant.cc
+++ /dev/null
@@ -1,151 +0,0 @@
-#include "frc971/control_loops/index/index_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00832470485812, 0.0, 0.68478614982;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.06201698456, 11.6687573378;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.0490373507155, 9.35402266105;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.0490373507155, 9.35402266105;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00901822957243, 0.0, 0.810292182273;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.0363437103863, 7.02270693014;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00927953099869, 0.0, 0.859452713637;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.0266707124098, 5.20285570613;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeIndex0DiscController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.58478614982, 48.4122215588;
- Eigen::Matrix<double, 1, 2> K;
- K << 1.90251621122, 0.0460029989298;
- return StateFeedbackController<2, 1, 1>(L, K, MakeIndex0DiscPlantCoefficients());
-}
-
-StateFeedbackController<2, 1, 1> MakeIndex1DiscController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.64731520998, 56.0569452572;
- Eigen::Matrix<double, 1, 2> K;
- K << 2.37331047876, 0.0642434141389;
- return StateFeedbackController<2, 1, 1>(L, K, MakeIndex1DiscPlantCoefficients());
-}
-
-StateFeedbackController<2, 1, 1> MakeIndex2DiscController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.64731520998, 56.0569452572;
- Eigen::Matrix<double, 1, 2> K;
- K << 2.37331047876, 0.0642434141389;
- return StateFeedbackController<2, 1, 1>(L, K, MakeIndex2DiscPlantCoefficients());
-}
-
-StateFeedbackController<2, 1, 1> MakeIndex3DiscController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71029218227, 64.1044007344;
- Eigen::Matrix<double, 1, 2> K;
- K << 3.16117420545, 0.0947502706704;
- return StateFeedbackController<2, 1, 1>(L, K, MakeIndex3DiscPlantCoefficients());
-}
-
-StateFeedbackController<2, 1, 1> MakeIndex4DiscController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.75945271364, 70.6153894746;
- Eigen::Matrix<double, 1, 2> K;
- K << 4.26688750446, 0.137549804289;
- return StateFeedbackController<2, 1, 1>(L, K, MakeIndex4DiscPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(5);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex0DiscPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex1DiscPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex2DiscPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex3DiscPlantCoefficients());
- plants[4] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex4DiscPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(5);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeIndex0DiscController());
- controllers[1] = new StateFeedbackController<2, 1, 1>(MakeIndex1DiscController());
- controllers[2] = new StateFeedbackController<2, 1, 1>(MakeIndex2DiscController());
- controllers[3] = new StateFeedbackController<2, 1, 1>(MakeIndex3DiscController());
- controllers[4] = new StateFeedbackController<2, 1, 1>(MakeIndex4DiscController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
deleted file mode 100644
index e82db6a..0000000
--- a/frc971/control_loops/index/index_motor_plant.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeIndex0DiscController();
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeIndex1DiscController();
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeIndex2DiscController();
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeIndex3DiscController();
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeIndex4DiscController();
-
-StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
deleted file mode 100644
index 0852b26..0000000
--- a/frc971/control_loops/index/transfer_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/index/transfer_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.0490373507155, 9.35402266105;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeTransferController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.64731520998, 56.0569452572;
- Eigen::Matrix<double, 1, 2> K;
- K << 1.06905877421, 0.0368709177253;
- return StateFeedbackController<2, 1, 1>(L, K, MakeTransferPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeTransferPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeTransferController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
deleted file mode 100644
index 596f9b3..0000000
--- a/frc971/control_loops/index/transfer_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeTransferController();
-
-StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
deleted file mode 100644
index 035880e..0000000
--- a/frc971/control_loops/shooter/shooter.cc
+++ /dev/null
@@ -1,121 +0,0 @@
-#include "frc971/control_loops/shooter/shooter.h"
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/control_loops/shooter/shooter_motor_plant.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-
-namespace frc971 {
-namespace control_loops {
-
-ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
- : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
- loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
- history_position_(0),
- position_goal_(0.0),
- last_position_(0.0),
- last_velocity_goal_(0) {
- memset(history_, 0, sizeof(history_));
-}
-
-/*static*/ const double ShooterMotor::dt = 0.01;
-/*static*/ const double ShooterMotor::kMaxSpeed =
- 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
-
-void ShooterMotor::RunIteration(
- const control_loops::ShooterLoop::Goal *goal,
- const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
- control_loops::ShooterLoop::Status *status) {
- double velocity_goal = std::min(goal->velocity, kMaxSpeed);
- const double current_position =
- (position == NULL ? loop_->X_hat(0, 0) : position->position);
- double output_voltage = 0.0;
-
- if (index_loop.status.FetchLatest() || index_loop.status.get()) {
- if (index_loop.status->is_shooting) {
- if (velocity_goal != last_velocity_goal_ &&
- velocity_goal < 130) {
- velocity_goal = last_velocity_goal_;
- }
- }
- } else {
- LOG(WARNING, "assuming index isn't shooting\n");
- }
- last_velocity_goal_ = velocity_goal;
-
- // Track the current position if the velocity goal is small.
- if (velocity_goal <= 1.0) {
- position_goal_ = current_position;
- }
-
- loop_->Y << current_position;
-
- // Add the position to the history.
- history_[history_position_] = current_position;
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- // Prevents integral windup by limiting the position error such that the
- // error can't produce much more than full power.
- const double kVelocityWeightScalar = 0.35;
- const double max_reference =
- (loop_->U_max(0, 0) - kVelocityWeightScalar *
- (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
- / loop_->K(0, 0) + loop_->X_hat(0, 0);
- const double min_reference =
- (loop_->U_min(0, 0) - kVelocityWeightScalar *
- (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
- / loop_->K(0, 0) + loop_->X_hat(0, 0);
-
- position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
- min_reference);
- loop_->R << position_goal_, velocity_goal;
- position_goal_ += velocity_goal * dt;
-
- loop_->Update(position, output == NULL);
-
- // Kill power at low velocity goals.
- if (velocity_goal < 1.0) {
- loop_->U[0] = 0.0;
- } else {
- output_voltage = loop_->U[0];
- }
-
- LOG(DEBUG,
- "PWM: %f, raw_pos: %f rotations: %f "
- "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
- output_voltage, current_position,
- current_position / (2 * M_PI),
- (current_position - last_position_) / dt,
- loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
-
- // Calculates the velocity over the last kHistoryLength * .01 seconds
- // by taking the difference between the current and next history positions.
- int old_history_position = ((history_position_ == 0) ?
- kHistoryLength : history_position_) - 1;
- average_velocity_ = (history_[old_history_position] -
- history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
-
- status->average_velocity = average_velocity_;
-
- // Determine if the velocity is close enough to the goal to be ready.
- if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
- velocity_goal != 0.0) {
- LOG(DEBUG, "Steady: ");
- status->ready = true;
- } else {
- LOG(DEBUG, "Not ready: ");
- status->ready = false;
- }
- LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
-
- last_position_ = current_position;
-
- if (output) {
- output->voltage = output_voltage;
- }
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
deleted file mode 100644
index 70f635f..0000000
--- a/frc971/control_loops/shooter/shooter.gyp
+++ /dev/null
@@ -1,79 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'shooter_loop',
- 'type': 'static_library',
- 'sources': ['shooter_motor.q'],
- 'variables': {
- 'header_path': 'frc971/control_loops/shooter',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'shooter_lib',
- 'type': 'static_library',
- 'sources': [
- 'shooter.cc',
- 'shooter_motor_plant.cc',
- ],
- 'dependencies': [
- 'shooter_loop',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(AOS)/common/common.gyp:controls',
- 'shooter_loop',
- ],
- },
- {
- 'target_name': 'shooter_lib_test',
- 'type': 'executable',
- 'sources': [
- 'shooter_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'shooter_loop',
- 'shooter_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'shooter_csv',
- 'type': 'executable',
- 'sources': [
- 'shooter_csv.cc',
- ],
- 'dependencies': [
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:timing',
- 'shooter_loop',
- ],
- },
- {
- 'target_name': 'shooter',
- 'type': 'executable',
- 'sources': [
- 'shooter_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'shooter_lib',
- 'shooter_loop',
- ],
- },
- ],
-}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
deleted file mode 100644
index 7947f7a..0000000
--- a/frc971/control_loops/shooter/shooter.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
-#define FRC971_CONTROL_LOOPS_SHOOTER_H_
-
-#include <memory>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-
-class ShooterMotor
- : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
- public:
- explicit ShooterMotor(
- control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
-
- // Control loop time step.
- static const double dt;
-
- // Maximum speed of the shooter wheel which the encoder is rated for in
- // rad/sec.
- static const double kMaxSpeed;
-
- protected:
- virtual void RunIteration(
- const control_loops::ShooterLoop::Goal *goal,
- const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
- control_loops::ShooterLoop::Status *status);
-
- private:
- // The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
-
- // History array and stuff for determining average velocity and whether
- // we are ready to shoot.
- static const int kHistoryLength = 5;
- double history_[kHistoryLength];
- ptrdiff_t history_position_;
- double average_velocity_;
-
- double position_goal_;
- double last_position_;
-
- // For making sure it keeps spinning if we're shooting.
- double last_velocity_goal_;
-
- DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
-};
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/frc971/control_loops/shooter/shooter_csv.cc b/frc971/control_loops/shooter/shooter_csv.cc
deleted file mode 100644
index 26f2866..0000000
--- a/frc971/control_loops/shooter/shooter_csv.cc
+++ /dev/null
@@ -1,50 +0,0 @@
-#include "stdio.h"
-
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/time.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-
-using ::frc971::control_loops::shooter;
-using ::aos::time::Time;
-
-int main(int argc, char * argv[]) {
- FILE *data_file = NULL;
- FILE *output_file = NULL;
-
- if (argc == 2) {
- data_file = fopen(argv[1], "w");
- output_file = data_file;
- } else {
- printf("Logging to stdout instead\n");
- output_file = stdout;
- }
-
- fprintf(data_file, "time, power, position");
-
- ::aos::Init();
-
- Time start_time = Time::Now();
-
- while (true) {
- ::aos::time::PhasedLoop10MS(2000);
- shooter.goal.FetchLatest();
- shooter.status.FetchLatest();
- shooter.position.FetchLatest();
- shooter.output.FetchLatest();
- if (shooter.output.get() &&
- shooter.position.get()) {
- fprintf(output_file, "\n%f, %f, %f",
- (shooter.position->sent_time - start_time).ToSeconds(),
- shooter.output->voltage,
- shooter.position->position);
- }
- }
-
- if (data_file) {
- fclose(data_file);
- }
-
- ::aos::Cleanup();
- return 0;
-}
-
diff --git a/frc971/control_loops/shooter/shooter_data.csv b/frc971/control_loops/shooter/shooter_data.csv
deleted file mode 100644
index 3515070..0000000
--- a/frc971/control_loops/shooter/shooter_data.csv
+++ /dev/null
@@ -1,638 +0,0 @@
-0.009404, 0.000000, 1484.878965
-0.019423, 0.000000, 1484.878965
-0.029389, 0.000000, 1484.878965
-0.039354, 0.000000, 1484.878965
-0.049887, 0.000000, 1484.878965
-0.059522, 0.000000, 1484.878965
-0.069479, 0.000000, 1484.878965
-0.079381, 0.000000, 1484.878965
-0.089338, 0.000000, 1484.878965
-0.099357, 0.000000, 1484.878965
-0.109409, 0.000000, 1484.878965
-0.119355, 0.000000, 1484.878965
-0.129358, 0.000000, 1484.878965
-0.139354, 0.000000, 1484.878965
-0.149480, 0.000000, 1484.878965
-0.159363, 0.000000, 1484.878965
-0.169341, 0.000000, 1484.878965
-0.179367, 0.000000, 1484.878965
-0.189636, 0.000000, 1484.878965
-0.199875, 0.000000, 1484.878965
-0.210070, 0.000000, 1484.878965
-0.219349, 0.000000, 1484.878965
-0.229544, 0.000000, 1484.878965
-0.239404, 0.000000, 1484.878965
-0.249410, 0.000000, 1484.878965
-0.259839, 0.000000, 1484.878965
-0.269492, 0.000000, 1484.878965
-0.279847, 0.000000, 1484.878965
-0.290056, 0.000000, 1484.878965
-0.299362, 0.000000, 1484.878965
-0.309457, 0.000000, 1484.878965
-0.319829, 0.000000, 1484.878965
-0.329446, 0.000000, 1484.878965
-0.339818, 0.000000, 1484.878965
-0.349444, 0.000000, 1484.878965
-0.359899, 0.000000, 1484.878965
-0.370053, 0.000000, 1484.878965
-0.379510, 0.000000, 1484.878965
-0.390136, 0.000000, 1484.878965
-0.399366, 0.000000, 1484.878965
-0.409472, 0.000000, 1484.878965
-0.419898, 0.000000, 1484.878965
-0.430131, 0.000000, 1484.878965
-0.439363, 0.000000, 1484.878965
-0.449459, 0.000000, 1484.878965
-0.459840, 0.000000, 1484.878965
-0.469382, 0.000000, 1484.878965
-0.479846, 0.000000, 1484.878965
-0.489432, 0.000000, 1484.878965
-0.499342, 0.000000, 1484.878965
-0.509350, 0.000000, 1484.878965
-0.519406, 0.000000, 1484.878965
-0.530084, 0.000000, 1484.878965
-0.539341, 0.000000, 1484.878965
-0.549406, 0.000000, 1484.878965
-0.559401, 0.000000, 1484.878965
-0.569409, 0.000000, 1484.878965
-0.579831, 0.000000, 1484.878965
-0.589469, 0.000000, 1484.878965
-0.599356, 0.000000, 1484.878965
-0.610099, 0.000000, 1484.878965
-0.619333, 0.000000, 1484.878965
-0.629479, 0.000000, 1484.878965
-0.639805, 0.000000, 1484.878965
-0.650053, 0.000000, 1484.878965
-0.659423, 0.000000, 1484.878965
-0.669413, 0.000000, 1484.878965
-0.679822, 0.000000, 1484.878965
-0.690045, 0.000000, 1484.878965
-0.699411, 0.000000, 1484.878965
-0.709584, 0.000000, 1484.878965
-0.719866, 0.000000, 1484.878965
-0.729475, 0.000000, 1484.878965
-0.739328, 0.000000, 1484.878965
-0.749396, 0.000000, 1484.878965
-0.759836, 0.000000, 1484.878965
-0.769492, 0.000000, 1484.878965
-0.779340, 0.000000, 1484.878965
-0.789401, 0.000000, 1484.878965
-0.799405, 0.000000, 1484.878965
-0.809407, 0.000000, 1484.878965
-0.819830, 0.000000, 1484.878965
-0.829411, 0.000000, 1484.878965
-0.839413, 0.000000, 1484.878965
-0.849409, 0.000000, 1484.878965
-0.859349, 0.000000, 1484.878965
-0.869453, 0.000000, 1484.878965
-0.879831, 0.000000, 1484.878965
-0.889426, 0.000000, 1484.878965
-0.899332, 0.000000, 1484.878965
-0.909439, 0.000000, 1484.878965
-0.919830, 0.000000, 1484.878965
-0.929464, 0.000000, 1484.878965
-0.939328, 0.000000, 1484.878965
-0.949440, 0.000000, 1484.878965
-0.959871, 0.000000, 1484.878965
-0.969393, 0.000000, 1484.878965
-0.980080, 0.000000, 1484.878965
-0.989440, 0.000000, 1484.878965
-0.999370, 0.000000, 1484.878965
-1.009338, 0.000000, 1484.878965
-1.019368, 0.000000, 1484.878965
-1.029492, 0.000000, 1484.878965
-1.039816, 0.000000, 1484.878965
-1.049430, 0.000000, 1484.878965
-1.059324, 0.000000, 1484.878965
-1.069351, 0.000000, 1484.878965
-1.079867, 0.000000, 1484.878965
-1.089417, 0.000000, 1484.878965
-1.099324, 0.000000, 1484.878965
-1.109348, 0.000000, 1484.878965
-1.119389, 0.000000, 1484.878965
-1.129331, 0.000000, 1484.878965
-1.139306, 0.000000, 1484.878965
-1.149394, 0.000000, 1484.878965
-1.159374, 0.000000, 1484.878965
-1.169335, 0.000000, 1484.878965
-1.179817, 0.000000, 1484.878965
-1.189415, 0.000000, 1484.878965
-1.199338, 0.000000, 1484.878965
-1.209349, 0.000000, 1484.878965
-1.219333, 0.000000, 1484.878965
-1.229518, 0.000000, 1484.878965
-1.239329, 0.000000, 1484.878965
-1.249334, 0.000000, 1484.878965
-1.259316, 0.000000, 1484.878965
-1.269388, 0.000000, 1484.878965
-1.279357, 0.000000, 1484.878965
-1.289451, 0.000000, 1484.878965
-1.299350, 0.000000, 1484.878965
-1.309350, 0.000000, 1484.878965
-1.319848, 0.000000, 1484.878965
-1.329384, 0.000000, 1484.878965
-1.339375, 0.000000, 1484.878965
-1.349359, 0.000000, 1484.878965
-1.359384, 0.000000, 1484.878965
-1.369428, 0.000000, 1484.878965
-1.379443, 0.000000, 1484.878965
-1.389498, 0.000000, 1484.878965
-1.399332, 0.000000, 1484.878965
-1.409393, 0.000000, 1484.878965
-1.419325, 0.000000, 1484.878965
-1.430129, 0.000000, 1484.878965
-1.439419, 0.000000, 1484.878965
-1.449510, 0.000000, 1484.878965
-1.459828, 0.000000, 1484.878965
-1.469377, 0.000000, 1484.878965
-1.479834, 0.000000, 1484.878965
-1.489367, 0.000000, 1484.878965
-1.499316, 0.000000, 1484.878965
-1.509405, 0.000000, 1484.878965
-1.519341, 0.000000, 1484.878965
-1.529334, 0.000000, 1484.878965
-1.539305, 0.000000, 1484.878965
-1.550118, 0.000000, 1484.878965
-1.559386, 0.000000, 1484.878965
-1.569647, 0.000000, 1484.878965
-1.579395, 0.000000, 1484.878965
-1.589381, 0.000000, 1484.878965
-1.599819, 0.000000, 1484.878965
-1.609401, 0.000000, 1484.878965
-1.619404, 0.000000, 1484.878965
-1.629335, 0.000000, 1484.878965
-1.639327, 0.000000, 1484.878965
-1.649334, 0.000000, 1484.878965
-1.659341, 0.000000, 1484.878965
-1.669328, 0.000000, 1484.878965
-1.679850, 0.000000, 1484.878965
-1.689423, 0.000000, 1484.878965
-1.699320, 0.000000, 1484.878965
-1.710128, 0.000000, 1484.878965
-1.719388, 0.000000, 1484.878965
-1.730042, 0.000000, 1484.878965
-1.739338, 0.000000, 1484.878965
-1.749483, 0.000000, 1484.878965
-1.759420, 0.000000, 1484.878965
-1.769334, 0.000000, 1484.878965
-1.779289, 0.000000, 1484.878965
-1.789325, 0.000000, 1484.878965
-1.799395, 0.000000, 1484.878965
-1.809493, 0.000000, 1484.878965
-1.819312, 0.000000, 1484.878965
-1.829402, 0.000000, 1484.878965
-1.839317, 0.000000, 1484.878965
-1.849330, 0.000000, 1484.878965
-1.859354, 0.000000, 1484.878965
-1.869394, 0.000000, 1484.878965
-1.879816, 0.000000, 1484.878965
-1.889374, 0.000000, 1484.878965
-1.899381, 0.000000, 1484.878965
-1.909332, 0.000000, 1484.878965
-1.919359, 0.000000, 1484.878965
-1.929338, 0.000000, 1484.878965
-1.939359, 0.000000, 1484.878965
-1.949332, 0.000000, 1484.878965
-1.959325, 0.000000, 1484.878965
-1.969341, 0.000000, 1484.878965
-1.979362, 0.000000, 1484.878965
-1.989330, 0.000000, 1484.878965
-1.999479, 0.000000, 1484.878965
-2.009392, 0.000000, 1484.878965
-2.019318, 0.000000, 1484.878965
-2.029320, 0.000000, 1484.878965
-2.039323, 0.000000, 1484.878965
-2.049387, 0.000000, 1484.878965
-2.059818, 0.000000, 1484.878965
-2.069766, 0.000000, 1484.878965
-2.079835, 0.000000, 1484.878965
-2.089372, 0.000000, 1484.878965
-2.099322, 0.000000, 1484.878965
-2.109357, 0.000000, 1484.878965
-2.119387, 0.000000, 1484.878965
-2.129327, 0.000000, 1484.878965
-2.139458, 0.000000, 1484.878965
-2.149392, 0.000000, 1484.878965
-2.159826, 0.000000, 1484.878965
-2.169591, 0.000000, 1484.878965
-2.179656, 0.000000, 1484.878965
-2.189392, 0.000000, 1484.878965
-2.199491, 0.000000, 1484.878965
-2.209541, 0.000000, 1484.878965
-2.219287, 0.000000, 1484.878965
-2.229123, 0.000000, 1484.878965
-2.239347, 0.000000, 1484.878965
-2.249390, 0.000000, 1484.878965
-2.259407, 0.000000, 1484.878965
-2.269393, 0.000000, 1484.878965
-2.279375, 0.000000, 1484.878965
-2.289416, 0.000000, 1484.878965
-2.299368, 0.000000, 1484.878965
-2.309379, 0.000000, 1484.878965
-2.319382, 0.000000, 1484.878965
-2.329435, 0.000000, 1484.878965
-2.339329, 0.000000, 1484.878965
-2.349389, 0.000000, 1484.878965
-2.359454, 0.000000, 1484.878965
-2.369832, 0.000000, 1484.878965
-2.379390, 0.000000, 1484.878965
-2.389381, 0.000000, 1484.878965
-2.399429, 0.000000, 1484.878965
-2.409394, 0.000000, 1484.878965
-2.419367, 0.000000, 1484.878965
-2.429384, 0.000000, 1484.878965
-2.439408, 0.000000, 1484.878965
-2.449391, 0.000000, 1484.878965
-2.459343, 0.000000, 1484.878965
-2.469424, 0.000000, 1484.878965
-2.479357, 0.000000, 1484.878965
-2.489388, 0.000000, 1484.878965
-2.499413, 0.000000, 1484.878965
-2.510081, 0.000000, 1484.878965
-2.519397, 0.000000, 1484.878965
-2.529342, 0.000000, 1484.878965
-2.539372, 0.000000, 1484.878965
-2.549674, 0.000000, 1484.878965
-2.559586, 0.000000, 1484.878965
-2.569807, 0.000000, 1484.878965
-2.579362, 0.000000, 1484.878965
-2.589325, 0.000000, 1484.878965
-2.599300, 0.000000, 1484.878965
-2.609436, 0.000000, 1484.878965
-2.619476, 0.000000, 1484.878965
-2.629668, 0.000000, 1484.878965
-2.639301, 0.000000, 1484.878965
-2.649411, 0.000000, 1484.878965
-2.659301, 0.000000, 1484.878965
-2.669336, 0.000000, 1484.878965
-2.679460, 0.000000, 1484.878965
-2.689691, 0.000000, 1484.878965
-2.699310, 0.000000, 1484.878965
-2.710046, 0.000000, 1484.878965
-2.719584, 0.000000, 1484.878965
-2.729333, 0.000000, 1484.878965
-2.739288, 0.000000, 1484.878965
-2.749320, 0.000000, 1484.878965
-2.759517, 0.000000, 1484.878965
-2.769811, 0.000000, 1484.878965
-2.779463, 0.000000, 1484.878965
-2.789708, 0.000000, 1484.878965
-2.799310, 12.000000, 1484.878965
-2.809361, 12.000000, 1484.878965
-2.819345, 12.000000, 1484.943934
-2.829470, 12.000000, 1485.117183
-2.839666, 12.000000, 1485.355402
-2.849432, 12.000000, 1485.680245
-2.859547, 12.000000, 1486.091712
-2.869422, 12.000000, 1486.589805
-2.879352, 12.000000, 1487.152866
-2.889431, 12.000000, 1487.802552
-2.899538, 12.000000, 1488.538863
-2.909416, 12.000000, 1489.340142
-2.919676, 12.000000, 1490.163078
-2.929470, 11.856977, 1491.072638
-2.939341, 10.941776, 1492.090480
-2.949422, 9.709468, 1493.086665
-2.959343, 9.484298, 1494.212787
-2.969480, 9.024482, 1495.360566
-2.979482, 8.408468, 1496.616625
-2.989402, 7.584528, 1497.851029
-2.999839, 7.318006, 1499.193713
-3.009487, 6.726255, 1500.601366
-3.019345, 5.691274, 1502.030675
-3.029433, 5.445505, 1503.503297
-3.039661, 5.201068, 1505.019231
-3.049419, 4.805405, 1506.556821
-3.059323, 4.164102, 1508.116067
-3.069465, 3.901448, 1509.696970
-3.079658, 3.715078, 1511.321185
-3.089408, 3.656437, 1512.902087
-3.099346, 3.325052, 1514.504646
-3.109498, 3.310343, 1516.128861
-3.119381, 3.348580, 1517.753076
-3.129434, 3.031678, 1519.377291
-3.139461, 3.165136, 1520.979850
-3.149456, 3.276186, 1522.604064
-3.159650, 3.130954, 1524.228279
-3.169436, 3.017931, 1525.852494
-3.179542, 2.968543, 1527.455053
-3.189425, 3.107515, 1529.079268
-3.199654, 3.010200, 1530.681827
-3.209442, 3.078322, 1532.306042
-3.219518, 2.953745, 1533.908601
-3.229434, 3.021034, 1535.511159
-3.239303, 4.196084, 1537.113718
-3.249474, 2.523334, 1538.716277
-3.259481, 2.549791, 1540.318836
-3.269395, 2.856174, 1541.943051
-3.279668, 2.830169, 1543.545609
-3.289491, 2.903769, 1545.148168
-3.299343, 2.930722, 1546.729071
-3.309430, 2.915555, 1548.331629
-3.319847, 2.887528, 1549.934188
-3.329444, 3.022588, 1551.515091
-3.339468, 2.922583, 1553.095993
-3.349700, 3.155171, 1554.676896
-3.359307, 2.959473, 1556.257798
-3.369439, 2.963462, 1557.817045
-3.379865, 3.151337, 1559.397947
-3.389482, 3.237455, 1560.957194
-3.399670, 3.075969, 1562.538096
-3.409431, 3.127616, 1564.097342
-3.419355, 3.012946, 1565.656589
-3.429472, 3.094794, 1567.237491
-3.439645, 2.986884, 1568.796738
-3.449433, 3.228489, 1570.355984
-3.459540, 3.042079, 1571.915230
-3.469425, 3.052375, 1573.474477
-3.479338, 3.083112, 1575.055379
-3.489491, 2.926137, 1576.614626
-3.499576, 2.990285, 1578.152216
-3.509412, 3.204911, 1579.711462
-3.519368, 3.134930, 1581.270709
-3.529436, 3.050871, 1582.829955
-3.539512, 3.012085, 1584.389201
-3.549482, 3.160836, 1585.948448
-3.559848, 3.076263, 1587.486038
-3.569480, 2.996910, 1589.045284
-3.579466, 2.963210, 1590.604530
-3.589392, 3.113684, 1592.142121
-3.599645, 3.029053, 1593.679711
-3.609466, 3.111230, 1595.238957
-3.619478, 3.001191, 1596.776547
-3.629414, 3.083010, 1598.335794
-3.639639, 2.977469, 1599.873384
-3.649431, 3.061646, 1601.410974
-3.659343, 2.956478, 1602.970220
-3.669437, 3.040410, 1604.507810
-3.679660, 3.096927, 1606.067057
-3.689414, 3.104633, 1607.582991
-3.699466, 3.092523, 1609.120581
-3.709581, 3.079314, 1610.658171
-3.719844, 3.069195, 1612.195761
-3.729455, 3.060854, 1613.755008
-3.739454, 2.890971, 1615.270942
-3.749456, 3.120861, 1616.808532
-3.759652, 2.943141, 1618.367778
-3.769565, 2.963262, 1619.905368
-3.779406, 3.162518, 1621.442958
-3.789419, 3.096059, 1622.958892
-3.799647, 2.857986, 1624.496482
-3.809451, 3.063847, 1626.034073
-3.819535, 3.048330, 1627.550007
-3.829423, 3.158748, 1629.087597
-3.839290, 3.053598, 1630.625187
-3.849534, 2.974064, 1632.162777
-3.859585, 2.947301, 1633.678711
-3.869342, 3.104581, 1635.194645
-3.879651, 3.025376, 1636.753891
-3.889461, 3.112302, 1638.269825
-3.899471, 3.006920, 1639.785759
-3.909503, 3.093474, 1641.323349
-3.919650, 2.992726, 1642.860940
-3.929493, 3.081710, 1644.376873
-3.939457, 2.981346, 1645.914464
-3.949675, 2.908199, 1647.430398
-3.959566, 3.045687, 1648.946332
-3.969420, 3.126825, 1650.505578
-3.979850, 2.816686, 1652.021512
-3.989429, 3.120514, 1653.537446
-3.999650, 3.009544, 1655.053380
-4.009434, 3.053283, 1656.590970
-4.019490, 2.769875, 1658.106904
-4.029473, 2.932334, 1659.644494
-4.039639, 2.903060, 1661.182084
-4.049524, 3.176347, 1662.698018
-4.059463, 2.998063, 1664.213952
-4.069518, 3.013137, 1665.729886
-4.079855, 3.051287, 1667.267476
-4.089451, 3.065561, 1668.783410
-4.099514, 2.901543, 1670.299344
-4.109450, 2.971613, 1671.793622
-4.119835, 3.035151, 1673.331212
-4.129459, 3.052225, 1674.847146
-4.139393, 2.884954, 1676.363080
-4.149517, 3.114730, 1677.879014
-4.159643, 3.101587, 1679.373292
-4.169483, 3.212047, 1680.889226
-4.179478, 2.947840, 1682.405160
-4.189417, 3.111024, 1683.921094
-4.199352, 3.081820, 1685.415371
-4.209534, 3.196368, 1686.931305
-4.219472, 2.937203, 1688.447239
-4.229358, 3.102203, 1689.963173
-4.239581, 4.044265, 1691.479107
-4.249471, 2.568287, 1692.995041
-4.259564, 2.592233, 1694.510975
-4.269489, 2.874029, 1696.048565
-4.279649, 2.848913, 1697.564499
-4.289504, 3.102328, 1699.058777
-4.299467, 2.913154, 1700.574711
-4.309396, 2.926342, 1702.090645
-4.319635, 3.127416, 1703.584923
-4.329463, 3.066398, 1705.122513
-4.339462, 3.157757, 1706.616790
-4.349523, 3.054520, 1708.132724
-4.359685, 2.982710, 1709.627002
-4.369520, 3.124646, 1711.142936
-4.379481, 2.887285, 1712.658870
-4.389476, 3.058346, 1714.174804
-4.399662, 3.028207, 1715.647426
-4.409523, 3.140828, 1717.163360
-4.419532, 3.042732, 1718.679293
-4.429478, 3.131501, 1720.195227
-4.439668, 3.033777, 1721.667849
-4.449451, 3.127131, 1723.183783
-4.459519, 3.031674, 1724.699717
-4.469529, 3.125338, 1726.193995
-4.479652, 3.029621, 1727.709929
-4.489541, 3.123096, 1729.204206
-4.499285, 2.865452, 1730.720140
-4.509567, 3.035110, 1732.236074
-4.519358, 3.169872, 1733.752008
-4.529256, 3.046239, 1735.246286
-4.539423, 2.956041, 1736.762220
-4.549418, 3.092368, 1738.278154
-4.559287, 3.017417, 1739.772432
-4.569392, 3.113609, 1741.266709
-4.579389, 3.015401, 1742.804300
-4.589391, 3.107141, 1744.298577
-4.599271, 3.010950, 1745.792855
-4.609237, 3.104538, 1747.308789
-4.619397, 2.847040, 1748.803067
-4.629258, 3.016751, 1750.340657
-4.639391, 3.151513, 1751.834935
-4.649382, 3.027871, 1753.329212
-4.659249, 2.937667, 1754.845146
-4.669717, 3.073994, 1756.361080
-4.679709, 2.837164, 1757.877014
-4.689305, 3.009530, 1759.371292
-4.699414, 2.979957, 1760.865570
-4.709265, 3.254504, 1762.381504
-4.719267, 2.918277, 1763.875782
-4.729387, 3.014535, 1765.391715
-4.739263, 2.964507, 1766.885993
-4.749394, 3.242657, 1768.401927
-4.759241, 3.073913, 1769.917861
-4.769396, 2.934311, 1771.412139
-4.779436, 2.891905, 1772.928073
-4.789376, 3.055522, 1774.444007
-4.799259, 2.823787, 1775.938285
-4.809430, 2.993720, 1777.454219
-4.819715, 2.961969, 1778.948496
-4.829635, 3.235854, 1780.442774
-4.839637, 3.061570, 1781.958708
-4.849506, 3.081810, 1783.474642
-4.859505, 2.801335, 1784.968920
-4.869352, 3.134735, 1786.484854
-4.879650, 3.036602, 1787.979131
-4.889446, 3.084670, 1789.495065
-4.899459, 2.966217, 1790.989343
-4.909516, 3.056512, 1792.483621
-4.919642, 2.963315, 1793.999555
-4.929515, 3.058695, 1795.493833
-4.939540, 2.963436, 1796.988110
-4.949486, 3.056827, 1798.504044
-4.959635, 2.960935, 1800.019978
-4.969264, 3.054371, 1801.514256
-4.979559, 3.120496, 1803.030190
-4.989369, 2.975943, 1804.502812
-4.999639, 3.049612, 1806.018745
-5.009522, 3.114635, 1807.513023
-5.019539, 2.811284, 1809.028957
-5.029502, 3.124778, 1810.523235
-5.039639, 3.020720, 1812.017513
-5.049531, 3.069436, 1813.511790
-5.059571, 2.952407, 1815.006068
-5.069509, 3.205185, 1816.522002
-5.079646, 3.035861, 1818.016280
-5.089525, 3.224397, 1819.532214
-5.099572, 2.870103, 1821.026492
-5.109284, 3.134886, 1822.542426
-5.119546, 3.015399, 1824.036703
-5.129816, 3.065232, 1825.530981
-5.139566, 2.951614, 1827.025259
-5.149507, 3.044046, 1828.541193
-5.159648, 2.951048, 1830.035471
-5.169497, 3.208013, 1831.551405
-5.179545, 2.874516, 1833.045682
-5.189501, 3.137284, 1834.539960
-5.199633, 3.012938, 1836.034238
-5.209521, 3.222545, 1837.528516
-5.219560, 3.032578, 1839.044450
-5.229503, 3.056672, 1840.538727
-5.239594, 4.237957, 1842.033005
-5.249532, 2.592533, 1843.527283
-5.259542, 2.486074, 1845.043217
-5.269566, 2.894539, 1846.515838
-5.279650, 3.123100, 1848.010116
-5.289252, 3.165125, 1849.504394
-5.299546, 3.307151, 1850.998672
-5.309525, 3.213976, 1852.471293
-5.319632, 3.141194, 1853.943915
-5.329458, 3.284019, 1855.459849
-5.339575, 3.050528, 1856.932470
-5.349266, 3.226635, 1858.448404
-5.359682, 3.201591, 1859.921026
-5.369278, 3.319115, 1861.436960
-5.379549, 3.063938, 1862.931237
-5.389254, 3.071781, 1864.447171
-5.399651, 2.961775, 1865.919793
-5.409641, 3.063785, 1867.435727
-5.419564, 0.000000, 1868.930005
-5.429518, 0.000000, 1870.424282
-5.439596, 0.000000, 1871.918560
-5.449485, 0.000000, 1873.347869
-5.459469, 0.000000, 1874.733866
-5.469640, 0.000000, 1876.098207
-5.479575, 0.000000, 1877.419235
-5.489492, 0.000000, 1878.675294
-5.499454, 0.000000, 1879.909698
-5.509441, 0.000000, 1881.100789
-5.519669, 0.000000, 1882.270223
-5.529268, 0.000000, 1883.396346
-5.539531, 0.000000, 1884.479156
-5.549491, 0.000000, 1885.518653
-5.559591, 0.000000, 1886.536495
-5.569479, 0.000000, 1887.532680
-5.579553, 0.000000, 1888.485553
-5.589263, 0.000000, 1889.416769
-5.599628, 0.000000, 1890.283017
-5.609531, 0.000000, 1891.149265
-5.619452, 0.000000, 1891.993857
-5.629254, 0.000000, 1892.795136
-5.639595, 0.000000, 1893.574759
-5.649382, 0.000000, 1894.332726
-5.659518, 0.000000, 1895.047381
-5.669510, 0.000000, 1895.740379
-5.679630, 0.000000, 1896.433378
-5.689513, 0.000000, 1897.083064
-5.699559, 0.000000, 1897.689437
-5.709897, 0.000000, 1898.295811
-5.719663, 0.000000, 1898.880528
-5.729785, 0.000000, 1899.443590
-5.739550, 0.000000, 1899.984995
-5.749477, 0.000000, 1900.504743
-5.759631, 0.000000, 1901.002836
-5.769499, 0.000000, 1901.500928
-5.779545, 0.000000, 1901.955709
-5.789477, 0.000000, 1902.388833
-5.799588, 0.000000, 1902.821957
-5.809530, 0.000000, 1903.233424
-5.819550, 0.000000, 1903.623236
-5.829288, 0.000000, 1903.991391
-5.839583, 0.000000, 1904.359547
-5.849261, 0.000000, 1904.706046
-5.859488, 0.000000, 1905.030889
-5.869375, 0.000000, 1905.334076
-5.879665, 0.000000, 1905.658919
-5.889507, 0.000000, 1905.918793
-5.899530, 0.000000, 1906.200324
-5.909520, 0.000000, 1906.460198
-5.919633, 0.000000, 1906.720073
-5.929467, 0.000000, 1906.958291
-5.939535, 0.000000, 1907.174853
-5.949423, 0.000000, 1907.391415
-5.959583, 0.000000, 1907.586320
-5.969500, 0.000000, 1907.781226
-5.979548, 0.000000, 1907.954476
-5.989236, 0.000000, 1908.106069
-5.999631, 0.000000, 1908.279319
-6.009506, 0.000000, 1908.409256
-6.019548, 0.000000, 1908.560849
-6.029525, 0.000000, 1908.669130
-6.039582, 0.000000, 1908.799068
-6.049527, 0.000000, 1908.907349
-6.059538, 0.000000, 1909.015630
-6.069537, 0.000000, 1909.102254
-6.079650, 0.000000, 1909.188879
-6.089474, 0.000000, 1909.253848
-6.099533, 0.000000, 1909.318816
-6.109497, 0.000000, 1909.383785
-6.119587, 0.000000, 1909.448754
-6.129255, 0.000000, 1909.492066
-6.139524, 0.000000, 1909.513722
-6.149479, 0.000000, 1909.557035
-6.159584, 0.000000, 1909.578691
-6.169260, 0.000000, 1909.600347
-6.179536, 0.000000, 1909.600347
-6.189396, 0.000000, 1909.600347
-6.199574, 0.000000, 1909.600347
-6.209506, 0.000000, 1909.600347
-6.219521, 0.000000, 1909.600347
-6.229482, 0.000000, 1909.600347
-6.239565, 0.000000, 1909.600347
-6.249254, 0.000000, 1909.600347
-6.259521, 0.000000, 1909.600347
-6.269506, 0.000000, 1909.600347
-6.279589, 0.000000, 1909.600347
-6.289535, 0.000000, 1909.600347
-6.299542, 0.000000, 1909.600347
-6.309460, 0.000000, 1909.600347
-6.319858, 0.000000, 1909.600347
-6.329537, 0.000000, 1909.600347
-6.339535, 0.000000, 1909.600347
-6.349231, 0.000000, 1909.600347
-6.359649, 0.000000, 1909.600347
-6.369534, 0.000000, 1909.600347
-6.379535, 0.000000, 1909.600347
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
deleted file mode 100644
index beb62f1..0000000
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ /dev/null
@@ -1,193 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/shooter/shooter.h"
-#include "frc971/constants.h"
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-
-// Class which simulates the shooter and sends out queue messages with the
-// position.
-class ShooterMotorSimulation {
- public:
- // Constructs a shooter simulation. I'm not sure how the construction of
- // the queue (my_shooter_loop_) actually works (same format as wrist).
- ShooterMotorSimulation()
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
- my_shooter_loop_(".frc971.control_loops.shooter",
- 0x78d8e372, ".frc971.control_loops.shooter.goal",
- ".frc971.control_loops.shooter.position",
- ".frc971.control_loops.shooter.output",
- ".frc971.control_loops.shooter.status") {
- }
-
- // Sends a queue message with the position of the shooter.
- void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
- my_shooter_loop_.position.MakeMessage();
- position->position = shooter_plant_->Y(0, 0);
- position.Send();
- }
-
- // Simulates shooter for a single timestep.
- void Simulate() {
- EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
- shooter_plant_->U << my_shooter_loop_.output->voltage;
- shooter_plant_->Update();
- }
-
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
-
- private:
- ShooterLoop my_shooter_loop_;
-};
-
-class ShooterTest : public ::testing::Test {
- protected:
- ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
- 0x78d8e372,
- ".frc971.control_loops.shooter.goal",
- ".frc971.control_loops.shooter.position",
- ".frc971.control_loops.shooter.output",
- ".frc971.control_loops.shooter.status"),
- shooter_motor_(&my_shooter_loop_),
- shooter_motor_plant_() {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- SendDSPacket(true);
- }
-
- virtual ~ShooterTest() {
- ::aos::robot_state.Clear();
- }
-
- // Update the robot state. Without this, the Iteration of the control loop
- // will stop all the motors and the shooter won't go anywhere.
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
- }
-
- void VerifyNearGoal() {
- my_shooter_loop_.goal.FetchLatest();
- my_shooter_loop_.status.FetchLatest();
- EXPECT_NEAR(my_shooter_loop_.goal->velocity,
- my_shooter_loop_.status->average_velocity,
- 10.0);
- }
-
- // Bring up and down Core.
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointed to
- // shared memory that is no longer valid.
- ShooterLoop my_shooter_loop_;
-
- // Create a control loop and simulation.
- ShooterMotor shooter_motor_;
- ShooterMotorSimulation shooter_motor_plant_;
-};
-
-// Tests that the shooter does nothing when the goal is zero.
-TEST_F(ShooterTest, DoesNothing) {
- my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
- SendDSPacket(true);
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- VerifyNearGoal();
- my_shooter_loop_.output.FetchLatest();
- EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
-}
-
-// Tests that the shooter spins up to speed and that it then spins down
-// without applying any power.
-TEST_F(ShooterTest, SpinUpAndDown) {
- my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
- bool is_done = false;
- while (!is_done) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(true);
- EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
- is_done = my_shooter_loop_.status->ready;
- }
- VerifyNearGoal();
-
- my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
- for (int i = 0; i < 100; ++i) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(true);
- EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
- EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
- }
-}
-
-// Test to make sure that it does not exceed the encoder's rated speed.
-TEST_F(ShooterTest, RateLimitTest) {
- my_shooter_loop_.goal.MakeWithBuilder().velocity(1000.0).Send();
- bool is_done = false;
- while (!is_done) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(true);
- EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
- is_done = my_shooter_loop_.status->ready;
- }
-
- my_shooter_loop_.goal.FetchLatest();
- my_shooter_loop_.status.FetchLatest();
- EXPECT_GT(shooter_motor_.kMaxSpeed,
- shooter_motor_plant_.shooter_plant_->X(1, 0));
-}
-
-// Tests that the shooter can spin up nicely while missing position packets.
-TEST_F(ShooterTest, MissingPositionMessages) {
- my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
- for (int i = 0; i < 100; ++i) {
- if (i % 7) {
- shooter_motor_plant_.SendPositionMessage();
- }
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(true);
- }
-
- VerifyNearGoal();
-}
-
-// Tests that the shooter can spin up nicely while disabled for a while.
-TEST_F(ShooterTest, Disabled) {
- my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
- for (int i = 0; i < 100; ++i) {
- if (i % 7) {
- shooter_motor_plant_.SendPositionMessage();
- }
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(i > 50);
- }
-
- VerifyNearGoal();
-}
-
-} // namespace testing
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
deleted file mode 100644
index e4e25ad..0000000
--- a/frc971/control_loops/shooter/shooter_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/shooter/shooter.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- frc971::control_loops::ShooterMotor shooter;
- shooter.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/control_loops/shooter/shooter_motor.q b/frc971/control_loops/shooter/shooter_motor.q
deleted file mode 100644
index f2abf25..0000000
--- a/frc971/control_loops/shooter/shooter_motor.q
+++ /dev/null
@@ -1,31 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-queue_group ShooterLoop {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Goal velocity in rad/sec
- double velocity;
- };
-
- message Status {
- // True if the shooter is up to speed.
- bool ready;
- // The average velocity over the last 0.1 seconds.
- double average_velocity;
- };
-
- message Position {
- // The angle of the shooter wheel measured in rad/sec.
- double position;
- };
-
- queue Goal goal;
- queue Position position;
- queue aos.control_loops.Output output;
- queue Status status;
-};
-
-queue_group ShooterLoop shooter;
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
deleted file mode 100644
index 1a623f3..0000000
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/shooter/shooter_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00992127884628, 0.0, 0.984297191531;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.00398899915072, 0.795700859132;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeShooterController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.08429719153, 29.2677479765;
- Eigen::Matrix<double, 1, 2> K;
- K << 0.955132813139, 0.50205697652;
- return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
deleted file mode 100644
index 3588605..0000000
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeShooterController();
-
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/update_angle_adjust.sh b/frc971/control_loops/update_angle_adjust.sh
deleted file mode 100755
index 096f95e..0000000
--- a/frc971/control_loops/update_angle_adjust.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-#
-# Updates the angle adjust controller.
-
-./python/angle_adjust.py angle_adjust/angle_adjust_motor_plant.h \
- angle_adjust/angle_adjust_motor_plant.cc \
- angle_adjust/unaugmented_angle_adjust_motor_plant.h \
- angle_adjust/unaugmented_angle_adjust_motor_plant.cc \
diff --git a/frc971/control_loops/update_index.sh b/frc971/control_loops/update_index.sh
deleted file mode 100755
index 3b819b4..0000000
--- a/frc971/control_loops/update_index.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/bin/bash
-#
-# Updates the index controller.
-
-./python/index.py index/index_motor_plant.h index/index_motor_plant.cc
diff --git a/frc971/control_loops/update_shooter.sh b/frc971/control_loops/update_shooter.sh
deleted file mode 100755
index db98547..0000000
--- a/frc971/control_loops/update_shooter.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/bin/bash
-#
-# Updates the shooter controller.
-
-./python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
diff --git a/frc971/control_loops/update_transfer.sh b/frc971/control_loops/update_transfer.sh
deleted file mode 100755
index d7820fa..0000000
--- a/frc971/control_loops/update_transfer.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/bin/bash
-#
-# Updates the transfer controller.
-
-./python/transfer.py index/transfer_motor_plant.h index/transfer_motor_plant.cc
diff --git a/frc971/control_loops/update_wrist.sh b/frc971/control_loops/update_wrist.sh
deleted file mode 100755
index 1a3b54c..0000000
--- a/frc971/control_loops/update_wrist.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-#
-# Updates the wrist controller.
-
-./python/wrist.py wrist/wrist_motor_plant.h \
- wrist/wrist_motor_plant.cc \
- wrist/unaugmented_wrist_motor_plant.h \
- wrist/unaugmented_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
deleted file mode 100644
index a9871c4..0000000
--- a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.000326582411818, 0.0631746179893;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeRawWristController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.71581823335, 64.8264890043;
- Eigen::Matrix<double, 1, 2> K;
- K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
deleted file mode 100644
index c049420..0000000
--- a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawWristController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
deleted file mode 100644
index 8229c93..0000000
--- a/frc971/control_loops/wrist/wrist.cc
+++ /dev/null
@@ -1,79 +0,0 @@
-#include "frc971/control_loops/wrist/wrist.h"
-
-#include <stdio.h>
-
-#include <algorithm>
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/constants.h"
-#include "frc971/control_loops/wrist/wrist_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-
-WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
- : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- zeroed_joint_(MakeWristLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<1>::ConfigurationData config_data;
-
- config_data.lower_limit = GetValues().wrist_lower_limit;
- config_data.upper_limit = GetValues().wrist_upper_limit;
- config_data.hall_effect_start_angle[0] =
- GetValues().wrist_hall_effect_start_angle;
- config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().wrist_zeroing_speed;
-
- config_data.max_zeroing_voltage = 5.0;
- config_data.deadband_voltage = 0.0;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
-
-// Positive angle is up, and positive power is up.
-void WristMotor::RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status * status) {
-
- // Disable the motors now so that all early returns will return with the
- // motors disabled.
- if (output) {
- output->voltage = 0;
- }
-
- ZeroedJoint<1>::PositionData transformed_position;
- ZeroedJoint<1>::PositionData *transformed_position_ptr =
- &transformed_position;
- if (!position) {
- transformed_position_ptr = NULL;
- } else {
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
- }
-
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
- output != NULL,
- goal->goal, 0.0);
-
- if (position) {
- LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
- position->pos,
- position->hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
- }
-
- if (output) {
- output->voltage = voltage;
- }
- status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
deleted file mode 100644
index 60895ca..0000000
--- a/frc971/control_loops/wrist/wrist.gyp
+++ /dev/null
@@ -1,67 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'wrist_loop',
- 'type': 'static_library',
- 'sources': ['wrist_motor.q'],
- 'variables': {
- 'header_path': 'frc971/control_loops/wrist',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:control_loop_queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'wrist_lib',
- 'type': 'static_library',
- 'sources': [
- 'wrist.cc',
- 'wrist_motor_plant.cc',
- 'unaugmented_wrist_motor_plant.cc',
- ],
- 'dependencies': [
- 'wrist_loop',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(AOS)/common/common.gyp:controls',
- 'wrist_loop',
- ],
- },
- {
- 'target_name': 'wrist_lib_test',
- 'type': 'executable',
- 'sources': [
- 'wrist_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'wrist_loop',
- 'wrist_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'wrist',
- 'type': 'executable',
- 'sources': [
- 'wrist_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'wrist_lib',
- 'wrist_loop',
- ],
- },
- ],
-}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
deleted file mode 100644
index a3bad80..0000000
--- a/frc971/control_loops/wrist/wrist.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-#define FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-
-#include <memory>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/wrist/wrist_motor_plant.h"
-#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
-
-#include "frc971/control_loops/zeroed_joint.h"
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-class WristTest_NoWindupPositive_Test;
-class WristTest_NoWindupNegative_Test;
-};
-
-class WristMotor
- : public aos::control_loops::ControlLoop<control_loops::WristLoop> {
- public:
- explicit WristMotor(
- control_loops::WristLoop *my_wrist = &control_loops::wrist);
-
- // True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
-
- // True if the wrist is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
-
- // True if the wrist is zeroing.
- bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
-
- // True if the state machine is uninitialized.
- bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
-
- // True if the state machine is ready.
- bool is_ready() const { return zeroed_joint_.is_ready(); }
-
- protected:
- virtual void RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status *status);
-
- private:
- // Friend the test classes for acces to the internal state.
- friend class testing::WristTest_NoWindupPositive_Test;
- friend class testing::WristTest_NoWindupNegative_Test;
-
- // The zeroed joint to use.
- ZeroedJoint<1> zeroed_joint_;
-
- DISALLOW_COPY_AND_ASSIGN(WristMotor);
-};
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
deleted file mode 100644
index 7d248ce..0000000
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ /dev/null
@@ -1,335 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/wrist/wrist.h"
-#include "frc971/constants.h"
-
-
-using ::aos::time::Time;
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-
-
-// Class which simulates the wrist and sends out queue messages containing the
-// position.
-class WristMotorSimulation {
- public:
- // Constructs a motor simulation. initial_position is the inital angle of the
- // wrist, which will be treated as 0 by the encoder.
- WristMotorSimulation(double initial_position)
- : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
- my_wrist_loop_(".frc971.control_loops.wrist",
- 0x1a7b7094, ".frc971.control_loops.wrist.goal",
- ".frc971.control_loops.wrist.position",
- ".frc971.control_loops.wrist.output",
- ".frc971.control_loops.wrist.status") {
- Reinitialize(initial_position);
- }
-
- // Resets the plant so that it starts at initial_position.
- void Reinitialize(double initial_position) {
- initial_position_ = initial_position;
- wrist_plant_->X(0, 0) = initial_position_;
- wrist_plant_->X(1, 0) = 0.0;
- wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
- last_position_ = wrist_plant_->Y(0, 0);
- calibration_value_ = 0.0;
- last_voltage_ = 0.0;
- }
-
- // Returns the absolute angle of the wrist.
- double GetAbsolutePosition() const {
- return wrist_plant_->Y(0, 0);
- }
-
- // Returns the adjusted angle of the wrist.
- double GetPosition() const {
- return GetAbsolutePosition() - initial_position_;
- }
-
- // Sends out the position queue messages.
- void SendPositionMessage() {
- const double angle = GetPosition();
-
- ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
- my_wrist_loop_.position.MakeMessage();
- position->pos = angle;
-
- // Signal that the hall effect sensor has been triggered if it is within
- // the correct range.
- double abs_position = GetAbsolutePosition();
- if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
- abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
- position->hall_effect = true;
- } else {
- position->hall_effect = false;
- }
-
- // Only set calibration if it changed last cycle. Calibration starts out
- // with a value of 0.
- if ((last_position_ <
- constants::GetValues().wrist_hall_effect_start_angle ||
- last_position_ >
- constants::GetValues().wrist_hall_effect_stop_angle) &&
- position->hall_effect) {
- calibration_value_ =
- constants::GetValues().wrist_hall_effect_start_angle -
- initial_position_;
- }
-
- position->calibration = calibration_value_;
- position.Send();
- }
-
- // Simulates the wrist moving for one timestep.
- void Simulate() {
- last_position_ = wrist_plant_->Y(0, 0);
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- wrist_plant_->U << last_voltage_;
- wrist_plant_->Update();
-
- EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
- wrist_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
- wrist_plant_->Y(0, 0));
- last_voltage_ = my_wrist_loop_.output->voltage;
- }
-
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
- private:
- WristLoop my_wrist_loop_;
- double initial_position_;
- double last_position_;
- double calibration_value_;
- double last_voltage_;
-};
-
-class WristTest : public ::testing::Test {
- protected:
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- WristLoop my_wrist_loop_;
-
- // Create a loop and simulation plant.
- WristMotor wrist_motor_;
- WristMotorSimulation wrist_motor_plant_;
-
- WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
- 0x1a7b7094, ".frc971.control_loops.wrist.goal",
- ".frc971.control_loops.wrist.position",
- ".frc971.control_loops.wrist.output",
- ".frc971.control_loops.wrist.status"),
- wrist_motor_(&my_wrist_loop_),
- wrist_motor_plant_(0.5) {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- SendDSPacket(true);
- }
-
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder().enabled(enabled)
- .autonomous(false)
- .team_id(971).Send();
- ::aos::robot_state.FetchLatest();
- }
-
- void VerifyNearGoal() {
- my_wrist_loop_.goal.FetchLatest();
- my_wrist_loop_.position.FetchLatest();
- EXPECT_NEAR(my_wrist_loop_.goal->goal,
- wrist_motor_plant_.GetAbsolutePosition(),
- 1e-4);
- }
-
- virtual ~WristTest() {
- ::aos::robot_state.Clear();
- }
-};
-
-// Tests that the wrist zeros correctly and goes to a position.
-TEST_F(WristTest, ZerosCorrectly) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 400; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that the wrist zeros correctly starting on the hall effect sensor.
-TEST_F(WristTest, ZerosStartingOn) {
- wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
-
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that missing positions are correctly handled.
-TEST_F(WristTest, HandleMissingPosition) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 400; ++i) {
- if (i % 23) {
- wrist_motor_plant_.SendPositionMessage();
- }
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that loosing the encoder for a second triggers a re-zero.
-TEST_F(WristTest, RezeroWithMissingPos) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 800; ++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) {
- wrist_motor_plant_.SendPositionMessage();
- } else {
- if (i > 310) {
- // Should be re-zeroing now.
- EXPECT_TRUE(wrist_motor_.is_uninitialized());
- }
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
- }
- if (i == 410) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
-}
-
-// Tests that disabling while zeroing sends the state machine into the
-// uninitialized state.
-TEST_F(WristTest, DisableGoesUninitialized) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 800; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- // After 0.5 seconds, disable the robot.
- if (i > 50 && i < 200) {
- SendDSPacket(false);
- if (i > 100) {
- // Give the loop a couple cycled to get the message and then verify that
- // it is in the correct state.
- EXPECT_TRUE(wrist_motor_.is_uninitialized());
- // When disabled, we should be applying 0 power.
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- EXPECT_EQ(0, my_wrist_loop_.output->voltage);
- }
- } else {
- SendDSPacket(true);
- }
- if (i == 202) {
- // Verify that we are zeroing after the bot gets enabled again.
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- }
- VerifyNearGoal();
-}
-
-// Tests that the wrist can't get too far away from the zeroing position if the
-// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupNegative) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- // Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
- } else if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position,
- wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
- }
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
- // The cycle after we kick the zero position is the only cycle during
- // which we should expect to see a high uncapped power during zeroing.
- ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- } else {
- ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- }
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
- EXPECT_GT(3, capped_count);
-}
-
-// Tests that the wrist can't get too far away from the zeroing position if the
-// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupPositive) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
- for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- // Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
- } else {
- if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
- }
- }
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
- // The cycle after we kick the zero position is the only cycle during
- // which we should expect to see a high uncapped power during zeroing.
- EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- } else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
- }
- }
-
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
- SendDSPacket(true);
- }
- VerifyNearGoal();
- EXPECT_GT(3, capped_count);
-}
-
-} // namespace testing
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist_main.cc b/frc971/control_loops/wrist/wrist_main.cc
deleted file mode 100644
index 3da6d62..0000000
--- a/frc971/control_loops/wrist/wrist_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/wrist/wrist.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- frc971::control_loops::WristMotor wrist;
- wrist.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/control_loops/wrist/wrist_motor.q b/frc971/control_loops/wrist/wrist_motor.q
deleted file mode 100644
index 3dbeb53..0000000
--- a/frc971/control_loops/wrist/wrist_motor.q
+++ /dev/null
@@ -1,21 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-queue_group WristLoop {
- implements aos.control_loops.ControlLoop;
-
- message Position {
- double pos;
- bool hall_effect;
- // The exact pos when hall_effect last changed
- double calibration;
- };
-
- queue aos.control_loops.Goal goal;
- queue Position position;
- queue aos.control_loops.Output output;
- queue aos.control_loops.Status status;
-};
-
-queue_group WristLoop wrist;
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.cc b/frc971/control_loops/wrist/wrist_motor_plant.cc
deleted file mode 100644
index 64146d8..0000000
--- a/frc971/control_loops/wrist/wrist_motor_plant.cc
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "frc971/control_loops/wrist/wrist_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeWristController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.81581823335, 78.6334534274, 142.868137351;
- Eigen::Matrix<double, 1, 3> K;
- K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
- return StateFeedbackPlant<3, 1, 1>(plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
- ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
- return StateFeedbackLoop<3, 1, 1>(controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.h b/frc971/control_loops/wrist/wrist_motor_plant.h
deleted file mode 100644
index a40ffe5..0000000
--- a/frc971/control_loops/wrist/wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeWristController();
-
-StateFeedbackPlant<3, 1, 1> MakeWristPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
deleted file mode 100644
index 8f65cdd..0000000
--- a/frc971/input/JoystickReader.cc
+++ /dev/null
@@ -1,262 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include <math.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/prime/input/joystick_input.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/queues/Piston.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/queues/CameraTarget.q.h"
-
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shifters;
-using ::frc971::sensors::gyro;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::index_loop;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::hangers;
-using ::frc971::vision::target_angle;
-
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::ControlBit;
-
-namespace frc971 {
-namespace input {
-namespace joysticks {
-
-const ButtonLocation kDriveControlLoopEnable1(1, 7),
- kDriveControlLoopEnable2(1, 11);
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
-const ButtonLocation kQuickTurn(1, 5);
-
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
-const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
-
-const ButtonLocation kWristDown(3, 8);
-
-const ButtonLocation kFire(3, 11);
-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);
-
-class Reader : public ::aos::input::JoystickInput {
- public:
- static const bool kWristAlwaysDown = false;
-
- Reader() {
- shifters.MakeWithBuilder().set(true).Send();
- }
-
- virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- static bool is_high_gear = false;
- static double angle_adjust_goal = 0.42;
-
- if (data.GetControlBit(ControlBit::kAutonomous)) {
- if (data.PosEdge(ControlBit::kEnabled)){
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true).Send();
- } else if (data.NegEdge(ControlBit::kEnabled)) {
- LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false).Send();
- }
- } else { // teleop
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- double right_goal = 0.0;
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
- const double kThrottleGain = 1.0 / 2.5;
- if (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2)) {
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.position->right_encoder) / 2.0
- - throttle * kThrottleGain / 2.0;
- angle = gyro->angle;
- filtered_goal_distance = distance;
- }
- }
- is_control_loop_driving = true;
-
- //const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance < -kMaxVelocity * 0.02 +
- filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
- }
- if (!(drivetrain.goal.MakeWithBuilder()
- .steering(wheel)
- .throttle(throttle)
- .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal).right_goal(right_goal).Send())) {
- LOG(WARNING, "sending stick values failed\n");
- }
-
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear = false;
- }
- if (data.PosEdge(kShiftLow)) {
- is_high_gear = true;
- }
-
- // Whether we should change wrist positions to indicate that the hopper is
- // clear.
- bool hopper_clear = false;
-
- // Where the wrist should be to pick up a frisbee.
- // TODO(brians): Make these globally accessible and clean up auto.
- static const double kWristPickup = -0.580;
- static const double kWristNearGround = -0.4;
- // Where the wrist gets stored when up.
- // All the way up is 1.5.
- static const double kWristUp = 1.43;
- static const double kWristCleared = kWristUp - 0.2;
- static double wrist_down_position = kWristPickup;
- double wrist_up_position = kWristUp;
- double wrist_pickup_position = data.IsPressed(kIntake) ?
- kWristPickup : kWristNearGround;
- if (index_loop.status.FetchLatest() || index_loop.status.get()) {
- if (index_loop.status->hopper_disc_count >= 4) {
- wrist_down_position = kWristNearGround;
- } else {
- wrist_down_position = wrist_pickup_position;
- }
- hopper_clear = index_loop.status->hopper_clear;
- }
-
- ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Goal> shooter_goal =
- shooter.goal.MakeMessage();
- shooter_goal->velocity = 0;
- if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
- shooter_goal->velocity = 131;
- if (hopper_clear) wrist_up_position = kWristCleared;
- angle_adjust_goal = 0.70;
- } else if (data.IsPressed(kLongShot)) {
-#if 0
- target_angle.FetchLatest();
- if (target_angle.IsNewerThanMS(500)) {
- shooter_goal->velocity = target_angle->shooter_speed;
- angle_adjust_goal = target_angle->shooter_angle;
- // TODO(brians): do the math right here
- if (!hopper_clear) wrist_up_position = 0.70;
- } else {
- LOG(WARNING, "camera frame too old\n");
- // Pretend like no button is pressed.
- }
-#endif
- } else if (data.IsPressed(kMediumShot)) {
- // middle wheel on the back line (same as auto)
- shooter_goal->velocity = 395;
- if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.520;
- } else if (data.IsPressed(kShortShot)) {
- shooter_goal->velocity = 375;
- if (hopper_clear) wrist_up_position = kWristCleared;
- angle_adjust_goal = 0.671;
- }
- angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
-
- wrist.goal.MakeWithBuilder()
- .goal(data.IsPressed(kWristDown) ?
- wrist_down_position :
- wrist_up_position)
- .Send();
-
- ::aos::ScopedMessagePtr<control_loops::IndexLoop::Goal> index_goal =
- index_loop.goal.MakeMessage();
- if (data.IsPressed(kFire)) {
- // FIRE
- index_goal->goal_state = 4;
- } else if (shooter_goal->velocity != 0) {
- // get ready to shoot
- index_goal->goal_state = 3;
- } else if (data.IsPressed(kIntake)) {
- // intake
- index_goal->goal_state = 2;
- } else {
- // get ready to intake
- index_goal->goal_state = 1;
- }
- index_goal->force_fire = data.IsPressed(kForceFire);
-
- const bool index_up = data.IsPressed(kForceIndexUp);
- const bool index_down = data.IsPressed(kForceIndexDown);
- 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) {
- index_goal->index_voltage = 12.0;
- } 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();
- }
-
- static int hanger_cycles = 0;
- if (data.IsPressed(kDeployHangers)) {
- ++hanger_cycles;
- angle_adjust_goal = 0.4;
- } else {
- hanger_cycles = 0;
- }
- hangers.MakeWithBuilder().set(hanger_cycles >= 10).Send();
- }
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace frc971
-
-int main() {
- ::aos::Init();
- ::frc971::input::joysticks::Reader reader;
- reader.Run();
- ::aos::Cleanup();
-}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 33bf3c4..0fef215 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -1,22 +1,17 @@
{
'targets': [
{
- 'target_name': 'JoystickReader',
+ 'target_name': 'joystick_reader',
'type': 'executable',
'sources': [
- 'JoystickReader.cc',
+ 'joystick_reader.cc',
],
'dependencies': [
'<(AOS)/prime/input/input.gyp:joystick_input',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
],
@@ -30,10 +25,6 @@
'dependencies': [
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
new file mode 100644
index 0000000..4372098
--- /dev/null
+++ b/frc971/input/joystick_reader.cc
@@ -0,0 +1,124 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/piston.q.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace frc971 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kDriveControlLoopEnable1(1, 7),
+ kDriveControlLoopEnable2(1, 11);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ static bool is_high_gear = false;
+
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true).Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(false).Send();
+ }
+ } else { // teleop
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
+ const double kThrottleGain = 1.0 / 2.5;
+ if (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2)) {
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) / 2.0
+ - throttle * kThrottleGain / 2.0;
+ angle = gyro->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ //const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance < -kMaxVelocity * 0.02 +
+ filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!(drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send())) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear = true;
+ }
+ }
+ }
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 2f5c450..eb2bb9a 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -8,11 +8,7 @@
#include "bbb/sensor_reader.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/gyro_angle.q.h"
#include "frc971/constants.h"
#ifndef M_PI
@@ -20,10 +16,6 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
using ::frc971::sensors::gyro;
using ::aos::util::WrappingCounter;
@@ -36,28 +28,6 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-double wrist_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
-}
-
-double angle_adjust_translate(int32_t in) {
- static const double kCableDiameter = 0.060;
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
- (2 * M_PI);
-}
-
-double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
- (15.0 / 34.0) /*gears*/ * (2 * M_PI);
-}
-
-double index_translate(int32_t in) {
- return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
- (1.0) /*gears*/ * (2 * M_PI);
-}
-
// Translates values from the ADC into voltage.
double adc_translate(uint16_t in) {
static const double kVRefN = 0;
@@ -83,71 +53,6 @@
return (voltage - k.low) / (k.high - k.low);
}
-WrappingCounter top_rise_;
-WrappingCounter top_fall_;
-WrappingCounter bottom_rise_;
-WrappingCounter bottom_fall_delay_;
-WrappingCounter bottom_fall_;
-void ProcessData(const ::bbb::DataStruct *data, bool bad_gyro) {
- if (!bad_gyro) {
- gyro.MakeWithBuilder()
- .angle(gyro_translate(data->gyro_angle))
- .Send();
- }
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->main.right_drive))
- .left_encoder(-drivetrain_translate(data->main.left_drive))
- .left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data->main.right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage))
- .Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->main.wrist))
- .hall_effect(data->main.wrist_hall_effect)
- .calibration(wrist_translate(data->main.capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->main.shooter_angle))
- .bottom_hall_effect(data->main.angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->main.capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->main.shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->main.indexer))
- .top_disc_detect(data->main.top_disc)
- .top_disc_posedge_count(top_rise_.Update(data->main.top_rise_count))
- .top_disc_posedge_position(
- index_translate(data->main.capture_top_rise))
- .top_disc_negedge_count(top_fall_.Update(data->main.top_fall_count))
- .top_disc_negedge_position(
- index_translate(data->main.capture_top_fall))
- .bottom_disc_detect(data->main.bottom_disc)
- .bottom_disc_posedge_count(
- bottom_rise_.Update(data->main.bottom_rise_count))
- .bottom_disc_negedge_count(
- bottom_fall_.Update(data->main.bottom_fall_count))
- .bottom_disc_negedge_wait_position(index_translate(
- data->main.capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(
- bottom_fall_delay_.Update(data->main.bottom_fall_delay_count))
- .loader_top(data->main.loader_top)
- .loader_bottom(data->main.loader_bottom)
- .Send();
-}
-
void PacketReceived(const ::bbb::DataStruct *data,
const ::aos::time::Time &cape_timestamp) {
LOG(DEBUG, "cape timestamp %010" PRId32 ".%09" PRId32 "s\n",
@@ -169,12 +74,22 @@
} else {
bad_gyro = false;
}
- static int i = 0;
- ++i;
- if (i == 5) {
- i = 0;
- ProcessData(data, bad_gyro);
+
+ if (!bad_gyro) {
+ gyro.MakeWithBuilder()
+ .angle(gyro_translate(data->gyro_angle))
+ .Send();
}
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
+ .left_shifter_position(hall_translate(constants::GetValues().left_drive,
+ data->main.left_drive_hall))
+ .right_shifter_position(hall_translate(
+ constants::GetValues().right_drive, data->main.right_drive_hall))
+ .battery_voltage(battery_translate(data->main.battery_voltage))
+ .Send();
}
} // namespace
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index b9f5e6f..c6dbf63 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -6,20 +6,11 @@
#include "aos/common/logging/logging.h"
#include "aos/linux_code/init.h"
-#include "frc971/queues/Piston.q.h"
+#include "frc971/queues/piston.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
using ::frc971::control_loops::drivetrain;
using ::frc971::control_loops::shifters;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::hangers;
namespace frc971 {
namespace output {
@@ -55,54 +46,6 @@
SetSolenoid(1, shifters->set);
SetSolenoid(2, !shifters->set);
}
-
- wrist.output.FetchLatest();
- if (wrist.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(10, wrist.output->voltage / 12.0, kTalonBounds);
- } else {
- DisablePWMOutput(10);
- LOG(WARNING, "wrist not new enough\n");
- }
-
- shooter.output.FetchLatest();
- if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(4, shooter.output->voltage / 12.0, kTalonBounds);
- } else {
- DisablePWMOutput(4);
- LOG(WARNING, "shooter not new enough\n");
- }
-
- angle_adjust.output.FetchLatest();
- if (angle_adjust.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(1, -angle_adjust.output->voltage / 12.0, kTalonBounds);
- } else {
- DisablePWMOutput(1);
- LOG(WARNING, "angle adjust is not new enough\n");
- }
-
- index_loop.output.FetchLatest();
- if (index_loop.output.get()) {
- SetSolenoid(7, index_loop.output->loader_up);
- SetSolenoid(8, !index_loop.output->loader_up);
- SetSolenoid(6, index_loop.output->disc_clamped);
- SetSolenoid(3, index_loop.output->disc_ejected);
- }
- if (index_loop.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(8, index_loop.output->intake_voltage / 12.0, kTalonBounds);
- SetPWMOutput(9, index_loop.output->transfer_voltage / 12.0, kTalonBounds);
- SetPWMOutput(7, -index_loop.output->index_voltage / 12.0, kTalonBounds);
- } else {
- DisablePWMOutput(8);
- DisablePWMOutput(9);
- DisablePWMOutput(7);
- LOG(WARNING, "index not new enough\n");
- }
-
- hangers.FetchLatest();
- if (hangers.get()) {
- SetSolenoid(4, hangers->set);
- SetSolenoid(5, hangers->set);
- }
}
};
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 3474f33..1d81c69 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -23,7 +23,7 @@
],
},
{
- 'target_name': 'MotorWriter',
+ 'target_name': 'motor_writer',
'type': '<(aos_target)',
'sources': [
'motor_writer.cc'
@@ -32,10 +32,6 @@
'<(AOS)/prime/output/output.gyp:motor_output',
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index b1af09e..d841672 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -7,22 +7,9 @@
'<(AOS)/build/aos_all.gyp:Prime',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
- '../control_loops/wrist/wrist.gyp:wrist',
- '../control_loops/wrist/wrist.gyp:wrist_lib_test',
- '../control_loops/index/index.gyp:index',
- '../control_loops/index/index.gyp:index_lib_test',
- '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
- '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
- '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
- '../control_loops/shooter/shooter.gyp:shooter_lib_test',
- '../control_loops/shooter/shooter.gyp:shooter',
'../autonomous/autonomous.gyp:auto',
- '../input/input.gyp:JoystickReader',
- '../../vision/vision.gyp:OpenCVWorkTask',
- '../../vision/vision.gyp:GoalMaster',
- '../output/output.gyp:MotorWriter',
- '../output/output.gyp:CameraServer',
- #'camera/camera.gyp:frc971',
+ '../input/input.gyp:joystick_reader',
+ '../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:*',
],
diff --git a/frc971/prime/scripts/start_list.txt b/frc971/prime/scripts/start_list.txt
index 0991e96..79f9490 100644
--- a/frc971/prime/scripts/start_list.txt
+++ b/frc971/prime/scripts/start_list.txt
@@ -1,11 +1,6 @@
BinaryLogReader
-MotorWriter
-JoystickReader
+motor_writer
+joystick_reader
drivetrain
-CRIOLogReader
-angle_adjust
-wrist
-index
-shooter
auto
-gyro_sensor_receiver
+sensor_receiver
diff --git a/frc971/queues/CameraEnableQueue.q b/frc971/queues/CameraEnableQueue.q
deleted file mode 100644
index d61a7ee..0000000
--- a/frc971/queues/CameraEnableQueue.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.sensors;
-
-message CameraEnable {
- bool enable_camera;
-};
-
-queue CameraEnable camera_enable;
diff --git a/frc971/queues/CameraTarget.q b/frc971/queues/CameraTarget.q
deleted file mode 100644
index 5e11e0c..0000000
--- a/frc971/queues/CameraTarget.q
+++ /dev/null
@@ -1,17 +0,0 @@
-package frc971.vision;
-
-message CameraTarget {
- double percent_azimuth_off_center;
- double percent_elevation_off_center;
- // todo:(pschuh) add time syntax when havith more sleep
- uint64_t timestamp;
-};
-
-message TargetAngle {
- double target_angle;
- double shooter_speed;
- double shooter_angle;
-};
-
-queue TargetAngle target_angle;
-queue CameraTarget targets;
diff --git a/frc971/queues/GyroAngle.q b/frc971/queues/gyro_angle.q
similarity index 100%
rename from frc971/queues/GyroAngle.q
rename to frc971/queues/gyro_angle.q
diff --git a/frc971/queues/PhotoSensor.q b/frc971/queues/photo_sensor.q
similarity index 100%
rename from frc971/queues/PhotoSensor.q
rename to frc971/queues/photo_sensor.q
diff --git a/frc971/queues/Piston.q b/frc971/queues/piston.q
similarity index 100%
rename from frc971/queues/Piston.q
rename to frc971/queues/piston.q
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index ec5862a..277c91c 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -1,11 +1,9 @@
{
'variables': {
'queue_files': [
- 'CameraEnableQueue.q',
- 'GyroAngle.q',
- 'CameraTarget.q',
- 'PhotoSensor.q',
- 'Piston.q',
+ 'gyro_angle.q',
+ 'photo_sensor.q',
+ 'piston.q',
]
},
'targets': [
diff --git a/frc971/queues/sensor_values.h b/frc971/queues/sensor_values.h
deleted file mode 100644
index 95cfc27..0000000
--- a/frc971/queues/sensor_values.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef FRC971_QUEUES_SENSOR_VALUES_H_
-#define FRC971_QUEUES_SENSOR_VALUES_H_
-
-#include <stdint.h>
-
-namespace frc971 {
-
-struct sensor_values {
- // Anonymous union to make fixing the byte order on all of the 4-byte long
- // values easier.
- // TODO(brians) name this better
- union {
- struct {
- int32_t drive_left_encoder, drive_right_encoder;
- int32_t shooter_encoder;
- int32_t index_encoder, bottom_disc_negedge_wait_position;
- int32_t bottom_disc_posedge_count, bottom_disc_negedge_count;
- int32_t bottom_disc_negedge_wait_count;
- int32_t top_disc_posedge_count, top_disc_negedge_count;
- int32_t top_disc_posedge_position, top_disc_negedge_position;
- int32_t wrist_position, wrist_edge_position;
- int32_t angle_adjust_position;
- int32_t angle_adjust_middle_edge_position;
- int32_t angle_adjust_bottom_edge_position;
- };
- uint32_t encoders[17];
- };
-
- bool wrist_hall_effect;
- bool angle_adjust_middle_hall_effect;
- bool angle_adjust_bottom_hall_effect;
-
- bool top_disc, bottom_disc;
-};
-
-} // namespace frc971
-
-#endif // FRC971_QUEUES_SENSOR_VALUES_H_