001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.configs; 008 009import com.ctre.phoenix6.StatusCode; 010import com.ctre.phoenix6.configs.jni.ConfigJNI; 011import java.util.HashMap; 012import java.util.Map; 013 014import com.ctre.phoenix6.spns.*; 015import com.ctre.phoenix6.signals.*; 016 017/** 018 * Gains for the specified slot. 019 * <p> 020 * If this slot is selected, these gains are used in closed loop 021 * control requests. 022 */ 023public class SlotConfigs implements ParentConfiguration 024{ 025 /** 026 * Proportional Gain 027 * <p> 028 * The units for this gain is dependent on the control mode. Since 029 * this gain is multiplied by error in the input, the units should be 030 * defined as units of output per unit of input error. For example, 031 * when controlling velocity using a duty cycle closed loop, the units 032 * for the proportional gain will be duty cycle per rps of error, or 033 * 1/rps. 034 * 035 * <ul> 036 * <li> <b>Minimum Value:</b> 0 037 * <li> <b>Maximum Value:</b> 3.4e+38 038 * <li> <b>Default Value:</b> 0 039 * <li> <b>Units:</b> 040 * </ul> 041 */ 042 public double kP = 0; 043 /** 044 * Integral Gain 045 * <p> 046 * The units for this gain is dependent on the control mode. Since 047 * this gain is multiplied by error in the input integrated over time 048 * (in units of seconds), the units should be defined as units of 049 * output per unit of integrated input error. For example, when 050 * controlling velocity using a duty cycle closed loop, integrating 051 * velocity over time results in rps * s = rotations. Therefore, the 052 * units for the integral gain will be duty cycle per rotation of 053 * accumulated error, or 1/rot. 054 * 055 * <ul> 056 * <li> <b>Minimum Value:</b> 0 057 * <li> <b>Maximum Value:</b> 3.4e+38 058 * <li> <b>Default Value:</b> 0 059 * <li> <b>Units:</b> 060 * </ul> 061 */ 062 public double kI = 0; 063 /** 064 * Derivative Gain 065 * <p> 066 * The units for this gain is dependent on the control mode. Since 067 * this gain is multiplied by the derivative of error in the input 068 * with respect to time (in units of seconds), the units should be 069 * defined as units of output per unit of the differentiated input 070 * error. For example, when controlling velocity using a duty cycle 071 * closed loop, the derivative of velocity with respect to time is 072 * rps/s, which is acceleration. Therefore, the units for the 073 * derivative gain will be duty cycle per unit of acceleration error, 074 * or 1/(rps/s). 075 * 076 * <ul> 077 * <li> <b>Minimum Value:</b> 0 078 * <li> <b>Maximum Value:</b> 3.4e+38 079 * <li> <b>Default Value:</b> 0 080 * <li> <b>Units:</b> 081 * </ul> 082 */ 083 public double kD = 0; 084 /** 085 * Static Feedforward Gain 086 * <p> 087 * This is added to the closed loop output. The unit for this constant 088 * is dependent on the control mode, typically fractional duty cycle, 089 * voltage, or torque current. 090 * <p> 091 * The sign is typically determined by reference velocity when using 092 * position, velocity, and Motion Magic® closed loop modes. However, 093 * when using position closed loop with zero velocity reference (no 094 * motion profiling), the application can instead use the position 095 * closed loop error by setting the Static Feedforward Sign 096 * configuration parameter. When doing so, we recommend the minimal 097 * amount of kS, otherwise the motor output may dither when closed 098 * loop error is near zero. 099 * 100 * <ul> 101 * <li> <b>Minimum Value:</b> -512 102 * <li> <b>Maximum Value:</b> 511 103 * <li> <b>Default Value:</b> 0 104 * <li> <b>Units:</b> 105 * </ul> 106 */ 107 public double kS = 0; 108 /** 109 * Velocity Feedforward Gain 110 * <p> 111 * The units for this gain is dependent on the control mode. Since 112 * this gain is multiplied by the requested velocity, the units should 113 * be defined as units of output per unit of requested input velocity. 114 * For example, when controlling velocity using a duty cycle closed 115 * loop, the units for the velocity feedfoward gain will be duty cycle 116 * per requested rps, or 1/rps. 117 * 118 * <ul> 119 * <li> <b>Minimum Value:</b> 0 120 * <li> <b>Maximum Value:</b> 3.4e+38 121 * <li> <b>Default Value:</b> 0 122 * <li> <b>Units:</b> 123 * </ul> 124 */ 125 public double kV = 0; 126 /** 127 * Acceleration Feedforward Gain 128 * <p> 129 * The units for this gain is dependent on the control mode. Since 130 * this gain is multiplied by the requested acceleration, the units 131 * should be defined as units of output per unit of requested input 132 * acceleration. For example, when controlling velocity using a duty 133 * cycle closed loop, the units for the acceleration feedfoward gain 134 * will be duty cycle per requested rps/s, or 1/(rps/s). 135 * 136 * <ul> 137 * <li> <b>Minimum Value:</b> 0 138 * <li> <b>Maximum Value:</b> 3.4e+38 139 * <li> <b>Default Value:</b> 0 140 * <li> <b>Units:</b> 141 * </ul> 142 */ 143 public double kA = 0; 144 /** 145 * Gravity Feedforward/Feedback Gain 146 * <p> 147 * This is added to the closed loop output. The sign is determined by 148 * GravityType. The unit for this constant is dependent on the control 149 * mode, typically fractional duty cycle, voltage, or torque current. 150 * 151 * <ul> 152 * <li> <b>Minimum Value:</b> -512 153 * <li> <b>Maximum Value:</b> 511 154 * <li> <b>Default Value:</b> 0 155 * <li> <b>Units:</b> 156 * </ul> 157 */ 158 public double kG = 0; 159 /** 160 * Gravity Feedforward/Feedback Type 161 * <p> 162 * This determines the type of the gravity feedforward/feedback. 163 * <p> 164 * Choose Elevator_Static for systems where the gravity feedforward is 165 * constant, such as an elevator. The gravity feedforward output will 166 * always have the same sign. 167 * <p> 168 * Choose Arm_Cosine for systems where the gravity feedback is 169 * dependent on the angular position of the mechanism, such as an arm. 170 * The gravity feedback output will vary depending on the mechanism 171 * angular position. Note that the sensor offset and ratios must be 172 * configured so that the sensor reports a position of 0 when the 173 * mechanism is horizonal (parallel to the ground), and the reported 174 * sensor position is 1:1 with the mechanism. 175 * 176 */ 177 public GravityTypeValue GravityType = GravityTypeValue.Elevator_Static; 178 /** 179 * Static Feedforward Sign during position closed loop 180 * <p> 181 * This determines the sign of the applied kS during position 182 * closed-loop modes. The default behavior uses the velocity reference 183 * sign. This works well with velocity closed loop, Motion Magic® 184 * controls, and position closed loop when velocity reference is 185 * specified (motion profiling). 186 * <p> 187 * However, when using position closed loop with zero velocity 188 * reference (no motion profiling), the application may want to apply 189 * static feedforward based on the closed loop error sign instead. 190 * When doing so, we recommend the minimal amount of kS, otherwise the 191 * motor output may dither when closed loop error is near zero. 192 * 193 */ 194 public StaticFeedforwardSignValue StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; 195 196 /** 197 * Modifies this configuration's kP parameter and returns itself for 198 * method-chaining and easier to use config API. 199 * <p> 200 * Proportional Gain 201 * <p> 202 * The units for this gain is dependent on the control mode. Since 203 * this gain is multiplied by error in the input, the units should be 204 * defined as units of output per unit of input error. For example, 205 * when controlling velocity using a duty cycle closed loop, the units 206 * for the proportional gain will be duty cycle per rps of error, or 207 * 1/rps. 208 * 209 * <ul> 210 * <li> <b>Minimum Value:</b> 0 211 * <li> <b>Maximum Value:</b> 3.4e+38 212 * <li> <b>Default Value:</b> 0 213 * <li> <b>Units:</b> 214 * </ul> 215 * 216 * @param newKP Parameter to modify 217 * @return Itself 218 */ 219 public SlotConfigs withKP(double newKP) 220 { 221 kP = newKP; 222 return this; 223 } 224 /** 225 * Modifies this configuration's kI parameter and returns itself for 226 * method-chaining and easier to use config API. 227 * <p> 228 * Integral Gain 229 * <p> 230 * The units for this gain is dependent on the control mode. Since 231 * this gain is multiplied by error in the input integrated over time 232 * (in units of seconds), the units should be defined as units of 233 * output per unit of integrated input error. For example, when 234 * controlling velocity using a duty cycle closed loop, integrating 235 * velocity over time results in rps * s = rotations. Therefore, the 236 * units for the integral gain will be duty cycle per rotation of 237 * accumulated error, or 1/rot. 238 * 239 * <ul> 240 * <li> <b>Minimum Value:</b> 0 241 * <li> <b>Maximum Value:</b> 3.4e+38 242 * <li> <b>Default Value:</b> 0 243 * <li> <b>Units:</b> 244 * </ul> 245 * 246 * @param newKI Parameter to modify 247 * @return Itself 248 */ 249 public SlotConfigs withKI(double newKI) 250 { 251 kI = newKI; 252 return this; 253 } 254 /** 255 * Modifies this configuration's kD parameter and returns itself for 256 * method-chaining and easier to use config API. 257 * <p> 258 * Derivative Gain 259 * <p> 260 * The units for this gain is dependent on the control mode. Since 261 * this gain is multiplied by the derivative of error in the input 262 * with respect to time (in units of seconds), the units should be 263 * defined as units of output per unit of the differentiated input 264 * error. For example, when controlling velocity using a duty cycle 265 * closed loop, the derivative of velocity with respect to time is 266 * rps/s, which is acceleration. Therefore, the units for the 267 * derivative gain will be duty cycle per unit of acceleration error, 268 * or 1/(rps/s). 269 * 270 * <ul> 271 * <li> <b>Minimum Value:</b> 0 272 * <li> <b>Maximum Value:</b> 3.4e+38 273 * <li> <b>Default Value:</b> 0 274 * <li> <b>Units:</b> 275 * </ul> 276 * 277 * @param newKD Parameter to modify 278 * @return Itself 279 */ 280 public SlotConfigs withKD(double newKD) 281 { 282 kD = newKD; 283 return this; 284 } 285 /** 286 * Modifies this configuration's kS parameter and returns itself for 287 * method-chaining and easier to use config API. 288 * <p> 289 * Static Feedforward Gain 290 * <p> 291 * This is added to the closed loop output. The unit for this constant 292 * is dependent on the control mode, typically fractional duty cycle, 293 * voltage, or torque current. 294 * <p> 295 * The sign is typically determined by reference velocity when using 296 * position, velocity, and Motion Magic® closed loop modes. However, 297 * when using position closed loop with zero velocity reference (no 298 * motion profiling), the application can instead use the position 299 * closed loop error by setting the Static Feedforward Sign 300 * configuration parameter. When doing so, we recommend the minimal 301 * amount of kS, otherwise the motor output may dither when closed 302 * loop error is near zero. 303 * 304 * <ul> 305 * <li> <b>Minimum Value:</b> -512 306 * <li> <b>Maximum Value:</b> 511 307 * <li> <b>Default Value:</b> 0 308 * <li> <b>Units:</b> 309 * </ul> 310 * 311 * @param newKS Parameter to modify 312 * @return Itself 313 */ 314 public SlotConfigs withKS(double newKS) 315 { 316 kS = newKS; 317 return this; 318 } 319 /** 320 * Modifies this configuration's kV parameter and returns itself for 321 * method-chaining and easier to use config API. 322 * <p> 323 * Velocity Feedforward Gain 324 * <p> 325 * The units for this gain is dependent on the control mode. Since 326 * this gain is multiplied by the requested velocity, the units should 327 * be defined as units of output per unit of requested input velocity. 328 * For example, when controlling velocity using a duty cycle closed 329 * loop, the units for the velocity feedfoward gain will be duty cycle 330 * per requested rps, or 1/rps. 331 * 332 * <ul> 333 * <li> <b>Minimum Value:</b> 0 334 * <li> <b>Maximum Value:</b> 3.4e+38 335 * <li> <b>Default Value:</b> 0 336 * <li> <b>Units:</b> 337 * </ul> 338 * 339 * @param newKV Parameter to modify 340 * @return Itself 341 */ 342 public SlotConfigs withKV(double newKV) 343 { 344 kV = newKV; 345 return this; 346 } 347 /** 348 * Modifies this configuration's kA parameter and returns itself for 349 * method-chaining and easier to use config API. 350 * <p> 351 * Acceleration Feedforward Gain 352 * <p> 353 * The units for this gain is dependent on the control mode. Since 354 * this gain is multiplied by the requested acceleration, the units 355 * should be defined as units of output per unit of requested input 356 * acceleration. For example, when controlling velocity using a duty 357 * cycle closed loop, the units for the acceleration feedfoward gain 358 * will be duty cycle per requested rps/s, or 1/(rps/s). 359 * 360 * <ul> 361 * <li> <b>Minimum Value:</b> 0 362 * <li> <b>Maximum Value:</b> 3.4e+38 363 * <li> <b>Default Value:</b> 0 364 * <li> <b>Units:</b> 365 * </ul> 366 * 367 * @param newKA Parameter to modify 368 * @return Itself 369 */ 370 public SlotConfigs withKA(double newKA) 371 { 372 kA = newKA; 373 return this; 374 } 375 /** 376 * Modifies this configuration's kG parameter and returns itself for 377 * method-chaining and easier to use config API. 378 * <p> 379 * Gravity Feedforward/Feedback Gain 380 * <p> 381 * This is added to the closed loop output. The sign is determined by 382 * GravityType. The unit for this constant is dependent on the control 383 * mode, typically fractional duty cycle, voltage, or torque current. 384 * 385 * <ul> 386 * <li> <b>Minimum Value:</b> -512 387 * <li> <b>Maximum Value:</b> 511 388 * <li> <b>Default Value:</b> 0 389 * <li> <b>Units:</b> 390 * </ul> 391 * 392 * @param newKG Parameter to modify 393 * @return Itself 394 */ 395 public SlotConfigs withKG(double newKG) 396 { 397 kG = newKG; 398 return this; 399 } 400 /** 401 * Modifies this configuration's GravityType parameter and returns itself for 402 * method-chaining and easier to use config API. 403 * <p> 404 * Gravity Feedforward/Feedback Type 405 * <p> 406 * This determines the type of the gravity feedforward/feedback. 407 * <p> 408 * Choose Elevator_Static for systems where the gravity feedforward is 409 * constant, such as an elevator. The gravity feedforward output will 410 * always have the same sign. 411 * <p> 412 * Choose Arm_Cosine for systems where the gravity feedback is 413 * dependent on the angular position of the mechanism, such as an arm. 414 * The gravity feedback output will vary depending on the mechanism 415 * angular position. Note that the sensor offset and ratios must be 416 * configured so that the sensor reports a position of 0 when the 417 * mechanism is horizonal (parallel to the ground), and the reported 418 * sensor position is 1:1 with the mechanism. 419 * 420 * 421 * @param newGravityType Parameter to modify 422 * @return Itself 423 */ 424 public SlotConfigs withGravityType(GravityTypeValue newGravityType) 425 { 426 GravityType = newGravityType; 427 return this; 428 } 429 /** 430 * Modifies this configuration's StaticFeedforwardSign parameter and returns itself for 431 * method-chaining and easier to use config API. 432 * <p> 433 * Static Feedforward Sign during position closed loop 434 * <p> 435 * This determines the sign of the applied kS during position 436 * closed-loop modes. The default behavior uses the velocity reference 437 * sign. This works well with velocity closed loop, Motion Magic® 438 * controls, and position closed loop when velocity reference is 439 * specified (motion profiling). 440 * <p> 441 * However, when using position closed loop with zero velocity 442 * reference (no motion profiling), the application may want to apply 443 * static feedforward based on the closed loop error sign instead. 444 * When doing so, we recommend the minimal amount of kS, otherwise the 445 * motor output may dither when closed loop error is near zero. 446 * 447 * 448 * @param newStaticFeedforwardSign Parameter to modify 449 * @return Itself 450 */ 451 public SlotConfigs withStaticFeedforwardSign(StaticFeedforwardSignValue newStaticFeedforwardSign) 452 { 453 StaticFeedforwardSign = newStaticFeedforwardSign; 454 return this; 455 } 456 457 458 /** 459 * Chooses which slot these configs are for. 460 */ 461 public int SlotNumber = 0; 462 463 private class SlotSpns 464 { 465 int kPSpn; 466 int kISpn; 467 int kDSpn; 468 int kSSpn; 469 int kVSpn; 470 int kASpn; 471 int kGSpn; 472 int GravityTypeSpn; 473 int StaticFeedforwardSignSpn; 474 } 475 476 private Map<Integer, SlotSpns> genericMap = new HashMap<Integer, SlotSpns>() {{ 477 put(0, new SlotSpns() {{ 478 kPSpn = SpnValue.Slot0_kP.value; 479 kISpn = SpnValue.Slot0_kI.value; 480 kDSpn = SpnValue.Slot0_kD.value; 481 kSSpn = SpnValue.Slot0_kS.value; 482 kVSpn = SpnValue.Slot0_kV.value; 483 kASpn = SpnValue.Slot0_kA.value; 484 kGSpn = SpnValue.Slot0_kG.value; 485 GravityTypeSpn = SpnValue.Slot0_kG_Type.value; 486 StaticFeedforwardSignSpn = SpnValue.Slot0_kS_Sign.value; 487 }}); 488 put(1, new SlotSpns() {{ 489 kPSpn = SpnValue.Slot1_kP.value; 490 kISpn = SpnValue.Slot1_kI.value; 491 kDSpn = SpnValue.Slot1_kD.value; 492 kSSpn = SpnValue.Slot1_kS.value; 493 kVSpn = SpnValue.Slot1_kV.value; 494 kASpn = SpnValue.Slot1_kA.value; 495 kGSpn = SpnValue.Slot1_kG.value; 496 GravityTypeSpn = SpnValue.Slot1_kG_Type.value; 497 StaticFeedforwardSignSpn = SpnValue.Slot1_kS_Sign.value; 498 }}); 499 put(2, new SlotSpns() {{ 500 kPSpn = SpnValue.Slot2_kP.value; 501 kISpn = SpnValue.Slot2_kI.value; 502 kDSpn = SpnValue.Slot2_kD.value; 503 kSSpn = SpnValue.Slot2_kS.value; 504 kVSpn = SpnValue.Slot2_kV.value; 505 kASpn = SpnValue.Slot2_kA.value; 506 kGSpn = SpnValue.Slot2_kG.value; 507 GravityTypeSpn = SpnValue.Slot2_kG_Type.value; 508 StaticFeedforwardSignSpn = SpnValue.Slot2_kS_Sign.value; 509 }}); 510 }}; 511 512 public static SlotConfigs from(Slot0Configs value) 513 { 514 return new SlotConfigs() {{ 515 kP = value.kP; 516 kI = value.kI; 517 kD = value.kD; 518 kS = value.kS; 519 kV = value.kV; 520 kA = value.kA; 521 kG = value.kG; 522 GravityType = value.GravityType; 523 StaticFeedforwardSign = value.StaticFeedforwardSign; 524 SlotNumber = 0; 525 }}; 526 } 527 public static SlotConfigs from(Slot1Configs value) 528 { 529 return new SlotConfigs() {{ 530 kP = value.kP; 531 kI = value.kI; 532 kD = value.kD; 533 kS = value.kS; 534 kV = value.kV; 535 kA = value.kA; 536 kG = value.kG; 537 GravityType = value.GravityType; 538 StaticFeedforwardSign = value.StaticFeedforwardSign; 539 SlotNumber = 1; 540 }}; 541 } 542 public static SlotConfigs from(Slot2Configs value) 543 { 544 return new SlotConfigs() {{ 545 kP = value.kP; 546 kI = value.kI; 547 kD = value.kD; 548 kS = value.kS; 549 kV = value.kV; 550 kA = value.kA; 551 kG = value.kG; 552 GravityType = value.GravityType; 553 StaticFeedforwardSign = value.StaticFeedforwardSign; 554 SlotNumber = 2; 555 }}; 556 } 557 558 @Override 559 public String toString() 560 { 561 String ss = "Config Group: Slot\n"; 562 ss += "Name: \"kP\" Value: \"" + kP + "\"" + "\n"; 563 ss += "Name: \"kI\" Value: \"" + kI + "\"" + "\n"; 564 ss += "Name: \"kD\" Value: \"" + kD + "\"" + "\n"; 565 ss += "Name: \"kS\" Value: \"" + kS + "\"" + "\n"; 566 ss += "Name: \"kV\" Value: \"" + kV + "\"" + "\n"; 567 ss += "Name: \"kA\" Value: \"" + kA + "\"" + "\n"; 568 ss += "Name: \"kG\" Value: \"" + kG + "\"" + "\n"; 569 ss += "Name: \"GravityType\" Value: \"" + GravityType + "\"" + "\n"; 570 ss += "Name: \"StaticFeedforwardSign\" Value: \"" + StaticFeedforwardSign + "\"" + "\n"; 571 return ss; 572 } 573 574 /** 575 * 576 */ 577 public StatusCode deserialize(String to_deserialize) 578 { 579 SlotSpns currentSpns = genericMap.get(SlotNumber); 580 kP = ConfigJNI.Deserializedouble(currentSpns.kPSpn, to_deserialize); 581 kI = ConfigJNI.Deserializedouble(currentSpns.kISpn, to_deserialize); 582 kD = ConfigJNI.Deserializedouble(currentSpns.kDSpn, to_deserialize); 583 kS = ConfigJNI.Deserializedouble(currentSpns.kSSpn, to_deserialize); 584 kV = ConfigJNI.Deserializedouble(currentSpns.kVSpn, to_deserialize); 585 kA = ConfigJNI.Deserializedouble(currentSpns.kASpn, to_deserialize); 586 kG = ConfigJNI.Deserializedouble(currentSpns.kGSpn, to_deserialize); 587 GravityType = GravityTypeValue.valueOf(ConfigJNI.Deserializeint(currentSpns.GravityTypeSpn, to_deserialize)); 588 StaticFeedforwardSign = StaticFeedforwardSignValue.valueOf(ConfigJNI.Deserializeint(currentSpns.StaticFeedforwardSignSpn, to_deserialize)); 589 return StatusCode.OK; 590 } 591 592 /** 593 * 594 */ 595 public String serialize() 596 { 597 String ss = ""; 598 SlotSpns currentSpns = genericMap.get(SlotNumber); 599 ss += ConfigJNI.Serializedouble(currentSpns.kPSpn, kP); 600 ss += ConfigJNI.Serializedouble(currentSpns.kISpn, kI); 601 ss += ConfigJNI.Serializedouble(currentSpns.kDSpn, kD); 602 ss += ConfigJNI.Serializedouble(currentSpns.kSSpn, kS); 603 ss += ConfigJNI.Serializedouble(currentSpns.kVSpn, kV); 604 ss += ConfigJNI.Serializedouble(currentSpns.kASpn, kA); 605 ss += ConfigJNI.Serializedouble(currentSpns.kGSpn, kG); 606 ss += ConfigJNI.Serializeint(currentSpns.GravityTypeSpn, GravityType.value); 607 ss += ConfigJNI.Serializeint(currentSpns.StaticFeedforwardSignSpn, StaticFeedforwardSign.value); 608 return ss; 609 } 610} 611