Work on getting wpilib_interface finished
I moved all the constants together to make it somewhat easier to work. I
also finished renaming serializer to indexer because Austin is more
stubborn than the rest of the team combined.
It at least compiles now. The TODOs are also also much more specific to
the few remaining pieces.
Change-Id: Id3baf2b5be0a946345b152612c2099cf1b599727
diff --git a/y2017/BUILD b/y2017/BUILD
index beaa043..55ea1ca 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -14,6 +14,11 @@
'//aos/common:mutex',
'//frc971:constants',
'//y2017/control_loops/drivetrain:polydrivetrain_plants',
+ '//y2017/control_loops/superstructure/shooter:shooter_plants',
+ '//y2017/control_loops/superstructure/intake:intake_plants',
+ '//y2017/control_loops/superstructure/turret:turret_plants',
+ '//y2017/control_loops/superstructure/indexer:indexer_plants',
+ '//y2017/control_loops/superstructure/hood:hood_plants',
],
)
@@ -51,7 +56,6 @@
'//frc971/wpilib:pdp_fetcher',
'//frc971/wpilib:ADIS16448',
'//frc971/wpilib:dma',
- '//y2017/control_loops/drivetrain:polydrivetrain_plants',
'//y2017/control_loops/superstructure:superstructure_queue',
'//y2017/actors:autonomous_action_queue',
],
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 05eca31..295a800 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -27,24 +27,34 @@
const int Values::kZeroingSampleSize;
-constexpr double Values::kDrivetrainEncoderRatio;
+constexpr double Values::kDrivetrainCyclesPerRevolution,
+ Values::kDrivetrainEncoderCountsPerRevolution,
+ Values::kDrivetrainEncoderRatio,
+ Values::kMaxDrivetrainEncoderPulsesPerSecond;
-constexpr double Values::kShooterEncoderRatio;
+constexpr double Values::kShooterEncoderCountsPerRevolution,
+ Values::kShooterEncoderRatio, Values::kMaxShooterEncoderPulsesPerSecond;
-constexpr double Values::kIntakeEncoderRatio, Values::kIntakePotRatio,
- Values::kIntakeEncoderIndexDifference;
+constexpr double Values::kIntakeEncoderCountsPerRevolution,
+ Values::kIntakeEncoderRatio, Values::kIntakePotRatio,
+ Values::kIntakeEncoderIndexDifference,
+ Values::kMaxIntakeEncoderPulsesPerSecond;
constexpr ::frc971::constants::Range Values::kIntakeRange;
-constexpr double Values::kHoodEncoderRatio, Values::kHoodPotRatio,
- Values::kHoodEncoderIndexDifference;
+constexpr double Values::kHoodEncoderCountsPerRevolution,
+ Values::kHoodEncoderRatio, Values::kHoodPotRatio,
+ Values::kHoodEncoderIndexDifference, Values::kMaxHoodEncoderPulsesPerSecond;
constexpr ::frc971::constants::Range Values::kHoodRange;
-constexpr double Values::kTurretEncoderRatio, Values::kTurretPotRatio,
- Values::kTurretEncoderIndexDifference;
+constexpr double Values::kTurretEncoderCountsPerRevolution,
+ Values::kTurretEncoderRatio, Values::kTurretPotRatio,
+ Values::kTurretEncoderIndexDifference,
+ Values::kMaxTurretEncoderPulsesPerSecond;
constexpr ::frc971::constants::Range Values::kTurretRange;
-constexpr double Values::kIndexerEncoderRatio,
- Values::kIndexerEncoderIndexDifference;
+constexpr double Values::kMaxIndexerEncoderCountsPerRevolution,
+ Values::kIndexerEncoderRatio, Values::kIndexerEncoderIndexDifference,
+ Values::kMaxIndexerEncoderPulsesPerSecond;
namespace {
diff --git a/y2017/constants.h b/y2017/constants.h
index 20fcb56..2c8916f 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -6,6 +6,13 @@
#include "frc971/constants.h"
+#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
+#include "y2017/control_loops/superstructure/intake/intake_plant.h"
+#include "y2017/control_loops/superstructure/turret/turret_plant.h"
+#include "y2017/control_loops/superstructure/indexer/indexer_plant.h"
+#include "y2017/control_loops/superstructure/hood/hood_plant.h"
+
namespace y2017 {
namespace constants {
@@ -20,8 +27,6 @@
// All ratios are from the encoder shaft to the output units.
struct Values {
- // TODO(constants): Update/check these with what we're using this year.
-
struct Intake {
double pot_offset;
::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
@@ -39,60 +44,73 @@
static const int kZeroingSampleSize = 200;
- // TODO(Brian): This isn't all the way to the output yet.
- static constexpr double kDrivetrainEncoderRatio = 1.0 ;
+ static constexpr double kDrivetrainCyclesPerRevolution = 256;
+ static constexpr double kDrivetrainEncoderCountsPerRevolution =
+ kDrivetrainCyclesPerRevolution * 4;
+ static constexpr double kDrivetrainEncoderRatio =
+ 1.0 * control_loops::drivetrain::kWheelRadius;
+ static constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
+ control_loops::drivetrain::kFreeSpeed *
+ control_loops::drivetrain::kHighOutputRatio /
+ constants::Values::kDrivetrainEncoderRatio *
+ kDrivetrainEncoderCountsPerRevolution;
+ static constexpr double kShooterEncoderCountsPerRevolution = 2048 * 4;
static constexpr double kShooterEncoderRatio = 32.0 / 48.0;
+ static constexpr double kMaxShooterEncoderPulsesPerSecond =
+ control_loops::superstructure::shooter::kFreeSpeed *
+ control_loops::superstructure::shooter::kOutputRatio /
+ constants::Values::kShooterEncoderRatio *
+ kShooterEncoderCountsPerRevolution;
+ static constexpr double kIntakeEncoderCountsPerRevolution = 1024 * 4;
static constexpr double kIntakeEncoderRatio =
(16.0 * 0.25) * (20.0 / 40.0) / (2.0 * M_PI) * 0.0254;
static constexpr double kIntakePotRatio = (16 * 0.25) / (2.0 * M_PI) * 0.0254;
static constexpr double kIntakeEncoderIndexDifference =
2.0 * M_PI * kIntakeEncoderRatio;
-
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond =
+ control_loops::superstructure::intake::kFreeSpeed * control_loops::superstructure::intake::kOutputRatio /
+ constants::Values::kIntakeEncoderRatio *
+ kIntakeEncoderCountsPerRevolution;
static constexpr ::frc971::constants::Range kIntakeRange{
- // Lower hard stop in meters (from inches).
- -0.600 * 0.0254,
- // Upper hard stop in meters (from inches).
- 8.655 * 0.0254,
- // Lower soft stop in meters (from inches).
- 0.000,
- // Upper soft stop in meters (from inches).
- 8.530 * 0.0254};
+ -0.29878633 * 0.0254, 9.23012063 * 0.0254, (-0.29878633 + 0.125) * 0.0254,
+ (9.23012063 - 0.125) * 0.0254};
+ static constexpr double kHoodEncoderCountsPerRevolution = 2048 * 4;
static constexpr double kHoodEncoderRatio = 20.0 / 345.0;
static constexpr double kHoodPotRatio = 20.0 / 345.0;
static constexpr double kHoodEncoderIndexDifference =
2.0 * M_PI * kHoodEncoderRatio;
-
+ static constexpr double kMaxHoodEncoderPulsesPerSecond =
+ control_loops::superstructure::hood::kFreeSpeed * control_loops::superstructure::hood::kOutputRatio /
+ constants::Values::kHoodEncoderRatio * kHoodEncoderCountsPerRevolution;
static constexpr ::frc971::constants::Range kHoodRange{
- // Lower hard stop in radians.
- -1.0 / 345.0 * 2.0 * M_PI,
- // Upper hard stop in radians.
- 39.0 / 345.0 * 2.0 * M_PI,
- // Lower soft stop in radians.
- 0.0,
- // Upper soft stop in radians.
- (39.0 - 1.0) / 345.0 * 2.0 * M_PI};
+ -0.39 * M_PI / 2.0, 37.11 * M_PI / 2.0, (-0.39 + 1.0) * M_PI / 2.0,
+ (37.11 - 1.0) * M_PI / 2.0};
+ static constexpr double kTurretEncoderCountsPerRevolution = 1024 * 4;
static constexpr double kTurretEncoderRatio = 16.0 / 92.0;
static constexpr double kTurretPotRatio = 16.0 / 92.0;
static constexpr double kTurretEncoderIndexDifference =
2.0 * M_PI * kTurretEncoderRatio;
-
+ static constexpr double kMaxTurretEncoderPulsesPerSecond =
+ control_loops::superstructure::turret::kFreeSpeed * control_loops::superstructure::turret::kOutputRatio /
+ constants::Values::kTurretEncoderRatio *
+ kTurretEncoderCountsPerRevolution;
static constexpr ::frc971::constants::Range kTurretRange{
- // Lower hard stop in radians.
- -460.0 / 2.0 * M_PI / 180.0,
- // Upper hard stop in radians.
- 460.0 / 2.0 * M_PI / 180.0,
- // Lower soft stop in radians.
- -450.0 / 2.0 * M_PI / 180.0,
- // Upper soft stop in radians.
- 450.0 / 2.0 * M_PI / 180.0};
+ -460.0 / 2.0 * M_PI / 180.0, 460.0 / 2.0 * M_PI / 180.0,
+ -450.0 / 2.0 * M_PI / 180.0, 450.0 / 2.0 * M_PI / 180.0};
+ static constexpr double kMaxIndexerEncoderCountsPerRevolution = 256 * 4;
static constexpr double kIndexerEncoderRatio = (18.0 / 36.0) * (12.0 / 84.0);
static constexpr double kIndexerEncoderIndexDifference =
2.0 * M_PI * kIndexerEncoderRatio;
+ static constexpr double kMaxIndexerEncoderPulsesPerSecond =
+ control_loops::superstructure::indexer::kFreeSpeed *
+ control_loops::superstructure::indexer::kOutputRatio /
+ constants::Values::kIndexerEncoderRatio *
+ kMaxIndexerEncoderCountsPerRevolution;
double drivetrain_max_speed;
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
index 17e0079..8a65128 100755
--- a/y2017/control_loops/python/drivetrain.py
+++ b/y2017/control_loops/python/drivetrain.py
@@ -23,8 +23,9 @@
self.stall_torque = 2.42 * self.num_motors * 0.60
# Stall Current in Amps
self.stall_current = 133.0 * self.num_motors
- # Free Speed in RPM. Used number from last year.
- self.free_speed = 5500.0
+ self.free_speed_rpm = 5500.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60
# Free Current in Amps
self.free_current = 4.7 * self.num_motors
# Moment of inertia of the drivetrain in kg m^2
@@ -38,7 +39,7 @@
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -321,7 +322,7 @@
drivetrain_low_low.stall_torque))
dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
drivetrain_low_low.stall_current))
- dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeed", "%f",
drivetrain_low_low.free_speed))
dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
drivetrain_low_low.free_current))
@@ -343,6 +344,8 @@
drivetrain_low_low.G_low))
dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
drivetrain_high_high.G_high))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighOutputRatio", "%f",
+ drivetrain_high_high.G_high * drivetrain_high_high.r))
dog_loop_writer.Write(argv[1], argv[2])
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 6ab8072..52eec8e 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -24,15 +24,16 @@
self.stall_torque = 0.43
# Stall Current in Amps
self.stall_current = 53.0
- # Free Speed in RPM
- self.free_speed = 13180.0
+ self.free_speed_rpm = 13180.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60
# Free Current in Amps
self.free_current = 1.8
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -316,6 +317,10 @@
hood = Hood('Hood')
loop_writer = control_loop.ControlLoopWriter('Hood', [hood],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kFreeSpeed', '%f', hood.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kOutputRatio', '%f', hood.G))
loop_writer.Write(argv[1], argv[2])
integral_hood = IntegralHood('IntegralHood')
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 4797600..01d9797 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -20,8 +20,9 @@
self.stall_torque = 0.71
# Stall Current in Amps
self.stall_current = 134
- # Free Speed in RPM
- self.free_speed = 18730.0
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
self.free_current = 0.7
# Moment of inertia of the indexer halves in kg m^2
@@ -54,7 +55,7 @@
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -284,6 +285,10 @@
indexer = Indexer('Indexer')
loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kFreeSpeed', '%f', indexer.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kOutputRatio', '%f', indexer.G))
loop_writer.Write(argv[1], argv[2])
integral_indexer = IntegralIndexer('IntegralIndexer')
diff --git a/y2017/control_loops/python/intake.py b/y2017/control_loops/python/intake.py
index 41cfc0d..712dfa4 100755
--- a/y2017/control_loops/python/intake.py
+++ b/y2017/control_loops/python/intake.py
@@ -24,15 +24,16 @@
self.stall_torque = 0.71
# Stall Current in Amps
self.stall_current = 134.0
- # Free Speed in RPM
- self.free_speed = 18730.0
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
self.free_current = 0.7
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -308,6 +309,10 @@
intake = Intake('Intake')
loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant('kFreeSpeed', '%f',
+ intake.free_speed))
+ loop_writer.AddConstant(control_loop.Constant('kOutputRatio', '%f',
+ intake.G * intake.r))
loop_writer.Write(argv[1], argv[2])
integral_intake = IntegralIntake('IntegralIntake')
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index d7c505b..25e4b24 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -23,7 +23,9 @@
# Stall Current in Amps
self.stall_current = 134.0 * self.num_motors
# Free Speed in RPM
- self.free_speed = 18730.0
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
self.free_current = 0.7 * self.num_motors
# Moment of inertia of the shooter wheel in kg m^2
@@ -33,7 +35,7 @@
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -266,6 +268,10 @@
shooter = Shooter('Shooter')
loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kFreeSpeed', '%f', shooter.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kOutputRatio', '%f', shooter.G))
loop_writer.Write(argv[1], argv[2])
integral_shooter = IntegralShooter('IntegralShooter')
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index 454be9e..a69e3e1 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -24,15 +24,16 @@
self.stall_torque = 0.43
# Stall Current in Amps
self.stall_current = 53
- # Free Speed in RPM
- self.free_speed = 13180
+ self.free_speed_rpm = 13180
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
self.free_current = 1.8
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -302,6 +303,10 @@
turret = Turret('Turret')
loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
namespaces=namespaces)
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kFreeSpeed', '%f', turret.free_speed))
+ loop_writer.AddConstant(control_loop.Constant(
+ 'kOutputRatio', '%f', turret.G))
loop_writer.Write(argv[1], argv[2])
integral_turret = IntegralTurret('IntegralTurret')
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 7b03141..8f27d80 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -1,8 +1,5 @@
-package(default_visibility = ['//visibility:public'])
-
genrule(
name = 'genrule_hood',
- visibility = ['//visibility:private'],
cmd = '$(location //y2017/control_loops/python:hood) $(OUTS)',
tools = [
'//y2017/control_loops/python:hood',
@@ -17,6 +14,7 @@
cc_library(
name = 'hood_plants',
+ visibility = ['//visibility:public'],
srcs = [
'hood_plant.cc',
'hood_integral_plant.cc',
diff --git a/y2017/control_loops/superstructure/indexer/BUILD b/y2017/control_loops/superstructure/indexer/BUILD
index 01a52c4..3df0810 100644
--- a/y2017/control_loops/superstructure/indexer/BUILD
+++ b/y2017/control_loops/superstructure/indexer/BUILD
@@ -1,8 +1,5 @@
-package(default_visibility = ['//visibility:public'])
-
genrule(
name = 'genrule_indexer',
- visibility = ['//visibility:private'],
cmd = '$(location //y2017/control_loops/python:indexer) $(OUTS)',
tools = [
'//y2017/control_loops/python:indexer',
@@ -17,6 +14,7 @@
cc_library(
name = 'indexer_plants',
+ visibility = ['//visibility:public'],
srcs = [
'indexer_plant.cc',
'indexer_integral_plant.cc',
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 41e6869..68faeba 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -1,8 +1,5 @@
-package(default_visibility = ['//visibility:public'])
-
genrule(
name = 'genrule_intake',
- visibility = ['//visibility:private'],
cmd = '$(location //y2017/control_loops/python:intake) $(OUTS)',
tools = [
'//y2017/control_loops/python:intake',
@@ -17,6 +14,7 @@
cc_library(
name = 'intake_plants',
+ visibility = ['//visibility:public'],
srcs = [
'intake_plant.cc',
'intake_integral_plant.cc',
diff --git a/y2017/control_loops/superstructure/shooter/BUILD b/y2017/control_loops/superstructure/shooter/BUILD
index 588e12c..cb9fcf4 100644
--- a/y2017/control_loops/superstructure/shooter/BUILD
+++ b/y2017/control_loops/superstructure/shooter/BUILD
@@ -1,8 +1,5 @@
-package(default_visibility = ['//visibility:public'])
-
genrule(
name = 'genrule_shooter',
- visibility = ['//visibility:private'],
cmd = '$(location //y2017/control_loops/python:shooter) $(OUTS)',
tools = [
'//y2017/control_loops/python:shooter',
@@ -17,6 +14,7 @@
cc_library(
name = 'shooter_plants',
+ visibility = ['//visibility:public'],
srcs = [
'shooter_plant.cc',
'shooter_integral_plant.cc',
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index b1e63bb..be5b824 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -7,8 +7,8 @@
//import "frc971/control_loops/control_loops.q";
struct IntakeGoal {
- // Zero on the intake is when the intake is retracted inside the robot,
- // unable to intake. Positive is out.
+ // Zero for the intake is when the front tube is tangent with the front of the
+ // frame. Positive is out.
// Goal distance of the intake.
double distance;
@@ -20,8 +20,8 @@
double voltage_rollers;
};
-struct SerializerGoal {
- // Serializer angular velocity goals in radians/second.
+struct IndexerGoal {
+ // Indexer angular velocity goals in radians/second.
double angular_velocity;
};
@@ -38,8 +38,8 @@
};
struct HoodGoal {
- // Angle the hood is currently at. An angle of zero hood is at the lower soft stop, angle
- // increases as hood rises.
+ // Angle the hood is currently at. An angle of zero is at the lower hard
+ // stop, angle increases as hood rises.
double angle;
// Caps on velocity/acceleration for profiling. 0 for the default.
@@ -47,18 +47,20 @@
};
struct ShooterGoal {
- // Angular velocity goals in radians/second.
+ // Angular velocity goals in radians/second. Positive is shooting out of the
+ // robot.
double angular_velocity;
};
-struct SerializerStatus {
- // The current average velocity in radians/second.
+struct IndexerStatus {
+ // The current average velocity in radians/second. Positive is moving balls up
+ // towards the shooter. This is the angular velocity of the inner piece.
double avg_angular_velocity;
// The current instantaneous filtered velocity in radians/second.
double angular_velocity;
- // True if the serializer is ready. It is better to compare the velocities
+ // True if the indexer is ready. It is better to compare the velocities
// directly so there isn't confusion on if the goal is up to date.
bool ready;
@@ -86,7 +88,7 @@
message Goal {
IntakeGoal intake;
- SerializerGoal serializer;
+ IndexerGoal indexer;
TurretGoal turret;
HoodGoal hood;
ShooterGoal shooter;
@@ -103,7 +105,7 @@
.frc971.control_loops.ProfiledJointStatus intake;
.frc971.control_loops.ProfiledJointStatus turret;
.frc971.control_loops.ProfiledJointStatus hood;
- SerializerStatus serializer;
+ IndexerStatus indexer;
ShooterStatus shooter;
};
@@ -115,8 +117,8 @@
// out.
.frc971.PotAndIndexPosition intake;
- // Serializer angle in radians.
- double theta_serializer;
+ // Indexer angle in radians.
+ double theta_indexer;
// The sensor readings for the turret. The units and sign are defined the
// same as what's in the Goal message.
@@ -133,13 +135,13 @@
message Output {
// Voltages for some of the subsystems.
double voltage_intake;
- double voltage_serializer;
+ double voltage_indexer;
double voltage_shooter;
// Rollers on the intake.
double voltage_intake_rollers;
- // Roller on the serializer
- double voltage_serializer_rollers;
+ // Roller on the indexer
+ double voltage_indexer_rollers;
double voltage_turret;
double voltage_hood;
diff --git a/y2017/control_loops/superstructure/turret/BUILD b/y2017/control_loops/superstructure/turret/BUILD
index 2370fd7..70b9508 100644
--- a/y2017/control_loops/superstructure/turret/BUILD
+++ b/y2017/control_loops/superstructure/turret/BUILD
@@ -1,8 +1,5 @@
-package(default_visibility = ['//visibility:public'])
-
genrule(
name = 'genrule_turret',
- visibility = ['//visibility:private'],
cmd = '$(location //y2017/control_loops/python:turret) $(OUTS)',
tools = [
'//y2017/control_loops/python:turret',
@@ -17,6 +14,7 @@
cc_library(
name = 'turret_plants',
+ visibility = ['//visibility:public'],
srcs = [
'turret_plant.cc',
'turret_integral_plant.cc',
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 31ba2ff..e8bd214 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -32,7 +32,6 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/actors/autonomous_action.q.h"
@@ -57,12 +56,13 @@
using ::frc971::control_loops::drivetrain_queue;
using ::y2017::control_loops::superstructure_queue;
+using ::y2017::constants::Values;
namespace y2017 {
namespace wpilib {
namespace {
+
constexpr double kMaxBringupPower = 12.0;
-} // namespace
// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
// DMA stuff and then removing the * 2.0 in *_translate.
@@ -75,124 +75,125 @@
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(campbell): Update everything below to match sensors on the robot.
+// TODO(brian): Use ::std::max instead once we have C++14 so that can be
+// constexpr.
+template <typename T>
+constexpr T max(T a, T b) {
+ return (a > b) ? a : b;
+}
+template <typename T, typename... Rest>
+constexpr T max(T a, T b, T c, Rest... rest) {
+ return max(max(a, b), c, rest...);
+}
-// TODO(comran): Template these methods since there is a lot of repetition here.
double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
+ return static_cast<double>(in) /
+ Values::kDrivetrainEncoderCountsPerRevolution *
+ Values::kDrivetrainEncoderRatio * 2.0 * M_PI;
}
double drivetrain_velocity_translate(double in) {
- return (1.0 / in) / 256.0 /*cpr*/ *
- constants::Values::kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
+ return (1.0 / in) / Values::kDrivetrainCyclesPerRevolution *
+ Values::kDrivetrainEncoderRatio * 2.0 * M_PI;
}
double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kShooterEncoderRatio * (2 * M_PI /*radians*/);
+ return static_cast<double>(in) / Values::kShooterEncoderCountsPerRevolution *
+ Values::kShooterEncoderRatio * (2 * M_PI /*radians*/);
}
double intake_translate(int32_t in) {
- return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kIntakeEncoderRatio * (2 * M_PI /*radians*/);
+ return static_cast<double>(in) / Values::kIntakeEncoderCountsPerRevolution *
+ Values::kIntakeEncoderRatio * (2 * M_PI /*radians*/);
}
+// TODO(constants): Update the number of turns.
double intake_pot_translate(double voltage) {
- return voltage * constants::Values::kIntakePotRatio *
- (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
-}
-
-double turret_translate(int32_t in) {
- return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kTurretEncoderRatio * (2 * M_PI /*radians*/);
-}
-
-double turret_pot_translate(double voltage) {
- return voltage * constants::Values::kTurretPotRatio *
- (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
-}
-
-double serializer_translate(int32_t in) {
- return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kSerializerEncoderRatio *
+ return voltage * Values::kIntakePotRatio * (10.0 /*turns*/ / 5.0 /*volts*/) *
(2 * M_PI /*radians*/);
}
double hood_translate(int32_t in) {
- return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
- constants::Values::kHoodEncoderRatio * (2 * M_PI /*radians*/);
+ return static_cast<double>(in) / Values::kHoodEncoderCountsPerRevolution *
+ Values::kHoodEncoderRatio * (2 * M_PI /*radians*/);
}
-// TODO(campbell): Update all gear ratios below.
+// TODO(constants): Update the number of turns.
+double hood_pot_translate(double voltage) {
+ return voltage * Values::kHoodPotRatio * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+}
-constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
- 5600.0 /* CIM free speed RPM */ * 14.0 / 48.0 /* 1st reduction */ * 28.0 /
- 50.0 /* 2nd reduction (high gear) */ * 30.0 / 44.0 /* encoder gears */ /
- 60.0 /* seconds per minute */ * 256.0 /* CPR */ * 4 /* edges per cycle */;
+double turret_translate(int32_t in) {
+ return static_cast<double>(in) / Values::kTurretEncoderCountsPerRevolution *
+ Values::kTurretEncoderRatio * (2 * M_PI /*radians*/);
+}
-constexpr double kMaxIntakeEncoderPulsesPerSecond =
- 18700.0 /* 775pro free speed RPM */ * 12.0 /
- 18.0 /* motor to encoder reduction */ / 60.0 /* seconds per minute */ *
- 128.0 /* CPR */ * 4 /* edges per cycle */;
+// TODO(constants): Update the number of turns.
+double turret_pot_translate(double voltage) {
+ return voltage * Values::kTurretPotRatio * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+}
-constexpr double kMaxShooterEncoderPulsesPerSecond =
- 18700.0 /* 775pro free speed RPM */ * 12.0 /
- 18.0 /* motor to encoder reduction */ / 60.0 /* seconds per minute */ *
- 128.0 /* CPR */ * 4 /* edges per cycle */;
+double indexer_translate(int32_t in) {
+ return static_cast<double>(in) /
+ Values::kMaxIndexerEncoderCountsPerRevolution *
+ Values::kIndexerEncoderRatio * (2 * M_PI /*radians*/);
+}
-constexpr double kMaxSerializerEncoderPulsesPerSecond =
- 18700.0 /* 775pro free speed RPM */ * 12.0 /
- 56.0 /* motor to encoder reduction */ / 60.0 /* seconds per minute */ *
- 512.0 /* CPR */ * 4 /* index pulse every quarter cycle */;
-
-double kMaxEncoderPulsesPerSecond =
- ::std::max(kMaxSerializerEncoderPulsesPerSecond,
- ::std::max(kMaxIntakeEncoderPulsesPerSecond,
- ::std::max(kMaxDrivetrainEncoderPulsesPerSecond,
- kMaxShooterEncoderPulsesPerSecond)));
+constexpr double kMaxFastEncoderPulsesPerSecond =
+ max(Values::kMaxDrivetrainEncoderPulsesPerSecond,
+ Values::kMaxShooterEncoderPulsesPerSecond);
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+constexpr double kMaxMediumEncoderPulsesPerSecond =
+ max(Values::kMaxIntakeEncoderPulsesPerSecond,
+ Values::kMaxTurretEncoderPulsesPerSecond,
+ Values::kMaxIndexerEncoderPulsesPerSecond);
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+ "medium encoders are too fast");
+constexpr double kMaxSlowEncoderPulsesPerSecond =
+ Values::kMaxHoodEncoderPulsesPerSecond;
+static_assert(kMaxSlowEncoderPulsesPerSecond <= 100000,
+ "slow encoders are too fast");
// Class to send position messages with sensor readings to our loops.
class SensorReader {
public:
SensorReader() {
- // Set it to filter out anything shorter than 1/4 of the minimum pulse width
+ // Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
- drivetrain_shooter_encoder_filter_.SetPeriodNanoSeconds(
+ fast_encoder_filter_.SetPeriodNanoSeconds(
static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxDrivetrainShooterEncoderPulsesPerSecond * 1e9 +
+ kMaxFastEncoderPulsesPerSecond * 1e9 +
0.5));
- superstructure_encoder_filter_.SetPeriodNanoSeconds(
+ medium_encoder_filter_.SetPeriodNanoSeconds(
static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxSuperstructureEncoderPulsesPerSecond * 1e9 +
+ kMaxMediumEncoderPulsesPerSecond * 1e9 +
0.5));
- hall_filter_.SetPeriodNanoSeconds(100000);
+ slow_encoder_filter_.SetPeriodNanoSeconds(
+ static_cast<int>(1 / 4.0 /* built-in tolerance */ /
+ kMaxSlowEncoderPulsesPerSecond * 1e9 +
+ 0.5));
}
- // Drivetrain setters.
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
drivetrain_left_encoder_ = ::std::move(encoder);
}
void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
drivetrain_right_encoder_ = ::std::move(encoder);
}
- // Shooter setter.
void set_shooter_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
shooter_encoder_ = ::std::move(encoder);
}
- // Intake setters.
void set_intake_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
intake_encoder_.set_encoder(::std::move(encoder));
}
@@ -200,19 +201,19 @@
intake_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ // TODO(brian): This is wrong.
void set_intake_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
intake_encoder_.set_index(::std::move(index));
}
- // Serializer setters.
- void set_serializer_encoder(::std::unique_ptr<Encoder> encoder) {
- serializer_encoder_ = ::std::move(encoder);
+ void set_indexer_encoder(::std::unique_ptr<Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ indexer_encoder_ = ::std::move(encoder);
}
- // Turret setters.
void set_turret_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
turret_encoder_.set_encoder(::std::move(encoder));
}
@@ -220,14 +221,14 @@
turret_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ // TODO(brian): This is wrong.
void set_turret_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
turret_encoder_.set_index(::std::move(index));
}
- // Shooter hood setter.
void set_hood_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ slow_encoder_filter_.Add(encoder.get());
hood_encoder_.set_encoder(::std::move(encoder));
}
@@ -236,21 +237,20 @@
}
void set_hood_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ slow_encoder_filter_.Add(index.get());
hood_encoder_.set_index(::std::move(index));
}
- // Autonomous mode switch setter.
void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
autonomous_modes_.at(i) = ::std::move(sensor);
}
-
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ // TODO(constants): Update this.
dma_synchronizer_->Add(&intake_encoder_);
dma_synchronizer_->Add(&turret_encoder_);
dma_synchronizer_->Add(&hood_encoder_);
@@ -303,24 +303,22 @@
{
auto superstructure_message = superstructure_queue.position.MakeMessage();
- CopyPotAndAbsolutePosition(
- intake_encoder_, &superstructure_message->intake, intake_translate,
- intake_pot_translate, false, values.intake_pot_offset);
+ CopyPosition(intake_encoder_, &superstructure_message->intake,
+ intake_translate, intake_pot_translate, false,
+ values.intake.pot_offset);
- superstructure_message->theta_serializer =
- serializer_translate(serializer_encoder_->GetRaw());
+ superstructure_message->theta_indexer =
+ indexer_translate(indexer_encoder_->GetRaw());
superstructure_message->theta_shooter=
shooter_translate(shooter_encoder_->GetRaw());
- CopyPotAndAbsolutePosition(hood_encoder_, &superstructure_message->hood,
- hood_translate, hood_pot_translate, false,
- values.hood_pot_offset);
+ CopyPosition(hood_encoder_, &superstructure_message->hood, hood_translate,
+ hood_pot_translate, false, values.hood.pot_offset);
- CopyPotAndAbsolutePosition(turret_encoder_,
- &superstructure_message->turret,
- turret_translate, turret_pot_translate, false,
- values.turret_pot_offset);
+ CopyPosition(turret_encoder_, &superstructure_message->turret,
+ turret_translate, turret_pot_translate, false,
+ values.turret.pot_offset);
superstructure_message.Send();
}
@@ -341,12 +339,11 @@
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) {
+ void CopyPosition(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());
@@ -362,8 +359,9 @@
position->index_pulses = encoder.index_posedge_count();
}
+#if 0
// TODO(campbell): Fix this stuff. It is all wrong.
- void CopyPotAndAbsolutePosition(
+ void CopyPosition(
const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
::frc971::PotAndAbsolutePosition *position,
::std::function<double(int32_t)> encoder_translate,
@@ -380,7 +378,7 @@
}
// TODO(campbell): Fix this stuff. It is all wrong.
- void CopyAbsoluteAndIndexPosition(
+ void CopyPosition(
const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
::frc971::EncoderAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate, bool reverse) {
@@ -391,19 +389,23 @@
multiplier * encoder_translate(encoder.last_encoder_value());
position->index_pulses = encoder.index_posedge_count();
}
+#endif
int32_t my_pid_;
DriverStation *ds_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
+ DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
+ slow_encoder_filter_;
+
::std::unique_ptr<Encoder> drivetrain_left_encoder_,
drivetrain_right_encoder_;
::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_;
- ::std::unique_ptr<Encoder> serializer_encoder_;
- ::std::unique_ptr<AnalogInput> serializer_hall_;
+ ::std::unique_ptr<Encoder> indexer_encoder_;
+ ::std::unique_ptr<AnalogInput> indexer_hall_;
::frc971::wpilib::DMAEncoderAndPotentiometer turret_encoder_;
::frc971::wpilib::DMAEncoderAndPotentiometer hood_encoder_;
@@ -412,8 +414,6 @@
::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
::std::atomic<bool> run_{true};
- DigitalGlitchFilter drivetrain_shooter_encoder_filter_,
- superstructure_encoder_filter_, hall_filter_;
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
@@ -456,11 +456,11 @@
intake_rollers_victor_ = ::std::move(t);
}
- void set_serializer_victor(::std::unique_ptr<VictorSP> t) {
- serializer_victor_ = ::std::move(t);
+ void set_indexer_victor(::std::unique_ptr<VictorSP> t) {
+ indexer_victor_ = ::std::move(t);
}
- void set_serializer_roller_victor(::std::unique_ptr<VictorSP> t) {
- serializer_roller_victor_ = ::std::move(t);
+ void set_indexer_roller_victor(::std::unique_ptr<VictorSP> t) {
+ indexer_roller_victor_ = ::std::move(t);
}
void set_shooter_victor(::std::unique_ptr<VictorSP> t) {
@@ -485,8 +485,8 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
intake_rollers_victor_->SetSpeed(queue->voltage_intake_rollers / 12.0);
- serializer_victor_->SetSpeed(queue->voltage_serializer / 12.0);
- serializer_roller_victor_->SetSpeed(queue->voltage_serializer_rollers /
+ indexer_victor_->SetSpeed(queue->voltage_indexer / 12.0);
+ indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers /
12.0);
turret_victor_->SetSpeed(::aos::Clip(queue->voltage_turret,
-kMaxBringupPower, kMaxBringupPower) /
@@ -501,15 +501,15 @@
LOG(WARNING, "Superstructure output too old.\n");
intake_victor_->SetDisabled();
intake_rollers_victor_->SetDisabled();
- serializer_victor_->SetDisabled();
- serializer_roller_victor_->SetDisabled();
+ indexer_victor_->SetDisabled();
+ indexer_roller_victor_->SetDisabled();
turret_victor_->SetDisabled();
hood_victor_->SetDisabled();
shooter_victor_->SetDisabled();
}
::std::unique_ptr<VictorSP> intake_victor_, intake_rollers_victor_,
- serializer_victor_, serializer_roller_victor_, shooter_victor_,
+ indexer_victor_, indexer_roller_victor_, shooter_victor_,
turret_victor_, hood_victor_;
};
@@ -539,8 +539,7 @@
reader.set_intake_index(make_unique<DigitalInput>(0));
reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
- reader.set_serializer_encoder(make_encoder(3));
- reader.set_serializer_hall(make_unique<AnalogInput>(1));
+ reader.set_indexer_encoder(make_encoder(3));
reader.set_turret_encoder(make_encoder(5));
reader.set_turret_index(make_unique<DigitalInput>(1));
@@ -578,9 +577,9 @@
::std::unique_ptr<VictorSP>(new VictorSP(2)));
superstructure_writer.set_intake_rollers_victor(
::std::unique_ptr<VictorSP>(new VictorSP(3)));
- superstructure_writer.set_serializer_victor(
+ superstructure_writer.set_indexer_victor(
::std::unique_ptr<VictorSP>(new VictorSP(4)));
- superstructure_writer.set_serializer_roller_victor(
+ superstructure_writer.set_indexer_roller_victor(
::std::unique_ptr<VictorSP>(new VictorSP(5)));
superstructure_writer.set_turret_victor(
::std::unique_ptr<VictorSP>(new VictorSP(6)));
@@ -623,6 +622,7 @@
}
};
+} // namespace
} // namespace wpilib
} // namespace y2017