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.subsystems.micro.confs; 008 009import com.ctre.phoenix6.configs.CurrentLimitsConfigs; 010import com.ctre.phoenix6.signals.InvertedValue; 011import com.ctre.phoenix6.signals.NeutralModeValue; 012import edu.wpi.first.math.controller.ElevatorFeedforward; 013import edu.wpi.first.wpilibj.IterativeRobotBase; 014import tagalong.controls.FeedforwardConstants; 015import tagalong.controls.PIDSGVAConstants; 016import tagalong.devices.Motors; 017import tagalong.units.AccelerationUnits; 018import tagalong.units.DistanceUnits; 019import tagalong.units.MassUnits; 020import tagalong.units.VelocityUnits; 021 022/** 023 * Configuration for the elevator microsystem 024 */ 025public class ElevatorConf extends MicrosystemConf { 026 /** 027 * Units for the positional limit 028 */ 029 public final DistanceUnits positionalLimitUnit = DistanceUnits.METER; 030 /** 031 * Values for the positional minimum and maximum 032 */ 033 public final double positionalMin, positionalMax; 034 035 /** 036 * Unit for the drum diameter (default: meters) 037 */ 038 public final DistanceUnits drumDiameterUnit = DistanceUnits.METER; 039 /** 040 * Value of the drumDiameter 041 */ 042 public final double drumDiameter; 043 /** 044 * Value of the drum circumference 045 */ 046 public final double drumCircumference; 047 /** 048 * Feedforward model for the elevator 049 */ 050 public final ElevatorFeedforward feedForward; 051 052 /* -------- Simulation Specific Control -------- */ 053 /** 054 * Unit of the carriage mass (default: kilograms) 055 */ 056 public final MassUnits carriageMassUnit = MassUnits.KILOGRAMS; 057 /** 058 * Value of the carriage mass 059 */ 060 public final double carriageMassValue; 061 /** 062 * Dimension of the mechanical system 063 */ 064 public final double mech2dDim; 065 /** 066 * Sim root name 067 */ 068 public final String rootName; 069 /** 070 * Sim root x coordinate 071 */ 072 public final double rootX; 073 /** 074 * Sim root y coordinate 075 */ 076 public final double rootY; 077 /** 078 * Length of the line 079 */ 080 public final double lineLength; 081 /** 082 * Angle of the elevator 083 */ 084 public final double angle; 085 /** 086 * 087 */ 088 public static final FeedforwardConstants simFeedForward = 089 new FeedforwardConstants(0.0, 0.0, 0.0, 0.0); 090 091 /* --- Command Specific Controls --- */ 092 /** 093 * Power for zeroing the elevator 094 */ 095 public final double elevatorZeroingPower = 0.0; 096 /** 097 * Positional tolerance in meters for zeroing the elevator 098 */ 099 public final double elevatorZeroingStallToleranceM = 0.0; 100 /** 101 * Duration of stalling in seconds necessary for the elevator zero command to finish 102 */ 103 public final double elevatorZeroingDurationS = 0.0; 104 105 /** 106 *@param name name of the subsystem 107 * @param motorTypes array of motor types used 108 * @param motorDeviceIDs CAN IDs of the motors 109 * @param motorCanBus CAN buses which the motors are connected to 110 * @param motorDirection motor inversion settings 111 * @param motorEnabledBrakeMode brake mode when motors are enabled 112 * @param motorDisabledBrakeMode brake mode when motors are disabled 113 * @param gearRatio gear ratios 114 * @param positionalLimitsUnit units for positional limits on elevator 115 * @param positionalMin positional minimum on elevator 116 * @param positionalMax positional maximum on elevator 117 * @param trapezoidalLengthUnit units for trapezoidal motion length 118 * @param trapezoidalVelocityUnit units for trapezoidal motion velocityODO 119 * @param trapezoidalLimitsVelocity velocity limits for trapezoidal motion 120 * @param trapezoidalAccelerationUnit units for trapezoidal motion acceleration 121 * @param trapezoidalLimitsAcceleration acceleration limits for trapezoidal motion 122 * @param defaultTolerancesUnit default unit of the tolerance values 123 * @param defaultLowerTolerance default lower tolerance 124 * @param defaultUpperTolerance default upper tolerance 125 * @param feedForward feedforward constants 126 * @param simFeedForward simulated feedforward constants 127 * @param currentLimitsConfigs current limit configurations 128 * @param slot0 PID slot 0 configuration 129 * @param slot1 PID slot 1 configuration 130 * @param slot2 PID slot 2 configuration 131 * @param carriageMassUnit units for the carriage mass 132 * @param carriageMassValue value of the carriage mass 133 * @param mech2dDim dimensions of mechanical system 134 * @param rootX sim root x coordinate 135 * @param rootY sim root y coordinate 136 * @param rootName sim root name 137 * @param lineLength length of the line 138 * @param angle angle of the elevator 139 * @param simSlot0 simulated PID slot 0 configuration 140 * @param simSlot1 simulated PID slot 1 configuration 141 * @param simSlot2 simulated PID slot 2 configuration 142 * @param drumDiameterUnit units for the drum diameter 143 * @param drumDiameter diameter of the drum 144 * 145 */ 146 public ElevatorConf( 147 String name, 148 Motors[] motorTypes, 149 int[] motorDeviceIDs, 150 String[] motorCanBus, 151 InvertedValue[] motorDirection, 152 NeutralModeValue[] motorEnabledBrakeMode, 153 NeutralModeValue[] motorDisabledBrakeMode, 154 int[][] gearRatio, 155 DistanceUnits positionalLimitsUnit, 156 double positionalMin, 157 double positionalMax, 158 DistanceUnits trapezoidalLengthUnit, 159 VelocityUnits trapezoidalVelocityUnit, 160 double trapezoidalLimitsVelocity, 161 AccelerationUnits trapezoidalAccelerationUnit, 162 double trapezoidalLimitsAcceleration, 163 DistanceUnits defaultTolerancesUnit, 164 double defaultLowerTolerance, 165 double defaultUpperTolerance, 166 FeedforwardConstants feedForward, 167 FeedforwardConstants simFeedForward, 168 CurrentLimitsConfigs currentLimitsConfigs, 169 PIDSGVAConstants slot0, 170 PIDSGVAConstants slot1, 171 PIDSGVAConstants slot2, 172 MassUnits carriageMassUnit, 173 double carriageMassValue, 174 double mech2dDim, 175 String rootName, 176 double rootX, 177 double rootY, 178 double lineLength, 179 double angle, 180 PIDSGVAConstants simSlot0, 181 PIDSGVAConstants simSlot1, 182 PIDSGVAConstants simSlot2, 183 DistanceUnits drumDiameterUnit, 184 double drumDiameter 185 ) { 186 super( 187 name, 188 motorTypes, 189 motorDeviceIDs, 190 motorCanBus, 191 motorDirection, 192 motorEnabledBrakeMode, 193 motorDisabledBrakeMode, 194 gearRatio, 195 trapezoidalLengthUnit, 196 VelocityUnits.METERS_PER_SECOND, 197 trapezoidalVelocityUnit.convertX( 198 trapezoidalLimitsVelocity, VelocityUnits.METERS_PER_SECOND 199 ), 200 AccelerationUnits.METERS_PER_SECOND2, 201 trapezoidalAccelerationUnit.convertX( 202 trapezoidalLimitsAcceleration, AccelerationUnits.METERS_PER_SECOND2 203 ), 204 DistanceUnits.METER, 205 defaultTolerancesUnit.convertX(defaultLowerTolerance, DistanceUnits.METER), 206 defaultTolerancesUnit.convertX(defaultUpperTolerance, DistanceUnits.METER), 207 currentLimitsConfigs, 208 slot0, 209 slot1, 210 slot2, 211 simSlot0, 212 simSlot1, 213 simSlot2 214 ); 215 216 this.positionalMin = positionalLimitsUnit.convertX(positionalMin, this.positionalLimitUnit); 217 this.positionalMax = positionalLimitsUnit.convertX(positionalMax, this.positionalLimitUnit); 218 219 this.drumDiameter = drumDiameterUnit.convertX(drumDiameter, this.drumDiameterUnit); 220 drumCircumference = Math.PI * this.drumDiameter; 221 222 this.carriageMassValue = carriageMassUnit.convertX(carriageMassValue, this.carriageMassUnit); 223 this.mech2dDim = mech2dDim; 224 this.rootName = rootName; 225 this.rootX = rootX; 226 this.rootY = rootY; 227 this.lineLength = lineLength; 228 this.angle = angle; 229 this.feedForward = IterativeRobotBase.isReal() ? feedForward.getElevatorFeedforward() 230 : simFeedForward.getElevatorFeedforward(); 231 } 232}