001/**
002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868
003 * Open Source Software; you may modify and/or share it under the terms of
004 * the 3-Clause BSD License found in the root directory of this project.
005 */
006
007package tagalong.controls;
008
009import com.ctre.phoenix6.configs.Slot0Configs;
010import com.ctre.phoenix6.configs.Slot1Configs;
011import com.ctre.phoenix6.configs.Slot2Configs;
012import com.ctre.phoenix6.configs.SlotConfigs;
013
014/**
015 * Wrapper class for PID + FeedForward coefficients for generic reuse in CTRE
016 * Devices
017 */
018public class PIDSGVAConstants extends PIDConstants {
019  /**
020   * FeedForward coefficient S or static in volts
021   */
022  public final double s;
023  /**
024   * FeedForward coefficient G or gravity in volts
025   */
026  public final double g;
027  /**
028   * FeedForward coefficient V or velocity in volts per unit of velocity
029   */
030  public final double v;
031  /**
032   * FeedForward coefficient A or acceleration in volts per unit of acceleration
033   */
034  public final double a;
035
036  private final SlotConfigs genericSlotConfigs;
037
038  /**
039   *
040   * @param p P Factor
041   * @param i I Factor
042   * @param d D Factor
043   * @param s S Factor
044   * @param g G Factor
045   * @param v V Factor
046   * @param a A Factor
047   */
048  public PIDSGVAConstants(double p, double i, double d, double s, double g, double v, double a) {
049    super(p, i, d);
050    this.s = s;
051    this.g = g;
052    this.v = v;
053    this.a = a;
054    genericSlotConfigs =
055        new SlotConfigs().withKP(p).withKI(i).withKD(d).withKS(s).withKG(g).withKV(v).withKA(a);
056  }
057
058  /**
059   *
060   * @return Values as CTRE Device Slot 0 configuration
061   */
062  public Slot0Configs toCTRESlot0Configuration() {
063    return Slot0Configs.from(genericSlotConfigs);
064  }
065
066  /**
067   *
068   * @return Values as CTRE Device Slot 1 configuration
069   */
070  public Slot1Configs toCTRESlot1Configuration() {
071    return Slot1Configs.from(genericSlotConfigs);
072  }
073
074  /**
075   *
076   * @return Values as CTRE Device Slot 2 configuration
077   */
078  public Slot2Configs toCTRESlot2Configuration() {
079    return Slot2Configs.from(genericSlotConfigs);
080  }
081}