882 auto driveName = drive->
GetName();
884 module->SetupTelemetry(driveName);
887 auto inst = wpi::nt::NetworkTableInstance::GetDefault();
888 m_dataTable = inst.GetTable(
"Mechanisms")->GetSubTable(driveName);
889 m_tuningTable = inst.GetTable(
"Tuning")->GetSubTable(driveName);
891 bool nt4Enabled = m_config.GetNT4Enabled();
892 auto dataLogName = m_config.GetDataLogName();
898 auto& doubleFields = m_config.GetDoubleFields();
899 if (translationPID) {
900 doubleFields.at(DoubleTelemetryField::TranslationP)
901 .SetDefaultValue(translationPID->get().GetP());
902 doubleFields.at(DoubleTelemetryField::TranslationI)
903 .SetDefaultValue(translationPID->get().GetI());
904 doubleFields.at(DoubleTelemetryField::TranslationD)
905 .SetDefaultValue(translationPID->get().GetD());
907 doubleFields.at(DoubleTelemetryField::TranslationP).Disable();
908 doubleFields.at(DoubleTelemetryField::TranslationI).Disable();
909 doubleFields.at(DoubleTelemetryField::TranslationD).Disable();
912 doubleFields.at(DoubleTelemetryField::RotationP).SetDefaultValue(rotationPID->get().GetP());
913 doubleFields.at(DoubleTelemetryField::RotationI).SetDefaultValue(rotationPID->get().GetI());
914 doubleFields.at(DoubleTelemetryField::RotationD).SetDefaultValue(rotationPID->get().GetD());
916 doubleFields.at(DoubleTelemetryField::RotationP).Disable();
917 doubleFields.at(DoubleTelemetryField::RotationI).Disable();
918 doubleFields.at(DoubleTelemetryField::RotationD).Disable();
921 if (!translationPID || !rotationPID) {
922 doubleFields.at(DoubleTelemetryField::AutoAlignPoseX).Disable();
923 doubleFields.at(DoubleTelemetryField::AutoAlignPoseY).Disable();
924 doubleFields.at(DoubleTelemetryField::AutoAlignPoseRotation).Disable();
925 m_config.GetBoolFields().at(BooleanTelemetryField::AutoAlignEnabled).Disable();
929 if (!modules.empty()) {
930 auto* driveMotor = modules[0]->GetDriveMotorController();
932 driveMotor->GetConfig().GetSlotGains(driveMotor->GetClosedLoopControllerSlot());
933 m_config.GetDoubleFields()
934 .at(DoubleTelemetryField::ModulesDriveP)
935 .SetDefaultValue(driveGains.kP);
936 m_config.GetDoubleFields()
937 .at(DoubleTelemetryField::ModulesDriveI)
938 .SetDefaultValue(driveGains.kI);
939 m_config.GetDoubleFields()
940 .at(DoubleTelemetryField::ModulesDriveD)
941 .SetDefaultValue(driveGains.kD);
942 m_config.GetDoubleFields()
943 .at(DoubleTelemetryField::ModulesDriveKs)
944 .SetDefaultValue(driveGains.kS);
945 m_config.GetDoubleFields()
946 .at(DoubleTelemetryField::ModulesDriveKv)
947 .SetDefaultValue(driveGains.kV);
948 m_config.GetDoubleFields()
949 .at(DoubleTelemetryField::ModulesDriveKa)
950 .SetDefaultValue(driveGains.kA);
952 auto* azimuthMotor = modules[0]->GetAzimuthMotorController();
954 azimuthMotor->GetConfig().GetSlotGains(azimuthMotor->GetClosedLoopControllerSlot());
955 m_config.GetDoubleFields()
956 .at(DoubleTelemetryField::ModulesAzimuthP)
957 .SetDefaultValue(azimuthGains.kP);
958 m_config.GetDoubleFields()
959 .at(DoubleTelemetryField::ModulesAzimuthI)
960 .SetDefaultValue(azimuthGains.kI);
961 m_config.GetDoubleFields()
962 .at(DoubleTelemetryField::ModulesAzimuthD)
963 .SetDefaultValue(azimuthGains.kD);
964 m_config.GetDoubleFields()
965 .at(DoubleTelemetryField::ModulesAzimuthKs)
966 .SetDefaultValue(azimuthGains.kS);
967 m_config.GetDoubleFields()
968 .at(DoubleTelemetryField::ModulesAzimuthKv)
969 .SetDefaultValue(azimuthGains.kV);
970 m_config.GetDoubleFields()
971 .at(DoubleTelemetryField::ModulesAzimuthKa)
972 .SetDefaultValue(azimuthGains.kA);
976 for (
auto& [field, dt] : m_config.GetDoubleFields()) {
977 if (!dt.IsEnabled())
continue;
978 if (nt4Enabled) dt.SetupNetworkTables(m_dataTable, m_tuningTable);
979 if (dataLogName) dt.SetupDataLog(*dataLogName);
981 for (
auto& [field, bt] : m_config.GetBoolFields()) {
982 if (!bt.IsEnabled())
continue;
983 if (nt4Enabled) bt.SetupNetworkTables(m_dataTable, m_tuningTable);
984 if (dataLogName) bt.SetupDataLog(*dataLogName);
986 for (
auto& [field, stt] : m_config.GetPoseFields()) {
987 if (!stt.IsEnabled())
continue;
988 if (nt4Enabled) stt.SetupNetworkTables(m_dataTable, m_tuningTable);
989 if (dataLogName) stt.SetupDataLog(*dataLogName);
991 for (
auto& [field, stt] : m_config.GetChassisSpeedsFields()) {
992 if (!stt.IsEnabled())
continue;
993 if (nt4Enabled) stt.SetupNetworkTables(m_dataTable, m_tuningTable);
994 if (dataLogName) stt.SetupDataLog(*dataLogName);
996 for (
auto& [field, stat] : m_config.GetModuleStatesFields()) {
997 if (!stat.IsEnabled())
continue;
998 if (nt4Enabled) stat.SetupNetworkTables(m_dataTable, m_tuningTable);
999 if (dataLogName) stat.SetupDataLog(*dataLogName);
1062 if (!m_config.GetNT4Enabled())
return;
1064 auto& boolFields = m_config.GetBoolFields();
1065 auto& doubleFields = m_config.GetDoubleFields();
1067 std::optional<wpi::math::PIDController> translationPID;
1069 std::optional<wpi::math::PIDController> rotationPID;
1077 std::set<TuningMode> modes;
1078 for (
auto& [field, bt] : boolFields) {
1079 if (!bt.IsEnabled() || !bt.Get())
continue;
1081 case BooleanTelemetryField::AutoAlignEnabled:
1082 modes.insert(TuningMode::AutoAlign);
1083 modes.erase(TuningMode::Drive);
1084 modes.erase(TuningMode::Azimuth);
1085 boolFields.at(BooleanTelemetryField::ModulesDriveTuningEnabled).ForceSet(
false);
1086 boolFields.at(BooleanTelemetryField::ModulesAzimuthTuningEnabled).ForceSet(
false);
1088 case BooleanTelemetryField::ModulesDriveTuningEnabled:
1089 if (!modes.contains(TuningMode::AutoAlign)) {
1090 modes.insert(TuningMode::Drive);
1091 modes.erase(TuningMode::Azimuth);
1092 boolFields.at(BooleanTelemetryField::ModulesAzimuthTuningEnabled).ForceSet(
false);
1095 case BooleanTelemetryField::ModulesDriveInPlace:
1096 modes.insert(TuningMode::DriveInPlace);
1098 case BooleanTelemetryField::ModulesDriveVoltageEnabled:
1099 modes.insert(TuningMode::DriveVoltage);
1101 case BooleanTelemetryField::ModulesAzimuthTuningEnabled:
1102 if (!modes.contains(TuningMode::AutoAlign) && !modes.contains(TuningMode::Drive)) {
1103 modes.insert(TuningMode::Azimuth);
1106 case BooleanTelemetryField::ModulesAzimuthVoltageEnabled:
1107 modes.insert(TuningMode::AzimuthVoltage);
1112 if (!modes.contains(TuningMode::Drive)) {
1113 modes.erase(TuningMode::DriveInPlace);
1114 modes.erase(TuningMode::DriveVoltage);
1116 if (!modes.contains(TuningMode::Azimuth)) modes.erase(TuningMode::AzimuthVoltage);
1120 wpi::util::array<wpi::math::Rotation2d, NumModules> driveAngles{wpi::util::empty_array};
1121 for (
size_t i = 0; i < NumModules; ++i) {
1122 driveAngles[i] = wpi::math::Rotation2d{};
1123 if (modes.contains(TuningMode::DriveInPlace)) {
1124 auto moduleLocation = *modules[i]->GetConfig().GetLocation();
1125 driveAngles[i] = moduleLocation.Angle().value_or(wpi::math::Rotation2d{}) +
1126 wpi::math::Rotation2d{wpi::units::degree_t{90}};
1131 if (modes.contains(TuningMode::AutoAlign)) {
1132 wpi::math::Pose2d targetPose{
1133 wpi::units::meter_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseX).Get()},
1134 wpi::units::meter_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseY).Get()},
1135 wpi::math::Rotation2d{
1136 wpi::units::degree_t{doubleFields.at(DoubleTelemetryField::AutoAlignPoseRotation).Get()}}};
1142 if (modes != m_lastModes) {
1143 const std::set<TuningMode> lastModes = m_lastModes.value_or(std::set<TuningMode>{});
1144 m_lastModes = modes;
1146 for (
auto* module : modules) {
1147 if (lastModes.contains(TuningMode::DriveVoltage) && !modes.contains(TuningMode::DriveVoltage)) {
1148 module->GetDriveMotorController()->StartClosedLoopController();
1150 if (lastModes.contains(TuningMode::AzimuthVoltage) &&
1151 !modes.contains(TuningMode::AzimuthVoltage)) {
1152 module->GetAzimuthMotorController()->StartClosedLoopController();
1155 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states{wpi::util::empty_array};
1156 if (modes.empty()) {
1159 for (
size_t i = 0; i < NumModules; ++i) {
1160 states[i] = wpi::math::SwerveModuleVelocity{wpi::units::meters_per_second_t{0},
1161 modules[i]->GetState().angle};
1165 for (
auto mode : modes) {
1167 case TuningMode::AutoAlign:
1170 case TuningMode::DriveInPlace:
1173 case TuningMode::Drive:
1174 if (!modes.contains(TuningMode::DriveVoltage)) {
1175 for (
size_t i = 0; i < NumModules; ++i) {
1176 states[i] = wpi::math::SwerveModuleVelocity{
1177 wpi::units::meters_per_second_t{
1178 doubleFields.at(DoubleTelemetryField::ModulesDriveVelocity).Get()},
1184 case TuningMode::DriveVoltage:
1185 for (
size_t i = 0; i < NumModules; ++i) {
1187 if (!lastModes.contains(TuningMode::DriveVoltage)) {
1188 modules[i]->GetDriveMotorController()->StopClosedLoopController();
1191 modules[i]->GetDriveMotorController()->SetVoltage(
1192 wpi::units::volt_t{doubleFields.at(DoubleTelemetryField::ModulesDriveVoltage).Get()});
1193 modules[i]->GetAzimuthMotorController()->SetPosition(
1194 wpi::units::degree_t{driveAngles[i].Degrees()});
1197 case TuningMode::Azimuth:
1198 if (!modes.contains(TuningMode::AzimuthVoltage)) {
1202 doubleFields.at(DoubleTelemetryField::ModulesAzimuthAngle).Get()}});
1205 case TuningMode::AzimuthVoltage:
1206 for (
auto* module : modules) {
1208 if (!lastModes.contains(TuningMode::AzimuthVoltage)) {
1209 module->GetAzimuthMotorController()->StopClosedLoopController();
1212 module->GetDriveMotorController()->SetVelocity(wpi::units::meters_per_second_t{0} +
1213 module->GetCouplingVelocity());
1214 module->GetAzimuthMotorController()->SetVoltage(wpi::units::volt_t{
1215 doubleFields.at(DoubleTelemetryField::ModulesAzimuthVoltage).Get()});
1223 for (
auto field : {DoubleTelemetryField::TranslationP,
1224 DoubleTelemetryField::TranslationI,
1225 DoubleTelemetryField::TranslationD,
1226 DoubleTelemetryField::RotationP,
1227 DoubleTelemetryField::RotationI,
1228 DoubleTelemetryField::RotationD,
1229 DoubleTelemetryField::AutoAlignPoseX,
1230 DoubleTelemetryField::AutoAlignPoseY,
1231 DoubleTelemetryField::AutoAlignPoseRotation,
1232 DoubleTelemetryField::ModulesDriveP,
1233 DoubleTelemetryField::ModulesDriveI,
1234 DoubleTelemetryField::ModulesDriveD,
1235 DoubleTelemetryField::ModulesDriveKs,
1236 DoubleTelemetryField::ModulesDriveKv,
1237 DoubleTelemetryField::ModulesDriveKa,
1238 DoubleTelemetryField::ModulesDriveVelocity,
1239 DoubleTelemetryField::ModulesDriveVoltage,
1240 DoubleTelemetryField::ModulesAzimuthP,
1241 DoubleTelemetryField::ModulesAzimuthI,
1242 DoubleTelemetryField::ModulesAzimuthD,
1243 DoubleTelemetryField::ModulesAzimuthKs,
1244 DoubleTelemetryField::ModulesAzimuthKv,
1245 DoubleTelemetryField::ModulesAzimuthKa,
1246 DoubleTelemetryField::ModulesAzimuthAngle,
1247 DoubleTelemetryField::ModulesAzimuthVoltage}) {
1248 auto& dt = doubleFields.at(field);
1249 if (!dt.IsEnabled() || !dt.IsTunable())
continue;
1251 case DoubleTelemetryField::TranslationP:
1252 if (translationPID) {
1253 translationPID->SetP(dt.Get());
1257 case DoubleTelemetryField::TranslationI:
1258 if (translationPID) {
1259 translationPID->SetI(dt.Get());
1263 case DoubleTelemetryField::TranslationD:
1264 if (translationPID) {
1265 translationPID->SetD(dt.Get());
1269 case DoubleTelemetryField::RotationP:
1271 rotationPID->SetP(dt.Get());
1275 case DoubleTelemetryField::RotationI:
1277 rotationPID->SetI(dt.Get());
1281 case DoubleTelemetryField::RotationD:
1283 rotationPID->SetD(dt.Get());
1287 case DoubleTelemetryField::AutoAlignPoseX:
1288 case DoubleTelemetryField::AutoAlignPoseY:
1289 case DoubleTelemetryField::AutoAlignPoseRotation:
1292 case DoubleTelemetryField::ModulesDriveP:
1293 for (
auto* module : modules)
module->GetDriveMotorController()->SetKp(dt.Get());
1295 case DoubleTelemetryField::ModulesDriveI:
1296 for (
auto* module : modules)
module->GetDriveMotorController()->SetKi(dt.Get());
1298 case DoubleTelemetryField::ModulesDriveD:
1299 for (
auto* module : modules)
module->GetDriveMotorController()->SetKd(dt.Get());
1301 case DoubleTelemetryField::ModulesDriveKs:
1302 for (
auto* module : modules)
module->GetDriveMotorController()->SetKs(dt.Get());
1304 case DoubleTelemetryField::ModulesDriveKv:
1305 for (
auto* module : modules)
module->GetDriveMotorController()->SetKv(dt.Get());
1307 case DoubleTelemetryField::ModulesDriveKa:
1308 for (
auto* module : modules)
module->GetDriveMotorController()->SetKa(dt.Get());
1310 case DoubleTelemetryField::ModulesAzimuthP:
1311 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKp(dt.Get());
1313 case DoubleTelemetryField::ModulesAzimuthI:
1314 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKi(dt.Get());
1316 case DoubleTelemetryField::ModulesAzimuthD:
1317 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKd(dt.Get());
1319 case DoubleTelemetryField::ModulesAzimuthKs:
1320 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKs(dt.Get());
1322 case DoubleTelemetryField::ModulesAzimuthKv:
1323 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKv(dt.Get());
1325 case DoubleTelemetryField::ModulesAzimuthKa:
1326 for (
auto* module : modules)
module->GetAzimuthMotorController()->SetKa(dt.Get());
1328 case DoubleTelemetryField::ModulesDriveVelocity:
1329 if (modes.contains(TuningMode::Drive) && !modes.contains(TuningMode::DriveVoltage)) {
1330 wpi::util::array<wpi::math::SwerveModuleVelocity, NumModules> states{
1331 wpi::util::empty_array};
1332 for (
size_t i = 0; i < NumModules; ++i) {
1333 states[i] = wpi::math::SwerveModuleVelocity{wpi::units::meters_per_second_t{dt.Get()},
1339 case DoubleTelemetryField::ModulesDriveVoltage:
1340 if (modes.contains(TuningMode::DriveVoltage)) {
1341 for (
size_t i = 0; i < NumModules; ++i) {
1343 modules[i]->GetDriveMotorController()->SetVoltage(wpi::units::volt_t{dt.Get()});
1344 modules[i]->GetAzimuthMotorController()->SetPosition(
1345 wpi::units::degree_t{driveAngles[i].Degrees()});
1349 case DoubleTelemetryField::ModulesAzimuthAngle:
1350 if (modes.contains(TuningMode::Azimuth) && !modes.contains(TuningMode::AzimuthVoltage)) {
1353 drive->
SetAzimuthAngles(wpi::math::Rotation2d{wpi::units::degree_t{dt.Get()}});
1356 case DoubleTelemetryField::ModulesAzimuthVoltage:
1357 if (modes.contains(TuningMode::AzimuthVoltage)) {
1358 for (
auto* module : modules) {
1360 module->GetDriveMotorController()->SetVelocity(wpi::units::meters_per_second_t{0} +
1361 module->GetCouplingVelocity());
1362 module->GetAzimuthMotorController()->SetVoltage(wpi::units::volt_t{dt.Get()});