Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/constants.cc b/y2015/constants.cc
new file mode 100644
index 0000000..50f6672
--- /dev/null
+++ b/y2015/constants.cc
@@ -0,0 +1,381 @@
+#include "y2015/constants.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/once.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/mutex.h"
+
+#include "y2015/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2015/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace frc971 {
+namespace constants {
+namespace {
+
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+
+// ///// Drivetrain Constants
+
+// These three constants were set by Daniel on 2/13/15.
+const double kDrivetrainEncoderRatio = 20.0 / 50.0;
+const double kLowGearRatio = kDrivetrainEncoderRatio * 20.0 / 50.0;
+const double kHighGearRatio = kLowGearRatio;
+
+const ShifterHallEffect kCompRightDriveShifter{555, 657, 660, 560, 0.2, 0.7};
+const ShifterHallEffect kCompLeftDriveShifter{555, 660, 644, 552, 0.2, 0.7};
+
+const ShifterHallEffect kPracticeRightDriveShifter{2.95, 3.95, 3.95,
+ 2.95, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{2.95, 4.2, 3.95,
+ 3.0, 0.2, 0.7};
+const double kToteHeight = 0.3;
+
+// Set by Daniel on 2/13/15.
+// Distance from the center of the left wheel to the center of the right wheel.
+const double kRobotWidth = 37.806 /*inches*/ * 0.0254;
+
+// ///// Superstructure Constants
+
+// Elevator gearbox pulley output constants.
+const int kElevatorGearboxOutputPulleyTeeth = 32; // 32 teeth
+const double kElevatorGearboxOutputPitch = 0.005; // 5 mm/tooth
+const double kElevatorGearboxOutputRadianDistance =
+ kElevatorGearboxOutputPulleyTeeth * kElevatorGearboxOutputPitch /
+ (2.0 * M_PI);
+
+const double kArmZeroingHeight = 0.2;
+
+const double kMaxAllowedLeftRightArmDifference = 0.12; // radians
+const double kMaxAllowedLeftRightElevatorDifference = 0.04; // meters
+
+const Values::ClawGeometry kClawGeometry{
+ // Horizontal distance from the center of the grabber to the end.
+ 0.5 * 18.0 * 0.0254,
+ // Vertical distance from the arm rotation center to the bottom of
+ // the
+ // grabber. Distance measured with arm vertical (theta = 0).
+ 5.1 * 0.0254,
+ // Vertical separation of the claw and arm rotation centers with the
+ // elevator at 0.0 and the arm angle set to zero.
+ 10.5 * 0.0254,
+ // Horizontal separation of the claw and arm rotation centers with
+ // the
+ // elevator at 0.0 and the arm angle set to zero.
+ 6.5 * 0.0254,
+ // Distance between the center of the claw to the top of the claw.
+ // 2.75 inches would work most of the time. Using 3.5 inches because
+ // of the
+ // pnumatics fitting on the piston.
+ 3.5 * 0.0254,
+ // The grabber is safe at any height if it is behind this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ (18.0 - 0.3) * 0.0254,
+ // The grabber is safe at any x if it is above this location.
+ // The location of the grabber is used here and not the location of
+ // the end
+ // of the grabber. The grabber location is (0, 0) when the elevator
+ // is at 0
+ // and the arm angle is 0.
+ // The "-5.4" is the location of the bottom of the grabber when
+ // the elevator is at the bottom (-0.3 inches) and arm angle is 0.
+ -8.0 * 0.0254,
+};
+
+// Gearing ratios of the pots and encoders for the elevator and arm.
+// Ratio is output shaft rotations per encoder/pot rotation
+// Checked by Daniel on 2/13/15.
+const double kArmEncoderRatio = 18.0 / 48.0 * 16.0 / 72.0;
+const double kArmPotRatio = 48.0 / 48.0 * 16.0 / 72.0;
+const double kElevatorEncoderRatio = 14.0 / 84.0;
+const double kElevatorPotRatio = 1.0;
+const double kClawEncoderRatio = 18.0 / 72.0;
+const double kClawPotRatio = 18.0 / 72.0;
+
+// Number of radians between each index pulse on the arm.
+const double kArmEncoderIndexDifference = 2.0 * M_PI * kArmEncoderRatio;
+// Number of meters between each index pulse on the elevator.
+const double kElevatorEncoderIndexDifference =
+ kElevatorEncoderRatio * 2.0 * M_PI * // radians
+ kElevatorGearboxOutputRadianDistance;
+// Number of radians between index pulses on the claw.
+const double kClawEncoderIndexDifference = 2.0 * M_PI * kClawEncoderRatio;
+
+const int kZeroingSampleSize = 200;
+
+// The length of the arm.
+const double kArmLength = 0.7366;
+
+// TODO(danielp): All these values might need to change.
+const double kClawPistonSwitchTime = 0.4;
+const double kClawZeroingRange = 0.3;
+
+const Values *DoGetValuesForTeam(uint16_t team) {
+ switch (team) {
+ case 1: // for tests
+ return new Values{
+ kDrivetrainEncoderRatio,
+ kArmEncoderRatio,
+ kArmPotRatio,
+ kElevatorEncoderRatio,
+ kElevatorPotRatio,
+ kElevatorGearboxOutputRadianDistance,
+ kClawEncoderRatio,
+ kClawPotRatio,
+ kToteHeight,
+ kLowGearRatio,
+ kHighGearRatio,
+ kCompLeftDriveShifter,
+ kCompRightDriveShifter,
+ false,
+ 0.5,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
+ 0.02, // drivetrain done delta
+ 5.0, // drivetrain max speed
+
+ // Motion ranges: hard_lower_limit, hard_upper_limit,
+ // soft_lower_limit, soft_upper_limit
+ {// Claw values, in radians.
+ // 0 is level with the ground.
+ // Positive moves in the direction of positive encoder values.
+ {-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
+
+ // Zeroing constants for wrist.
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.9, 0.3},
+
+ 6.308141,
+ kClawPistonSwitchTime,
+ kClawZeroingRange},
+
+ {// Elevator values, in meters.
+ // 0 is the portion of the elevator carriage that Spencer removed
+ // lining up with the bolt.
+ // Positive is up.
+ {-0.005, 0.689000, 0.010000, 0.680000},
+
+ // Arm values, in radians.
+ // 0 is sticking straight out horizontally over the intake/front.
+ // Positive is rotating up and into the robot (towards the back).
+ {-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
+
+ // Elevator zeroing constants: left, right.
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0, 0.3},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0, 0.3},
+ // Arm zeroing constants: left, right.
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0, 0.3},
+ 0.7,
+ -0.08,
+ -3.5 - 0.01 - -0.02,
+ 3.5 - 0.17 - -0.15,
+
+ kArmZeroingHeight,
+ kArmLength,
+ },
+ // End "sensor" values.
+
+ kMaxAllowedLeftRightArmDifference,
+ kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
+ };
+ break;
+ case kPracticeTeamNumber:
+ return new Values{
+ kDrivetrainEncoderRatio,
+ kArmEncoderRatio,
+ kArmPotRatio,
+ kElevatorEncoderRatio,
+ kElevatorPotRatio,
+ kElevatorGearboxOutputRadianDistance,
+ kClawEncoderRatio,
+ kClawPotRatio,
+ kToteHeight,
+ kLowGearRatio,
+ kHighGearRatio,
+ kCompLeftDriveShifter,
+ kCompRightDriveShifter,
+ false,
+ kRobotWidth,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
+ 0.02, // drivetrain done delta
+ 5.0, // drivetrain max speed
+
+ // Motion ranges: hard_lower_limit, hard_upper_limit,
+ // soft_lower_limit, soft_upper_limit
+ // TODO(sensors): Get actual bounds before turning on robot.
+ {// Claw values, in radians.
+ // 0 is level with the ground.
+ // Positive moves in the direction of positive encoder values.
+ {-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
+
+ // Zeroing constants for wrist.
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.9104180000000001,
+ 0.3},
+
+ 6.308141,
+ kClawPistonSwitchTime,
+ kClawZeroingRange},
+
+ {// Elevator values, in meters.
+ // 0 is the portion of the elevator carriage that Spencer removed
+ // lining up with the bolt.
+ // Positive is up.
+ {-0.00500, 0.689000, 0.010000, 0.680000},
+
+ // Arm values, in radians.
+ // 0 is sticking straight out horizontally over the intake/front.
+ // Positive is rotating up and into the robot (towards the back).
+ {-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
+
+ // Elevator zeroing constants: left, right.
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.110677, 0.3}, // Was 0.088984 (3 mm too high)
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.109974, 0.3}, // Was 0.104557 (4 mm too low)
+ // Arm zeroing constants: left, right.
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.324437, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.064683, 0.3},
+ 0.722230 - -0.000594 - -0.026183 - 0.003442, // Left Elevator Poteniometer adjustment
+ -0.081354 - -0.000374 - -0.024793 - -0.006916, // Right Elevator Poteniometer adjustment
+ -3.509611 - 0.007415 - -0.019081,
+ 3.506927 - 0.170017 - -0.147970,
+
+ kArmZeroingHeight,
+ kArmLength,
+ },
+ // End "sensor" values.
+
+ kMaxAllowedLeftRightArmDifference,
+ kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
+ };
+ break;
+ case kCompTeamNumber:
+ return new Values{
+ kDrivetrainEncoderRatio,
+ kArmEncoderRatio,
+ kArmPotRatio,
+ kElevatorEncoderRatio,
+ kElevatorPotRatio,
+ kElevatorGearboxOutputRadianDistance,
+ kClawEncoderRatio,
+ kClawPotRatio,
+ kToteHeight,
+ kLowGearRatio,
+ kHighGearRatio,
+ kPracticeLeftDriveShifter,
+ kPracticeRightDriveShifter,
+ false,
+ kRobotWidth,
+ control_loops::MakeVelocityDrivetrainLoop,
+ control_loops::MakeDrivetrainLoop,
+ 0.02, // drivetrain done delta
+ 5.0, // drivetrain max speed
+
+ // Motion ranges: hard_lower_limit, hard_upper_limit,
+ // soft_lower_limit, soft_upper_limit
+ // TODO(sensors): Get actual bounds before turning on robot.
+ {// Claw values, in radians.
+ // 0 is level with the ground.
+ // Positive moves in the direction of positive encoder values.
+ {-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
+
+ // Zeroing constants for wrist.
+ // TODO(sensors): Get actual offsets for these.
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.952602, 0.3},
+ 6.1663463999999992 + 0.015241,
+
+ kClawPistonSwitchTime,
+ kClawZeroingRange},
+
+ {// Elevator values, in meters.
+ // 0 is at the top of the elevator frame.
+ // Positive is down towards the drivebase.
+ {-0.00500, 0.689000, 0.010000, 0.680000},
+
+ // Arm values, in radians.
+ // 0 is sticking straight out horizontally over the intake/front.
+ // Positive is rotating up and into the robot (towards the back).
+ {-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
+
+ // Elevator zeroing constants: left, right.
+ // These are the encoder offsets.
+ // TODO(sensors): Get actual offsets for these.
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference,
+ 0.240286 - 0.007969 - 0.025, 0.3},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference,
+ 0.234583 - 0.000143, 0.3},
+ // Arm zeroing constants: left, right.
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.060592, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.210155, 0.3},
+ // These are the potentiometer offsets.
+ 0.72069366666666679 - 0.026008 - 0.024948 + 0.025,
+ -0.078959636363636357 - 0.024646 - 0.020260,
+ -3.509611 - 0.007415 - -0.019081 - 0.029393 - -0.013585,
+ 3.506927 - 0.170017 - -0.147970 - 0.005045 - -0.026504,
+
+ kArmZeroingHeight,
+ kArmLength,
+ },
+ // TODO(sensors): End "sensor" values.
+
+ kMaxAllowedLeftRightArmDifference,
+ kMaxAllowedLeftRightElevatorDifference,
+ kClawGeometry,
+ };
+ break;
+ default:
+ LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ }
+}
+
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ return DoGetValuesForTeam(team);
+}
+
+} // namespace
+
+const Values &GetValues() {
+ static ::aos::Once<const Values> once(DoGetValues);
+ return *once.Get();
+}
+
+const Values &GetValuesForTeam(uint16_t team_number) {
+ static ::aos::Mutex mutex;
+ ::aos::MutexLocker locker(&mutex);
+
+ // IMPORTANT: This declaration has to stay after the mutex is locked to avoid
+ // race conditions.
+ static ::std::map<uint16_t, const Values *> values;
+
+ if (values.count(team_number) == 0) {
+ values[team_number] = DoGetValuesForTeam(team_number);
+#if __has_feature(address_sanitizer)
+ __lsan_ignore_object(values[team_number]);
+#endif
+ }
+ return *values[team_number];
+}
+
+} // namespace constants
+} // namespace frc971