Add stuff we have so far to wpilib_interface.
Change-Id: If880956affacf585d12f4df799f3c6426cf677f1
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index e3de5fb..9d69c6b 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -41,6 +41,7 @@
'//frc971/control_loops:state_feedback_loop',
'//third_party/eigen',
'//y2016:constants',
+ '//y2016/control_loops/drivetrain:polydrivetrain_plants',
],
)
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
index 2276108..94835c8 100644
--- a/y2016/actors/drivetrain_actor.cc
+++ b/y2016/actors/drivetrain_actor.cc
@@ -13,6 +13,7 @@
#include "y2016/actors/drivetrain_actor.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace y2016 {
@@ -20,7 +21,7 @@
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(constants::GetValues().make_drivetrain_loop()) {
+ loop_(::y2016::control_loops::drivetrain::MakeDrivetrainLoop()) {
loop_.set_controller_index(3);
}
@@ -29,7 +30,7 @@
const double yoffset = params.y_offset;
const double turn_offset =
- params.theta_offset * constants::GetValues().turn_width / 2.0;
+ params.theta_offset * constants::Values::kTurnWidth / 2.0;
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
@@ -42,10 +43,10 @@
profile.set_maximum_acceleration(params.maximum_acceleration);
profile.set_maximum_velocity(params.maximum_velocity);
turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
- constants::GetValues().turn_width /
+ constants::Values::kTurnWidth /
2.0);
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- constants::GetValues().turn_width / 2.0);
+ constants::Values::kTurnWidth / 2.0);
while (true) {
::aos::time::PhasedLoopXMS(5, 2500);
diff --git a/y2016/autonomous/auto.cc b/y2016/autonomous/auto.cc
index ad211cf..56844e0 100644
--- a/y2016/autonomous/auto.cc
+++ b/y2016/autonomous/auto.cc
@@ -85,9 +85,9 @@
auto drivetrain_action = actors::MakeDrivetrainAction(params);
drivetrain_action->Start();
left_initial_position +=
- distance - theta * constants::GetValues().turn_width / 2.0;
+ distance - theta * constants::Values::kTurnWidth / 2.0;
right_initial_position +=
- distance + theta * constants::GetValues().turn_width / 2.0;
+ distance + theta * constants::Values::kTurnWidth / 2.0;
return ::std::move(drivetrain_action);
}
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 20bafae..56fb2ad 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -24,151 +24,79 @@
namespace y2016 {
namespace constants {
-namespace {
+// ///// Mutual constants between robots. /////
+const int Values::kZeroingSampleSize;
+
+constexpr double Values::kDrivetrainEncoderRatio, Values::kLowGearRatio,
+ Values::kHighGearRatio, Values::kTurnWidth, Values::kShooterEncoderRatio,
+ Values::kIntakeEncoderRatio, Values::kShoulderEncoderRatio,
+ Values::kWristEncoderRatio, Values::kIntakePotRatio,
+ Values::kShoulderPotRatio, Values::kWristPotRatio,
+ Values::kIntakeEncoderIndexDifference,
+ Values::kShoulderEncoderIndexDifference,
+ Values::kWristEncoderIndexDifference;
+constexpr Values::Range Values::kIntakeRange, Values::kShoulderRange,
+ Values::kWristRange;
+
+namespace {
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
-// TODO(constants): Update these to what we're using this year.
-const double kCompDrivetrainEncoderRatio =
- (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0) /*encoder gears*/;
-const double kCompLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
-const double kCompHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
-
-const double kPracticeDrivetrainEncoderRatio = kCompDrivetrainEncoderRatio;
-const double kPracticeLowGearRatio = kCompLowGearRatio;
-const double kPracticeHighGearRatio = kCompHighGearRatio;
-
-const ShifterHallEffect kCompLeftDriveShifter{2.61, 2.33, 4.25, 3.28, 0.2, 0.7};
-const ShifterHallEffect kCompRightDriveShifter{2.94, 4.31, 4.32,
- 3.25, 0.2, 0.7};
-
-const ShifterHallEffect kPracticeLeftDriveShifter{2.80, 3.05, 4.15,
- 3.2, 0.2, 0.7};
-const ShifterHallEffect kPracticeRightDriveShifter{2.90, 3.75, 3.80,
- 2.98, 0.2, 0.7};
-
-const double kRobotWidth = 25.0 / 100.0 * 2.54;
-
-const int kZeroingSampleSize = 200;
-
-constexpr Values::Range kIntakeRange{// lower hard stop
- -0.4,
- // upper hard stop
- 2,
- // lower soft limit
- -0.3,
- // upper soft limit
- 1.9};
-constexpr Values::Range kShoulderRange{// lower hard stop
- -0.2,
- // upper hard stop
- 2.0,
- // lower soft limit
- -0.1,
- // upper soft limit
- 1.9};
-constexpr Values::Range kWristRange{// lower hard stop
- -2,
- // upper hard stop
- 2,
- // lower soft limit
- -1.9,
- // upper soft limit
- 1.9};
+// ///// Dynamic constants. /////
const Values *DoGetValuesForTeam(uint16_t team) {
switch (team) {
- case 1: // for tests
- return new Values{
- kCompDrivetrainEncoderRatio,
- kCompLowGearRatio,
- kCompHighGearRatio,
- kCompLeftDriveShifter,
- kCompRightDriveShifter,
- 0.5,
- ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
- 5.0, // drivetrain max speed
-
- // Intake
- {
- kIntakeRange,
- {kZeroingSampleSize, INTAKE_ENCODER_INDEX_DIFFERENCE, 0.0, 0.3},
- },
-
- // Shoulder
- {
- kShoulderRange,
- {kZeroingSampleSize, SHOULDER_ENCODER_INDEX_DIFFERENCE, 0.0, 0.3},
- },
-
- // Wrist
- {
- kWristRange,
- {kZeroingSampleSize, WRIST_ENCODER_INDEX_DIFFERENCE, 0.0, 0.3},
- },
- };
- break;
+ case 1:
case kCompTeamNumber:
return new Values{
- kCompDrivetrainEncoderRatio,
- kCompLowGearRatio,
- kCompHighGearRatio,
- kCompLeftDriveShifter,
- kCompRightDriveShifter,
- kRobotWidth,
- ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
// Intake
{
- kIntakeRange,
- {kZeroingSampleSize, INTAKE_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kIntakeEncoderIndexDifference,
+ 0.9, 0.3},
},
// Shoulder
{
- kShoulderRange,
- {kZeroingSampleSize, SHOULDER_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
+ 0.9, 0.3},
},
// Wrist
{
- kWristRange,
- {kZeroingSampleSize, WRIST_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
+ 0.9, 0.3},
},
};
break;
case kPracticeTeamNumber:
return new Values{
- kPracticeDrivetrainEncoderRatio,
- kPracticeLowGearRatio,
- kPracticeHighGearRatio,
- kPracticeLeftDriveShifter,
- kPracticeRightDriveShifter,
- kRobotWidth,
- ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
5.0, // drivetrain max speed
// Intake
{
- kIntakeRange,
- {kZeroingSampleSize, INTAKE_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kIntakeEncoderIndexDifference,
+ 0.9, 0.3},
},
// Shoulder
{
- kShoulderRange,
- {kZeroingSampleSize, SHOULDER_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
+ 0.9, 0.3},
},
// Wrist
{
- kWristRange,
- {kZeroingSampleSize, WRIST_ENCODER_INDEX_DIFFERENCE, 0.9, 0.3},
+ 0.0,
+ {Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
+ 0.9, 0.3},
},
};
break;
diff --git a/y2016/constants.h b/y2016/constants.h
index 202b436..ba16902 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -7,18 +7,6 @@
#include "frc971/shifter_hall_effect.h"
#include "frc971/constants.h"
-#define INTAKE_ENCODER_RATIO (16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0)
-#define INTAKE_POT_RATIO (16.0 / 58.0)
-#define INTAKE_ENCODER_INDEX_DIFFERENCE (2.0 * M_PI * INTAKE_ENCODER_RATIO)
-
-#define SHOULDER_ENCODER_RATIO (16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0)
-#define SHOULDER_POT_RATIO (16.0 / 58.0)
-#define SHOULDER_ENCODER_INDEX_DIFFERENCE (2.0 * M_PI * SHOULDER_ENCODER_RATIO)
-
-#define WRIST_ENCODER_RATIO (16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0)
-#define WRIST_POT_RATIO (16.0 / 48.0)
-#define WRIST_ENCODER_INDEX_DIFFERENCE (2.0 * M_PI * WRIST_ENCODER_RATIO)
-
namespace y2016 {
namespace constants {
@@ -34,22 +22,6 @@
// This structure contains current values for all of the things that change.
struct Values {
- // The ratio from the encoder shaft to the drivetrain wheels.
- double drivetrain_encoder_ratio;
-
- // The gear ratios from motor shafts to the drivetrain wheels for high and low
- // gear.
- double low_gear_ratio;
- double high_gear_ratio;
- ShifterHallEffect left_drive, right_drive;
-
- double turn_width;
-
- ::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
- ::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
-
- double drivetrain_max_speed;
-
// Defines a range of motion for a subsystem.
// These are all absolute positions in scaled units.
struct Range {
@@ -59,20 +31,59 @@
double upper;
};
+ // ///// Mutual constants between robots. /////
+ // TODO(constants): Update/check these with what we're using this year.
+ static const int kZeroingSampleSize = 200;
+
+ // The ratio from the encoder shaft to the drivetrain wheels.
+ static constexpr double kDrivetrainEncoderRatio =
+ (18.0 / 50.0) /*output reduction*/ * (56.0 / 30.0);
+
+ // The gear ratios from motor shafts to the drivetrain wheels for high and low
+ // gear.
+ static constexpr double kLowGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
+ static constexpr double kHighGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
+
+ static constexpr double kTurnWidth = 25.0 / 100.0 * 2.54; // Robot width.
+
+ // Ratios for our subsystems.
+ static constexpr double kShooterEncoderRatio = 1.0;
+ static constexpr double kIntakeEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kShoulderEncoderRatio = 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ static constexpr double kWristEncoderRatio = 16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
+
+ static constexpr double kIntakePotRatio = 16.0 / 58.0;
+ static constexpr double kShoulderPotRatio = 16.0 / 58.0;
+ static constexpr double kWristPotRatio = 16.0 / 48.0;
+
+ // Difference in radians between index pulses.
+ static constexpr double kIntakeEncoderIndexDifference = 2.0 * M_PI * kIntakeEncoderRatio;
+ static constexpr double kShoulderEncoderIndexDifference = 2.0 * M_PI * kShoulderEncoderRatio;
+ static constexpr double kWristEncoderIndexDifference = 2.0 * M_PI * kWristEncoderRatio;
+
+ // Subsystem motion ranges, in whatever units that their respective queues say
+ // the use.
+ static constexpr Range kIntakeRange{-0.4, 2.0, -0.3, 1.9};
+ static constexpr Range kShoulderRange{-0.2, 2.0, -0.1, 1.9};
+ static constexpr Range kWristRange{-2.0, 2.0, -1.9, 1.0};
+
+ // ///// Dynamic constants. /////
+ double drivetrain_max_speed;
+
struct Intake {
- Range limits;
+ double pot_offset;
ZeroingConstants zeroing;
};
Intake intake;
struct Shoulder {
- Range limits;
+ double pot_offset;
ZeroingConstants zeroing;
};
Shoulder shoulder;
struct Wrist {
- Range limits;
+ double pot_offset;
ZeroingConstants zeroing;
};
Wrist wrist;
diff --git a/y2016/control_loops/drivetrain/BUILD b/y2016/control_loops/drivetrain/BUILD
index 1150426..cfdcf42 100644
--- a/y2016/control_loops/drivetrain/BUILD
+++ b/y2016/control_loops/drivetrain/BUILD
@@ -60,6 +60,7 @@
deps = [
':polydrivetrain_plants',
'//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971:shifter_hall_effect',
'//y2016:constants',
],
)
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 2e5138c..ae2e62f 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -2,6 +2,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
@@ -12,6 +13,10 @@
namespace y2016 {
namespace control_loops {
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.5, 0.5};
+
const DrivetrainConfig &GetDrivetrainConfig() {
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
@@ -33,11 +38,11 @@
drivetrain::kV,
drivetrain::kT,
- constants::GetValues().turn_width,
- constants::GetValues().high_gear_ratio,
- constants::GetValues().low_gear_ratio,
- constants::GetValues().left_drive,
- constants::GetValues().right_drive};
+ constants::Values::kTurnWidth,
+ constants::Values::kHighGearRatio,
+ constants::Values::kLowGearRatio,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter};
return kDrivetrainConfig;
};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7cd67e3..e43c245 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -84,18 +84,16 @@
}
void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- const auto &values = constants::GetValues();
-
// Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) >= values.intake.limits.upper) {
+ if ((*goal)(0, 0) >= constants::Values::kIntakeRange.upper) {
LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.intake.limits.upper);
- (*goal)(0, 0) = values.intake.limits.upper;
+ constants::Values::kIntakeRange.upper);
+ (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
}
- if ((*goal)(0, 0) <= values.intake.limits.lower) {
+ if ((*goal)(0, 0) <= constants::Values::kIntakeRange.lower) {
LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.intake.limits.lower);
- (*goal)(0, 0) = values.intake.limits.lower;
+ constants::Values::kIntakeRange.lower);
+ (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
}
}
@@ -133,13 +131,12 @@
}
bool Intake::CheckHardLimits() {
- const auto &values = constants::GetValues();
// Returns whether hard limits have been exceeded.
- if (angle() >= values.intake.limits.upper_hard ||
- angle() <= values.intake.limits.lower_hard) {
+ if (angle() >= constants::Values::kIntakeRange.upper_hard ||
+ angle() <= constants::Values::kIntakeRange.lower_hard) {
LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- values.intake.limits.lower_hard, values.intake.limits.upper_hard);
+ constants::Values::kIntakeRange.lower_hard, constants::Values::kIntakeRange.upper_hard);
return true;
}
@@ -255,30 +252,29 @@
void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
// Limit the goals to min/max allowable angles.
- const auto &values = constants::GetValues();
- if ((*goal)(0, 0) >= values.shoulder.limits.upper) {
+ if ((*goal)(0, 0) >= constants::Values::kShoulderRange.upper) {
LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.upper);
- (*goal)(0, 0) = values.shoulder.limits.upper;
+ constants::Values::kShoulderRange.upper);
+ (*goal)(0, 0) = constants::Values::kShoulderRange.upper;
}
- if ((*goal)(0, 0) <= values.shoulder.limits.lower) {
+ if ((*goal)(0, 0) <= constants::Values::kShoulderRange.lower) {
LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- values.shoulder.limits.lower);
- (*goal)(0, 0) = values.shoulder.limits.lower;
+ constants::Values::kShoulderRange.lower);
+ (*goal)(0, 0) = constants::Values::kShoulderRange.lower;
}
const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
- if (wrist_goal_angle_ungrounded >= values.wrist.limits.upper) {
+ if (wrist_goal_angle_ungrounded >= constants::Values::kWristRange.upper) {
LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.upper);
- (*goal)(2, 0) = values.wrist.limits.upper + (*goal)(0, 0);
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.upper);
+ (*goal)(2, 0) = constants::Values::kWristRange.upper + (*goal)(0, 0);
}
- if (wrist_goal_angle_ungrounded <= values.wrist.limits.lower) {
+ if (wrist_goal_angle_ungrounded <= constants::Values::kWristRange.lower) {
LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
- wrist_goal_angle_ungrounded, values.wrist.limits.lower);
- (*goal)(2, 0) = values.wrist.limits.lower + (*goal)(0, 0);
+ wrist_goal_angle_ungrounded, constants::Values::kWristRange.lower);
+ (*goal)(2, 0) = constants::Values::kWristRange.lower + (*goal)(0, 0);
}
}
@@ -313,20 +309,19 @@
}
bool Arm::CheckHardLimits() {
- const auto &values = constants::GetValues();
- if (shoulder_angle() >= values.shoulder.limits.upper_hard ||
- shoulder_angle() <= values.shoulder.limits.lower_hard) {
+ if (shoulder_angle() >= constants::Values::kShoulderRange.upper_hard ||
+ shoulder_angle() <= constants::Values::kShoulderRange.lower_hard) {
LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
- shoulder_angle(), values.shoulder.limits.lower_hard,
- values.shoulder.limits.upper_hard);
+ shoulder_angle(), constants::Values::kShoulderRange.lower_hard,
+ constants::Values::kShoulderRange.upper_hard);
return true;
}
- if (wrist_angle() - shoulder_angle() >= values.wrist.limits.upper_hard ||
- wrist_angle() - shoulder_angle() <= values.wrist.limits.lower_hard) {
+ if (wrist_angle() - shoulder_angle() >= constants::Values::kWristRange.upper_hard ||
+ wrist_angle() - shoulder_angle() <= constants::Values::kWristRange.lower_hard) {
LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
- wrist_angle() - shoulder_angle(), values.wrist.limits.lower_hard,
- values.wrist.limits.upper_hard);
+ wrist_angle() - shoulder_angle(), constants::Values::kWristRange.lower_hard,
+ constants::Values::kWristRange.upper_hard);
return true;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 0db3112..fa227bd 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -14,6 +14,7 @@
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
+
#include "y2016/constants.h"
using ::aos::time::Time;
@@ -80,9 +81,11 @@
SuperstructureSimulation()
: intake_plant_(new IntakePlant(MakeIntakePlant())),
plant_arm_(new ArmPlant(MakeArmPlant())),
- pot_encoder_intake_(INTAKE_ENCODER_INDEX_DIFFERENCE),
- pot_encoder_shoulder_(SHOULDER_ENCODER_INDEX_DIFFERENCE),
- pot_encoder_wrist_(WRIST_ENCODER_INDEX_DIFFERENCE),
+ pot_encoder_intake_(
+ constants::Values::kIntakeEncoderIndexDifference),
+ pot_encoder_shoulder_(
+ constants::Values::kShoulderEncoderIndexDifference),
+ pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
".y2016.control_loops.superstructure.goal",
".y2016.control_loops.superstructure.status",
@@ -164,14 +167,12 @@
pot_encoder_wrist_.MoveTo(angle_wrist);
// Validate that everything is within range.
- EXPECT_GE(angle_intake, constants::GetValues().intake.limits.lower_hard);
- EXPECT_LE(angle_intake, constants::GetValues().intake.limits.upper_hard);
- EXPECT_GE(angle_shoulder,
- constants::GetValues().shoulder.limits.lower_hard);
- EXPECT_LE(angle_shoulder,
- constants::GetValues().shoulder.limits.upper_hard);
- EXPECT_GE(angle_wrist, constants::GetValues().wrist.limits.lower_hard);
- EXPECT_LE(angle_wrist, constants::GetValues().wrist.limits.upper_hard);
+ EXPECT_GE(angle_intake, constants::Values::kIntakeRange.lower_hard);
+ EXPECT_LE(angle_intake, constants::Values::kIntakeRange.upper_hard);
+ EXPECT_GE(angle_shoulder, constants::Values::kShoulderRange.lower_hard);
+ EXPECT_LE(angle_shoulder, constants::Values::kShoulderRange.upper_hard);
+ EXPECT_GE(angle_wrist, constants::Values::kWristRange.lower_hard);
+ EXPECT_LE(angle_wrist, constants::Values::kWristRange.upper_hard);
}
private:
@@ -301,12 +302,11 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
- const auto &values = constants::GetValues();
- EXPECT_NEAR(values.intake.limits.upper,
+ EXPECT_NEAR(constants::Values::kIntakeRange.upper,
superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(values.shoulder.limits.upper,
+ EXPECT_NEAR(constants::Values::kShoulderRange.upper,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(values.wrist.limits.upper + values.shoulder.limits.upper,
+ EXPECT_NEAR(constants::Values::kWristRange.upper + constants::Values::kShoulderRange.upper,
superstructure_queue_.status->wrist.angle, 0.001);
// Set some ridiculous goals to test lower limits.
@@ -326,11 +326,11 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
- EXPECT_NEAR(values.intake.limits.lower,
+ EXPECT_NEAR(constants::Values::kIntakeRange.lower,
superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(values.shoulder.limits.lower,
+ EXPECT_NEAR(constants::Values::kShoulderRange.lower,
superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(values.wrist.limits.lower + values.shoulder.limits.lower,
+ EXPECT_NEAR(constants::Values::kWristRange.lower + constants::Values::kShoulderRange.lower,
superstructure_queue_.status->wrist.angle, 0.001);
}
@@ -363,11 +363,11 @@
// Tests that starting at the lower hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, LowerHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.lower);
+ constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.lower);
+ constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.lower);
+ constants::Values::kWristRange.lower);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(0.0)
@@ -382,11 +382,11 @@
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.upper);
+ constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.upper);
+ constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.upper);
+ constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(0.0)
@@ -401,11 +401,11 @@
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.upper);
+ constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.upper);
+ constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.upper);
+ constants::Values::kWristRange.upper);
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.3)
.angle_shoulder(0.3)
@@ -446,17 +446,17 @@
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
superstructure_plant_.InitializeIntakePosition(
- constants::GetValues().intake.limits.lower);
+ constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
- constants::GetValues().shoulder.limits.lower);
+ constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeWristPosition(
- constants::GetValues().wrist.limits.lower);
+ constants::Values::kWristRange.lower);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send();
+ .angle_intake(0.0)
+ .angle_shoulder(0.0)
+ .angle_wrist(0.0)
+ .Send();
RunForTime(Time::InSeconds(8));
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index 9521d74..eb8b289 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -18,6 +18,7 @@
'//aos/linux_code:init',
'//aos/externals:wpilib',
'//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops:queues',
'//frc971/wpilib:joystick_sender',
'//frc971/wpilib:loop_output_handler',
'//frc971/wpilib:buffered_pcm',
@@ -26,10 +27,11 @@
'//frc971/wpilib:interrupt_edge_counting',
'//frc971/wpilib:wpilib_robot_base',
'//frc971/wpilib:encoder_and_potentiometer',
- '//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
'//frc971/wpilib:pdp_fetcher',
'//y2016:constants',
+ '//y2016/control_loops/shooter:shooter_queue',
+ '//y2016/control_loops/superstructure:superstructure_queue',
],
)
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 7bcf0bc..156f90a 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -30,10 +30,11 @@
#include "aos/linux_code/init.h"
#include "aos/common/messages/robot_state.q.h"
-#include "frc971/shifter_hall_effect.h"
-
+#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -52,6 +53,8 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
+using ::y2016::control_loops::shooter::shooter_queue;
+using ::y2016::control_loops::superstructure_queue;
namespace y2016 {
namespace wpilib {
@@ -67,34 +70,60 @@
return std::unique_ptr<T>(new T(std::forward<U>(u)...));
}
+// Translates for the sensor values to convert raw index pulses into something
+// with proper units.
+
+// TODO(comran): Template these methods since there is a lot of repetition here.
+double hall_translate(double in) {
+ // Turn voltage from our 3-state halls into a ratio that the loop can use.
+ return in / 5.0;
+}
+
double drivetrain_translate(int32_t in) {
return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::GetValues().drivetrain_encoder_ratio *
+ constants::Values::kDrivetrainEncoderRatio *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
double drivetrain_velocity_translate(double in) {
return (1.0 / in) / 256.0 /*cpr*/ *
- constants::GetValues().drivetrain_encoder_ratio *
+ constants::Values::kDrivetrainEncoderRatio *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
-float hall_translate(const constants::ShifterHallEffect &k, float in_low,
- float in_high) {
- const float low_ratio =
- 0.5 * (in_low - static_cast<float>(k.low_gear_low)) /
- static_cast<float>(k.low_gear_middle - k.low_gear_low);
- const float high_ratio =
- 0.5 +
- 0.5 * (in_high - static_cast<float>(k.high_gear_middle)) /
- static_cast<float>(k.high_gear_high - k.high_gear_middle);
+double shooter_translate(int32_t in) {
+ return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ constants::Values::kShooterEncoderRatio * (2 * M_PI /*radians*/);
+}
- // Return low when we are below 1/2, and high when we are above 1/2.
- if (low_ratio + high_ratio < 1.0) {
- return low_ratio;
- } else {
- return high_ratio;
- }
+double intake_translate(int32_t in) {
+ return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ constants::Values::kIntakeEncoderRatio * (2 * M_PI /*radians*/);
+}
+
+double shoulder_translate(int32_t in) {
+ return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ constants::Values::kShoulderEncoderRatio * (2 * M_PI /*radians*/);
+}
+
+double wrist_translate(int32_t in) {
+ return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ constants::Values::kWristEncoderRatio * (2 * M_PI /*radians*/);
+}
+
+double intake_pot_translate(double voltage) {
+ return voltage * constants::Values::kIntakePotRatio *
+ (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
+double shoulder_pot_translate(double voltage) {
+ return voltage * constants::Values::kShoulderPotRatio *
+ (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
+double wrist_pot_translate(double voltage) {
+ return voltage * constants::Values::kWristPotRatio *
+ (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
// TODO(constants): Update.
@@ -104,6 +133,7 @@
66.0 /* top gear reduction */ * 48.0 / 18.0 /* encoder gears */ /
60.0 /* seconds / minute */ * 256.0 /* CPR */;
+// Class to send position messages with sensor readings to our loops.
class SensorReader {
public:
SensorReader() {
@@ -114,6 +144,7 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
+ // Drivetrain setters.
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_left_encoder_ = ::std::move(encoder);
drivetrain_left_encoder_->SetMaxPeriod(0.005);
@@ -124,20 +155,69 @@
drivetrain_right_encoder_->SetMaxPeriod(0.005);
}
- void set_high_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
- high_left_drive_hall_ = ::std::move(analog);
+ void set_drivetrain_left_hall(::std::unique_ptr<AnalogInput> analog) {
+ drivetrain_left_hall_ = ::std::move(analog);
}
- void set_low_right_drive_hall(::std::unique_ptr<AnalogInput> analog) {
- low_right_drive_hall_ = ::std::move(analog);
+ void set_drivetrain_right_hall(::std::unique_ptr<AnalogInput> analog) {
+ drivetrain_right_hall_ = ::std::move(analog);
}
- void set_high_right_drive_hall(::std::unique_ptr<AnalogInput> analog) {
- high_right_drive_hall_ = ::std::move(analog);
+ // Shooter setters.
+ void set_shooter_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_filter_.Add(encoder.get());
+ shooter_left_encoder_ = ::std::move(encoder);
}
- void set_low_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
- low_left_drive_hall_ = ::std::move(analog);
+ void set_shooter_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_filter_.Add(encoder.get());
+ shooter_right_encoder_ = ::std::move(encoder);
+ }
+
+ // Intake setters.
+ void set_intake_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_filter_.Add(encoder.get());
+ intake_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ intake_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_intake_index(::std::unique_ptr<DigitalInput> index) {
+ encoder_filter_.Add(index.get());
+ intake_encoder_.set_index(::std::move(index));
+ }
+
+ // Shoulder setters.
+ void set_shoulder_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_filter_.Add(encoder.get());
+ shoulder_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_shoulder_potentiometer(
+ ::std::unique_ptr<AnalogInput> potentiometer) {
+ shoulder_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_shoulder_index(::std::unique_ptr<DigitalInput> index) {
+ encoder_filter_.Add(index.get());
+ shoulder_encoder_.set_index(::std::move(index));
+ }
+
+ // Wrist setters.
+ void set_wrist_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_filter_.Add(encoder.get());
+ wrist_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_wrist_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ wrist_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_wrist_index(::std::unique_ptr<DigitalInput> index) {
+ encoder_filter_.Add(index.get());
+ wrist_encoder_.set_index(::std::move(index));
}
// All of the DMA-related set_* calls must be made before this, and it doesn't
@@ -147,6 +227,9 @@
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ dma_synchronizer_->Add(&intake_encoder_);
+ dma_synchronizer_->Add(&shoulder_encoder_);
+ dma_synchronizer_->Add(&wrist_encoder_);
}
void operator()() {
@@ -193,39 +276,77 @@
drivetrain_message->right_speed =
drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
- drivetrain_message->low_left_hall = low_left_drive_hall_->GetVoltage();
- drivetrain_message->high_left_hall = high_left_drive_hall_->GetVoltage();
drivetrain_message->left_shifter_position =
- hall_translate(values.left_drive, drivetrain_message->low_left_hall,
- drivetrain_message->high_left_hall);
-
- drivetrain_message->low_right_hall = low_right_drive_hall_->GetVoltage();
- drivetrain_message->high_right_hall =
- high_right_drive_hall_->GetVoltage();
+ hall_translate(drivetrain_left_hall_->GetVoltage());
drivetrain_message->right_shifter_position =
- hall_translate(values.right_drive, drivetrain_message->low_right_hall,
- drivetrain_message->high_right_hall);
+ hall_translate(drivetrain_right_hall_->GetVoltage());
drivetrain_message.Send();
}
dma_synchronizer_->RunIteration();
+
+ {
+ auto shooter_message = shooter_queue.position.MakeMessage();
+ shooter_message->theta_left =
+ shooter_translate(shooter_left_encoder_->GetRaw());
+ shooter_message->theta_right =
+ shooter_translate(shooter_right_encoder_->GetRaw());
+ shooter_message.Send();
+ }
+
+ {
+ auto superstructure_message = superstructure_queue.position.MakeMessage();
+ CopyPotAndIndexPosition(intake_encoder_, &superstructure_message->intake,
+ intake_translate, intake_pot_translate, false,
+ values.intake.pot_offset);
+ CopyPotAndIndexPosition(shoulder_encoder_,
+ &superstructure_message->shoulder,
+ shoulder_translate, shoulder_pot_translate, false,
+ values.shoulder.pot_offset);
+ CopyPotAndIndexPosition(wrist_encoder_, &superstructure_message->wrist,
+ wrist_translate, wrist_pot_translate, false,
+ values.wrist.pot_offset);
+
+ superstructure_message.Send();
+ }
}
void Quit() { run_ = false; }
private:
+ void CopyPotAndIndexPosition(
+ const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndIndexPosition *position,
+ ::std::function<double(int32_t)> encoder_translate,
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double pot_offset) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.polled_encoder_value());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.polled_potentiometer_voltage()) +
+ pot_offset;
+ position->latched_encoder =
+ multiplier * encoder_translate(encoder.last_encoder_value());
+ position->latched_pot =
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ pot_offset;
+ position->index_pulses = encoder.index_posedge_count();
+ }
+
int32_t my_pid_;
DriverStation *ds_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
- ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
- ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
- ::std::unique_ptr<AnalogInput> low_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> low_right_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_right_drive_hall_;
+ ::std::unique_ptr<Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+ ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
+
+ ::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
+ ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_, shoulder_encoder_, wrist_encoder_;
::std::atomic<bool> run_{true};
DigitalGlitchFilter encoder_filter_, hall_filter_;
@@ -303,8 +424,8 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_right_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
+ drivetrain_right_;
::std::unique_ptr<DigitalInput> pressure_switch_;
::std::unique_ptr<Relay> compressor_relay_;
@@ -316,12 +437,12 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
- void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
- left_drivetrain_talon_ = ::std::move(t);
+ void set_drivetrain_left_talon(::std::unique_ptr<Talon> t) {
+ drivetrain_left_talon_ = ::std::move(t);
}
- void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
- right_drivetrain_talon_ = ::std::move(t);
+ void set_drivetrain_right_talon(::std::unique_ptr<Talon> t) {
+ drivetrain_right_talon_ = ::std::move(t);
}
private:
@@ -332,18 +453,85 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
- right_drivetrain_talon_->Set(queue->right_voltage / 12.0);
+ drivetrain_left_talon_->Set(-queue->left_voltage / 12.0);
+ drivetrain_right_talon_->Set(queue->right_voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "drivetrain output too old\n");
- left_drivetrain_talon_->Disable();
- right_drivetrain_talon_->Disable();
+ drivetrain_left_talon_->Disable();
+ drivetrain_right_talon_->Disable();
}
- ::std::unique_ptr<Talon> left_drivetrain_talon_;
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
+ ::std::unique_ptr<Talon> drivetrain_left_talon_, drivetrain_right_talon_;
+};
+
+class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_shooter_left_talon(::std::unique_ptr<Talon> t) {
+ shooter_left_talon_ = ::std::move(t);
+ }
+
+ void set_shooter_right_talon(::std::unique_ptr<Talon> t) {
+ shooter_right_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2016::control_loops::shooter::shooter_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2016::control_loops::shooter::shooter_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ shooter_left_talon_->Set(queue->voltage_left / 12.0);
+ shooter_right_talon_->Set(queue->voltage_right / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Shooter output too old.\n");
+ shooter_left_talon_->Disable();
+ shooter_right_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> shooter_left_talon_, shooter_right_talon_;
+};
+
+class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_intake_talon(::std::unique_ptr<Talon> t) {
+ intake_talon_ = ::std::move(t);
+ }
+
+ void set_shoulder_talon(::std::unique_ptr<Talon> t) {
+ shoulder_talon_ = ::std::move(t);
+ }
+
+ void set_wrist_talon(::std::unique_ptr<Talon> t) {
+ wrist_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2016::control_loops::superstructure_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2016::control_loops::superstructure_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ intake_talon_->Set(queue->voltage_intake / 12.0);
+ shoulder_talon_->Set(-queue->voltage_shoulder / 12.0);
+ wrist_talon_->Set(queue->voltage_wrist / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Superstructure output too old.\n");
+ intake_talon_->Disable();
+ shoulder_talon_->Disable();
+ wrist_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -367,10 +555,23 @@
// TODO(constants): Update these input numbers.
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_high_left_drive_hall(make_unique<AnalogInput>(1));
- reader.set_low_left_drive_hall(make_unique<AnalogInput>(0));
- reader.set_high_right_drive_hall(make_unique<AnalogInput>(2));
- reader.set_low_right_drive_hall(make_unique<AnalogInput>(3));
+ reader.set_drivetrain_left_hall(make_unique<AnalogInput>(1));
+ reader.set_drivetrain_right_hall(make_unique<AnalogInput>(2));
+
+ reader.set_shooter_left_encoder(make_encoder(0));
+ reader.set_shooter_right_encoder(make_encoder(0));
+
+ reader.set_intake_encoder(make_encoder(0));
+ reader.set_intake_index(make_unique<DigitalInput>(0));
+ reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
+
+ reader.set_shoulder_encoder(make_encoder(0));
+ reader.set_shoulder_index(make_unique<DigitalInput>(0));
+ reader.set_shoulder_potentiometer(make_unique<AnalogInput>(0));
+
+ reader.set_wrist_encoder(make_encoder(0));
+ reader.set_wrist_index(make_unique<DigitalInput>(0));
+ reader.set_wrist_potentiometer(make_unique<AnalogInput>(0));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -379,12 +580,29 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_left_drivetrain_talon(
+ drivetrain_writer.set_drivetrain_left_talon(
::std::unique_ptr<Talon>(new Talon(5)));
- drivetrain_writer.set_right_drivetrain_talon(
+ drivetrain_writer.set_drivetrain_right_talon(
::std::unique_ptr<Talon>(new Talon(2)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ ShooterWriter shooter_writer;
+ shooter_writer.set_shooter_left_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ shooter_writer.set_shooter_right_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
+
+ SuperstructureWriter superstructure_writer;
+ superstructure_writer.set_intake_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ superstructure_writer.set_shoulder_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ superstructure_writer.set_wrist_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)));
+ ::std::thread superstructure_writer_thread(
+ ::std::ref(superstructure_writer));
+
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
@@ -418,6 +636,10 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
+ shooter_writer.Quit();
+ shooter_writer_thread.join();
+ superstructure_writer.Quit();
+ superstructure_writer_thread.join();
solenoid_writer.Quit();
solenoid_thread.join();