Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 1 | #include "y2020/constants.h" |
| 2 | |
| 3 | #include <inttypes.h> |
| 4 | |
| 5 | #include <map> |
| 6 | |
| 7 | #if __has_feature(address_sanitizer) |
| 8 | #include "sanitizer/lsan_interface.h" |
| 9 | #endif |
| 10 | |
| 11 | #include "absl/base/call_once.h" |
| 12 | #include "aos/logging/logging.h" |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 13 | #include "aos/network/team_number.h" |
Brian Silverman | 1463c09 | 2020-10-30 17:28:24 -0700 | [diff] [blame] | 14 | #include "aos/stl_mutex/stl_mutex.h" |
ravago | 901c426 | 2020-02-16 15:33:14 -0800 | [diff] [blame] | 15 | #include "y2020/control_loops/superstructure/control_panel/integral_control_panel_plant.h" |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 16 | #include "y2020/control_loops/superstructure/hood/integral_hood_plant.h" |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 17 | #include "y2020/control_loops/superstructure/intake/integral_intake_plant.h" |
| 18 | #include "y2020/control_loops/superstructure/turret/integral_turret_plant.h" |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 19 | |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 20 | namespace y2020 { |
| 21 | namespace constants { |
| 22 | |
| 23 | const int Values::kZeroingSampleSize; |
| 24 | |
| 25 | namespace { |
| 26 | |
| 27 | const uint16_t kCompTeamNumber = 971; |
| 28 | const uint16_t kPracticeTeamNumber = 9971; |
James Kuszmaul | 6535d56 | 2020-05-09 16:58:13 -0700 | [diff] [blame] | 29 | const uint16_t kSpareRoborioTeamNumber = 6971; |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 30 | |
| 31 | const Values *DoGetValuesForTeam(uint16_t team) { |
| 32 | Values *const r = new Values(); |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 33 | ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams< |
Ravago Jones | 937587c | 2020-12-26 17:21:09 -0800 | [diff] [blame^] | 34 | ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator> |
| 35 | *const hood = &r->hood; |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 36 | |
| 37 | // Hood constants. |
| 38 | hood->zeroing_voltage = 3.0; |
| 39 | hood->operating_voltage = 12.0; |
| 40 | hood->zeroing_profile_params = {0.5, 3.0}; |
| 41 | hood->default_profile_params = {6.0, 30.0}; |
| 42 | hood->range = Values::kHoodRange(); |
| 43 | hood->make_integral_loop = |
| 44 | control_loops::superstructure::hood::MakeIntegralHoodLoop; |
| 45 | hood->zeroing_constants.average_filter_size = Values::kZeroingSampleSize; |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 46 | hood->zeroing_constants.zeroing_threshold = 0.0005; |
| 47 | hood->zeroing_constants.moving_buffer_size = 20; |
| 48 | hood->zeroing_constants.allowable_encoder_error = 0.9; |
Ravago Jones | 937587c | 2020-12-26 17:21:09 -0800 | [diff] [blame^] | 49 | hood->zeroing_constants.one_revolution_distance = |
| 50 | M_PI * 2.0 * constants::Values::kHoodEncoderRatio(); |
| 51 | hood->zeroing_constants.single_turn_middle_position = |
| 52 | Values::kHoodRange().middle(); |
| 53 | hood->zeroing_constants.single_turn_one_revolution_distance = |
| 54 | M_PI * 2.0 * constants::Values::kHoodSingleTurnEncoderRatio(); |
| 55 | hood->zeroing_constants.measured_absolute_position = 0; |
| 56 | hood->zeroing_constants.single_turn_measured_absolute_position = 0; |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 57 | |
Sabina Davis | e8d3899 | 2020-02-02 15:00:31 -0800 | [diff] [blame] | 58 | ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams< |
| 59 | ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake = |
| 60 | &r->intake; |
| 61 | |
| 62 | // Intake constants. |
| 63 | intake->zeroing_voltage = 3.0; |
| 64 | intake->operating_voltage = 12.0; |
| 65 | intake->zeroing_profile_params = {0.5, 3.0}; |
| 66 | intake->default_profile_params = {6.0, 30.0}; |
| 67 | intake->range = Values::kIntakeRange(); |
| 68 | intake->make_integral_loop = |
| 69 | control_loops::superstructure::intake::MakeIntegralIntakeLoop; |
| 70 | intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize; |
| 71 | intake->zeroing_constants.one_revolution_distance = |
| 72 | M_PI * 2.0 * constants::Values::kIntakeEncoderRatio(); |
| 73 | intake->zeroing_constants.zeroing_threshold = 0.0005; |
| 74 | intake->zeroing_constants.moving_buffer_size = 20; |
| 75 | intake->zeroing_constants.allowable_encoder_error = 0.9; |
| 76 | intake->zeroing_constants.middle_position = Values::kIntakeRange().middle(); |
| 77 | |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 78 | Values::PotAndAbsEncoderConstants *const turret = &r->turret; |
| 79 | ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams< |
| 80 | ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator> |
| 81 | *const turret_params = &turret->subsystem_params; |
| 82 | |
ravago | 901c426 | 2020-02-16 15:33:14 -0800 | [diff] [blame] | 83 | // Turret Constants |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 84 | turret_params->zeroing_voltage = 4.0; |
James Kuszmaul | 519585d | 2020-03-08 22:32:48 -0700 | [diff] [blame] | 85 | turret_params->operating_voltage = 8.0; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 86 | // TODO(austin): Tune these. |
| 87 | turret_params->zeroing_profile_params = {0.5, 2.0}; |
| 88 | turret_params->default_profile_params = {15.0, 40.0}; |
| 89 | turret_params->range = Values::kTurretRange(); |
| 90 | turret_params->make_integral_loop = |
| 91 | &control_loops::superstructure::turret::MakeIntegralTurretLoop; |
| 92 | turret_params->zeroing_constants.average_filter_size = |
| 93 | Values::kZeroingSampleSize; |
| 94 | turret_params->zeroing_constants.one_revolution_distance = |
| 95 | M_PI * 2.0 * constants::Values::kTurretEncoderRatio(); |
| 96 | turret_params->zeroing_constants.zeroing_threshold = 0.0005; |
| 97 | turret_params->zeroing_constants.moving_buffer_size = 20; |
| 98 | turret_params->zeroing_constants.allowable_encoder_error = 0.9; |
| 99 | |
Austin Schuh | 9dcd520 | 2020-02-20 20:06:04 -0800 | [diff] [blame] | 100 | CHECK_LE(hood->range.range(), |
Ravago Jones | 937587c | 2020-12-26 17:21:09 -0800 | [diff] [blame^] | 101 | hood->zeroing_constants.single_turn_one_revolution_distance); |
Austin Schuh | 9dcd520 | 2020-02-20 20:06:04 -0800 | [diff] [blame] | 102 | CHECK_LE(intake->range.range(), |
| 103 | intake->zeroing_constants.one_revolution_distance); |
| 104 | |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 105 | switch (team) { |
| 106 | // A set of constants for tests. |
| 107 | case 1: |
James Kuszmaul | 6535d56 | 2020-05-09 16:58:13 -0700 | [diff] [blame] | 108 | case kSpareRoborioTeamNumber: |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 109 | break; |
| 110 | |
| 111 | case kCompTeamNumber: |
Austin Schuh | 78f0bfd | 2020-02-29 23:04:21 -0800 | [diff] [blame] | 112 | hood->zeroing_constants.measured_absolute_position = 0.266691815725476; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 113 | |
Sabina Davis | f7afd11 | 2020-02-23 13:42:14 -0800 | [diff] [blame] | 114 | intake->zeroing_constants.measured_absolute_position = |
| 115 | 1.42977866919024 - Values::kIntakeZero(); |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 116 | |
Austin Schuh | 2fb2364 | 2020-02-29 15:10:51 -0800 | [diff] [blame] | 117 | turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 + |
| 118 | 0.0109413725126625 - 0.223719825811759; |
Sabina Davis | f7afd11 | 2020-02-23 13:42:14 -0800 | [diff] [blame] | 119 | turret_params->zeroing_constants.measured_absolute_position = |
Austin Schuh | 2fb2364 | 2020-02-29 15:10:51 -0800 | [diff] [blame] | 120 | 0.547478339799516; |
Ravago Jones | 937587c | 2020-12-26 17:21:09 -0800 | [diff] [blame^] | 121 | |
| 122 | hood->zeroing_constants.measured_absolute_position = 0.03207; |
| 123 | hood->zeroing_constants.single_turn_measured_absolute_position = 0.31055; |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 124 | break; |
| 125 | |
| 126 | case kPracticeTeamNumber: |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 127 | hood->zeroing_constants.measured_absolute_position = 0.0; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 128 | |
Sabina Davis | e8d3899 | 2020-02-02 15:00:31 -0800 | [diff] [blame] | 129 | intake->zeroing_constants.measured_absolute_position = 0.0; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 130 | |
| 131 | turret->potentiometer_offset = 0.0; |
| 132 | turret_params->zeroing_constants.measured_absolute_position = 0.0; |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 133 | break; |
| 134 | |
Austin Schuh | 83873c3 | 2020-02-22 14:58:39 -0800 | [diff] [blame] | 135 | case Values::kCodingRobotTeamNumber: |
Sabina Davis | a587fbd | 2020-01-31 22:11:15 -0800 | [diff] [blame] | 136 | hood->zeroing_constants.measured_absolute_position = 0.0; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 137 | |
Sabina Davis | e8d3899 | 2020-02-02 15:00:31 -0800 | [diff] [blame] | 138 | intake->zeroing_constants.measured_absolute_position = 0.0; |
Kai Tinkess | 10943cf | 2020-02-01 15:49:57 -0800 | [diff] [blame] | 139 | |
| 140 | turret->potentiometer_offset = 0.0; |
| 141 | turret_params->zeroing_constants.measured_absolute_position = 0.0; |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 142 | break; |
| 143 | |
| 144 | default: |
| 145 | AOS_LOG(FATAL, "unknown team #%" PRIu16 "\n", team); |
| 146 | } |
| 147 | |
| 148 | return r; |
| 149 | } |
| 150 | |
| 151 | void DoGetValues(const Values **result) { |
| 152 | uint16_t team = ::aos::network::GetTeamNumber(); |
| 153 | AOS_LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team); |
| 154 | *result = DoGetValuesForTeam(team); |
| 155 | } |
| 156 | |
| 157 | } // namespace |
| 158 | |
| 159 | const Values &GetValues() { |
| 160 | static absl::once_flag once; |
| 161 | static const Values *result; |
| 162 | absl::call_once(once, DoGetValues, &result); |
| 163 | return *result; |
| 164 | } |
| 165 | |
| 166 | const Values &GetValuesForTeam(uint16_t team_number) { |
Brian Silverman | 1463c09 | 2020-10-30 17:28:24 -0700 | [diff] [blame] | 167 | static aos::stl_mutex mutex; |
| 168 | std::unique_lock<aos::stl_mutex> locker(mutex); |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 169 | |
ravago | 901c426 | 2020-02-16 15:33:14 -0800 | [diff] [blame] | 170 | // IMPORTANT: This declaration has to stay after the mutex is locked to |
| 171 | // avoid race conditions. |
Stephan Massalt | d021f97 | 2020-01-05 20:41:23 -0800 | [diff] [blame] | 172 | static ::std::map<uint16_t, const Values *> values; |
| 173 | |
| 174 | if (values.count(team_number) == 0) { |
| 175 | values[team_number] = DoGetValuesForTeam(team_number); |
| 176 | #if __has_feature(address_sanitizer) |
| 177 | __lsan_ignore_object(values[team_number]); |
| 178 | #endif |
| 179 | } |
| 180 | return *values[team_number]; |
| 181 | } |
| 182 | |
| 183 | } // namespace constants |
| 184 | } // namespace y2020 |