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 rot 072 * per sec², which is acceleration. Therefore, the units for the 073 * derivative gain will be duty cycle per unit of acceleration error, 074 * or 1/(rot per sec²). 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 rot per sec², or 1/(rot per sec²). 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 sign of closed loop error instead. 190 * When doing so, we recommend using the minimal amount of kS, 191 * otherwise the motor output may dither when closed loop error is 192 * near zero. 193 * 194 */ 195 public StaticFeedforwardSignValue StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; 196 197 /** 198 * Modifies this configuration's kP parameter and returns itself for 199 * method-chaining and easier to use config API. 200 * <p> 201 * Proportional Gain. 202 * <p> 203 * The units for this gain is dependent on the control mode. Since 204 * this gain is multiplied by error in the input, the units should be 205 * defined as units of output per unit of input error. For example, 206 * when controlling velocity using a duty cycle closed loop, the units 207 * for the proportional gain will be duty cycle per rps of error, or 208 * 1/rps. 209 * 210 * <ul> 211 * <li> <b>Minimum Value:</b> 0 212 * <li> <b>Maximum Value:</b> 3.4e+38 213 * <li> <b>Default Value:</b> 0 214 * <li> <b>Units:</b> 215 * </ul> 216 * 217 * @param newKP Parameter to modify 218 * @return Itself 219 */ 220 public SlotConfigs withKP(double newKP) 221 { 222 kP = newKP; 223 return this; 224 } 225 226 /** 227 * Modifies this configuration's kI parameter and returns itself for 228 * method-chaining and easier to use config API. 229 * <p> 230 * Integral Gain. 231 * <p> 232 * The units for this gain is dependent on the control mode. Since 233 * this gain is multiplied by error in the input integrated over time 234 * (in units of seconds), the units should be defined as units of 235 * output per unit of integrated input error. For example, when 236 * controlling velocity using a duty cycle closed loop, integrating 237 * velocity over time results in rps * s = rotations. Therefore, the 238 * units for the integral gain will be duty cycle per rotation of 239 * accumulated error, or 1/rot. 240 * 241 * <ul> 242 * <li> <b>Minimum Value:</b> 0 243 * <li> <b>Maximum Value:</b> 3.4e+38 244 * <li> <b>Default Value:</b> 0 245 * <li> <b>Units:</b> 246 * </ul> 247 * 248 * @param newKI Parameter to modify 249 * @return Itself 250 */ 251 public SlotConfigs withKI(double newKI) 252 { 253 kI = newKI; 254 return this; 255 } 256 257 /** 258 * Modifies this configuration's kD parameter and returns itself for 259 * method-chaining and easier to use config API. 260 * <p> 261 * Derivative Gain. 262 * <p> 263 * The units for this gain is dependent on the control mode. Since 264 * this gain is multiplied by the derivative of error in the input 265 * with respect to time (in units of seconds), the units should be 266 * defined as units of output per unit of the differentiated input 267 * error. For example, when controlling velocity using a duty cycle 268 * closed loop, the derivative of velocity with respect to time is rot 269 * per sec², which is acceleration. Therefore, the units for the 270 * derivative gain will be duty cycle per unit of acceleration error, 271 * or 1/(rot per sec²). 272 * 273 * <ul> 274 * <li> <b>Minimum Value:</b> 0 275 * <li> <b>Maximum Value:</b> 3.4e+38 276 * <li> <b>Default Value:</b> 0 277 * <li> <b>Units:</b> 278 * </ul> 279 * 280 * @param newKD Parameter to modify 281 * @return Itself 282 */ 283 public SlotConfigs withKD(double newKD) 284 { 285 kD = newKD; 286 return this; 287 } 288 289 /** 290 * Modifies this configuration's kS parameter and returns itself for 291 * method-chaining and easier to use config API. 292 * <p> 293 * Static Feedforward Gain. 294 * <p> 295 * This is added to the closed loop output. The unit for this constant 296 * is dependent on the control mode, typically fractional duty cycle, 297 * voltage, or torque current. 298 * <p> 299 * The sign is typically determined by reference velocity when using 300 * position, velocity, and Motion Magic® closed loop modes. However, 301 * when using position closed loop with zero velocity reference (no 302 * motion profiling), the application can instead use the position 303 * closed loop error by setting the Static Feedforward Sign 304 * configuration parameter. When doing so, we recommend the minimal 305 * amount of kS, otherwise the motor output may dither when closed 306 * loop error is near zero. 307 * 308 * <ul> 309 * <li> <b>Minimum Value:</b> -512 310 * <li> <b>Maximum Value:</b> 511 311 * <li> <b>Default Value:</b> 0 312 * <li> <b>Units:</b> 313 * </ul> 314 * 315 * @param newKS Parameter to modify 316 * @return Itself 317 */ 318 public SlotConfigs withKS(double newKS) 319 { 320 kS = newKS; 321 return this; 322 } 323 324 /** 325 * Modifies this configuration's kV parameter and returns itself for 326 * method-chaining and easier to use config API. 327 * <p> 328 * Velocity Feedforward Gain. 329 * <p> 330 * The units for this gain is dependent on the control mode. Since 331 * this gain is multiplied by the requested velocity, the units should 332 * be defined as units of output per unit of requested input velocity. 333 * For example, when controlling velocity using a duty cycle closed 334 * loop, the units for the velocity feedfoward gain will be duty cycle 335 * per requested rps, or 1/rps. 336 * 337 * <ul> 338 * <li> <b>Minimum Value:</b> 0 339 * <li> <b>Maximum Value:</b> 3.4e+38 340 * <li> <b>Default Value:</b> 0 341 * <li> <b>Units:</b> 342 * </ul> 343 * 344 * @param newKV Parameter to modify 345 * @return Itself 346 */ 347 public SlotConfigs withKV(double newKV) 348 { 349 kV = newKV; 350 return this; 351 } 352 353 /** 354 * Modifies this configuration's kA parameter and returns itself for 355 * method-chaining and easier to use config API. 356 * <p> 357 * Acceleration Feedforward Gain. 358 * <p> 359 * The units for this gain is dependent on the control mode. Since 360 * this gain is multiplied by the requested acceleration, the units 361 * should be defined as units of output per unit of requested input 362 * acceleration. For example, when controlling velocity using a duty 363 * cycle closed loop, the units for the acceleration feedfoward gain 364 * will be duty cycle per requested rot per sec², or 1/(rot per sec²). 365 * 366 * <ul> 367 * <li> <b>Minimum Value:</b> 0 368 * <li> <b>Maximum Value:</b> 3.4e+38 369 * <li> <b>Default Value:</b> 0 370 * <li> <b>Units:</b> 371 * </ul> 372 * 373 * @param newKA Parameter to modify 374 * @return Itself 375 */ 376 public SlotConfigs withKA(double newKA) 377 { 378 kA = newKA; 379 return this; 380 } 381 382 /** 383 * Modifies this configuration's kG parameter and returns itself for 384 * method-chaining and easier to use config API. 385 * <p> 386 * Gravity Feedforward/Feedback Gain. 387 * <p> 388 * This is added to the closed loop output. The sign is determined by 389 * GravityType. The unit for this constant is dependent on the control 390 * mode, typically fractional duty cycle, voltage, or torque current. 391 * 392 * <ul> 393 * <li> <b>Minimum Value:</b> -512 394 * <li> <b>Maximum Value:</b> 511 395 * <li> <b>Default Value:</b> 0 396 * <li> <b>Units:</b> 397 * </ul> 398 * 399 * @param newKG Parameter to modify 400 * @return Itself 401 */ 402 public SlotConfigs withKG(double newKG) 403 { 404 kG = newKG; 405 return this; 406 } 407 408 /** 409 * Modifies this configuration's GravityType parameter and returns itself for 410 * method-chaining and easier to use config API. 411 * <p> 412 * Gravity Feedforward/Feedback Type. 413 * <p> 414 * This determines the type of the gravity feedforward/feedback. 415 * <p> 416 * Choose Elevator_Static for systems where the gravity feedforward is 417 * constant, such as an elevator. The gravity feedforward output will 418 * always have the same sign. 419 * <p> 420 * Choose Arm_Cosine for systems where the gravity feedback is 421 * dependent on the angular position of the mechanism, such as an arm. 422 * The gravity feedback output will vary depending on the mechanism 423 * angular position. Note that the sensor offset and ratios must be 424 * configured so that the sensor reports a position of 0 when the 425 * mechanism is horizonal (parallel to the ground), and the reported 426 * sensor position is 1:1 with the mechanism. 427 * 428 * 429 * @param newGravityType Parameter to modify 430 * @return Itself 431 */ 432 public SlotConfigs withGravityType(GravityTypeValue newGravityType) 433 { 434 GravityType = newGravityType; 435 return this; 436 } 437 438 /** 439 * Modifies this configuration's StaticFeedforwardSign parameter and returns itself for 440 * method-chaining and easier to use config API. 441 * <p> 442 * Static Feedforward Sign during position closed loop. 443 * <p> 444 * This determines the sign of the applied kS during position 445 * closed-loop modes. The default behavior uses the velocity reference 446 * sign. This works well with velocity closed loop, Motion Magic® 447 * controls, and position closed loop when velocity reference is 448 * specified (motion profiling). 449 * <p> 450 * However, when using position closed loop with zero velocity 451 * reference (no motion profiling), the application may want to apply 452 * static feedforward based on the sign of closed loop error instead. 453 * When doing so, we recommend using the minimal amount of kS, 454 * otherwise the motor output may dither when closed loop error is 455 * near zero. 456 * 457 * 458 * @param newStaticFeedforwardSign Parameter to modify 459 * @return Itself 460 */ 461 public SlotConfigs withStaticFeedforwardSign(StaticFeedforwardSignValue newStaticFeedforwardSign) 462 { 463 StaticFeedforwardSign = newStaticFeedforwardSign; 464 return this; 465 } 466 467 468 /** 469 * Chooses which slot these configs are for. 470 */ 471 public int SlotNumber = 0; 472 473 private class SlotSpns 474 { 475 int kPSpn; 476 int kISpn; 477 int kDSpn; 478 int kSSpn; 479 int kVSpn; 480 int kASpn; 481 int kGSpn; 482 int GravityTypeSpn; 483 int StaticFeedforwardSignSpn; 484 } 485 486 private Map<Integer, SlotSpns> genericMap = new HashMap<Integer, SlotSpns>() {{ 487 put(0, new SlotSpns() {{ 488 kPSpn = SpnValue.Slot0_kP.value; 489 kISpn = SpnValue.Slot0_kI.value; 490 kDSpn = SpnValue.Slot0_kD.value; 491 kSSpn = SpnValue.Slot0_kS.value; 492 kVSpn = SpnValue.Slot0_kV.value; 493 kASpn = SpnValue.Slot0_kA.value; 494 kGSpn = SpnValue.Slot0_kG.value; 495 GravityTypeSpn = SpnValue.Slot0_kG_Type.value; 496 StaticFeedforwardSignSpn = SpnValue.Slot0_kS_Sign.value; 497 }}); 498 put(1, new SlotSpns() {{ 499 kPSpn = SpnValue.Slot1_kP.value; 500 kISpn = SpnValue.Slot1_kI.value; 501 kDSpn = SpnValue.Slot1_kD.value; 502 kSSpn = SpnValue.Slot1_kS.value; 503 kVSpn = SpnValue.Slot1_kV.value; 504 kASpn = SpnValue.Slot1_kA.value; 505 kGSpn = SpnValue.Slot1_kG.value; 506 GravityTypeSpn = SpnValue.Slot1_kG_Type.value; 507 StaticFeedforwardSignSpn = SpnValue.Slot1_kS_Sign.value; 508 }}); 509 put(2, new SlotSpns() {{ 510 kPSpn = SpnValue.Slot2_kP.value; 511 kISpn = SpnValue.Slot2_kI.value; 512 kDSpn = SpnValue.Slot2_kD.value; 513 kSSpn = SpnValue.Slot2_kS.value; 514 kVSpn = SpnValue.Slot2_kV.value; 515 kASpn = SpnValue.Slot2_kA.value; 516 kGSpn = SpnValue.Slot2_kG.value; 517 GravityTypeSpn = SpnValue.Slot2_kG_Type.value; 518 StaticFeedforwardSignSpn = SpnValue.Slot2_kS_Sign.value; 519 }}); 520 }}; 521 522 public static SlotConfigs from(Slot0Configs value) 523 { 524 return new SlotConfigs() {{ 525 kP = value.kP; 526 kI = value.kI; 527 kD = value.kD; 528 kS = value.kS; 529 kV = value.kV; 530 kA = value.kA; 531 kG = value.kG; 532 GravityType = value.GravityType; 533 StaticFeedforwardSign = value.StaticFeedforwardSign; 534 SlotNumber = 0; 535 }}; 536 } 537 public static SlotConfigs from(Slot1Configs value) 538 { 539 return new SlotConfigs() {{ 540 kP = value.kP; 541 kI = value.kI; 542 kD = value.kD; 543 kS = value.kS; 544 kV = value.kV; 545 kA = value.kA; 546 kG = value.kG; 547 GravityType = value.GravityType; 548 StaticFeedforwardSign = value.StaticFeedforwardSign; 549 SlotNumber = 1; 550 }}; 551 } 552 public static SlotConfigs from(Slot2Configs value) 553 { 554 return new SlotConfigs() {{ 555 kP = value.kP; 556 kI = value.kI; 557 kD = value.kD; 558 kS = value.kS; 559 kV = value.kV; 560 kA = value.kA; 561 kG = value.kG; 562 GravityType = value.GravityType; 563 StaticFeedforwardSign = value.StaticFeedforwardSign; 564 SlotNumber = 2; 565 }}; 566 } 567 568 @Override 569 public String toString() 570 { 571 String ss = "Config Group: Slot\n"; 572 ss += " kP: " + kP + "\n"; 573 ss += " kI: " + kI + "\n"; 574 ss += " kD: " + kD + "\n"; 575 ss += " kS: " + kS + "\n"; 576 ss += " kV: " + kV + "\n"; 577 ss += " kA: " + kA + "\n"; 578 ss += " kG: " + kG + "\n"; 579 ss += " GravityType: " + GravityType + "\n"; 580 ss += " StaticFeedforwardSign: " + StaticFeedforwardSign + "\n"; 581 return ss; 582 } 583 584 /** 585 * 586 */ 587 public StatusCode deserialize(String to_deserialize) 588 { 589 SlotSpns currentSpns = genericMap.get(SlotNumber); 590 kP = ConfigJNI.Deserializedouble(currentSpns.kPSpn, to_deserialize); 591 kI = ConfigJNI.Deserializedouble(currentSpns.kISpn, to_deserialize); 592 kD = ConfigJNI.Deserializedouble(currentSpns.kDSpn, to_deserialize); 593 kS = ConfigJNI.Deserializedouble(currentSpns.kSSpn, to_deserialize); 594 kV = ConfigJNI.Deserializedouble(currentSpns.kVSpn, to_deserialize); 595 kA = ConfigJNI.Deserializedouble(currentSpns.kASpn, to_deserialize); 596 kG = ConfigJNI.Deserializedouble(currentSpns.kGSpn, to_deserialize); 597 GravityType = GravityTypeValue.valueOf(ConfigJNI.Deserializeint(currentSpns.GravityTypeSpn, to_deserialize)); 598 StaticFeedforwardSign = StaticFeedforwardSignValue.valueOf(ConfigJNI.Deserializeint(currentSpns.StaticFeedforwardSignSpn, to_deserialize)); 599 return StatusCode.OK; 600 } 601 602 /** 603 * 604 */ 605 public String serialize() 606 { 607 String ss = ""; 608 SlotSpns currentSpns = genericMap.get(SlotNumber); 609 ss += ConfigJNI.Serializedouble(currentSpns.kPSpn, kP); 610 ss += ConfigJNI.Serializedouble(currentSpns.kISpn, kI); 611 ss += ConfigJNI.Serializedouble(currentSpns.kDSpn, kD); 612 ss += ConfigJNI.Serializedouble(currentSpns.kSSpn, kS); 613 ss += ConfigJNI.Serializedouble(currentSpns.kVSpn, kV); 614 ss += ConfigJNI.Serializedouble(currentSpns.kASpn, kA); 615 ss += ConfigJNI.Serializedouble(currentSpns.kGSpn, kG); 616 ss += ConfigJNI.Serializeint(currentSpns.GravityTypeSpn, GravityType.value); 617 ss += ConfigJNI.Serializeint(currentSpns.StaticFeedforwardSignSpn, StaticFeedforwardSign.value); 618 return ss; 619 } 620} 621