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.configs.TalonFXConfiguration; 011import com.ctre.phoenix6.signals.InvertedValue; 012import com.ctre.phoenix6.signals.NeutralModeValue; 013import edu.wpi.first.math.trajectory.TrapezoidProfile; 014import edu.wpi.first.wpilibj.IterativeRobotBase; 015import tagalong.controls.PIDSGVAConstants; 016import tagalong.devices.Motors; 017import tagalong.units.AccelerationUnits; 018import tagalong.units.DistanceUnits; 019import tagalong.units.VelocityUnits; 020 021/** 022 * Configuration for a microsystem 023 */ 024public class MicrosystemConf { 025 // Generalized to every motorized system 026 // ff configurations 027 // primary motor full configurations 028 // follower motor type, ids, canbus, and direction 029 // ---- otherwise assume all control is the same as primary 030 // mechanism gear ratio 031 // trapezoidal limits 032 // default tolerances 033 // sim motor configs 034 035 /** 036 * Full microsystem name, default is subsystem name followed by microsystem name 037 */ 038 public final String name; 039 040 /** 041 * Number of motors used by microsystem, also the length of the following arrays 042 */ 043 public final int numMotors; 044 /** 045 * The type of each motor in the microsystem 046 */ 047 public final Motors[] motorTypes; 048 /** 049 * The can ids of the motors 050 */ 051 public final int[] motorDeviceIDs; 052 /** 053 * The can bus of the motors 054 */ 055 public final String[] motorCanBus; 056 /** 057 * The motor directionality 058 */ 059 public final InvertedValue[] motorDirection; 060 /** 061 * Motor brake mode while robot is enabled 062 */ 063 public final NeutralModeValue[] motorEnabledBrakeMode; 064 /** 065 * Motor brake mode while robot is disabled 066 */ 067 public final NeutralModeValue[] motorDisabledBrakeMode; 068 /** 069 * Net gear ratio of they system motor to mechanism 070 */ 071 public final double motorToMechRatio; 072 073 @SuppressWarnings("unused") 074 /** 075 * Unit of trapezoidal motion length 076 */ 077 private final DistanceUnits trapezoidalLengthUnit; 078 /** 079 * Unit of trapezoidal motion velocity 080 */ 081 public final VelocityUnits trapezoidalVelocityUnit; 082 /** 083 * Unit of trapezoidal motion acceleration 084 */ 085 public final AccelerationUnits trapezoidalAccelerationUnit; 086 /** 087 * Trapezoidal motion velocity limits 088 */ 089 public final double trapezoidalLimitsVelocity; 090 /** 091 * Trapezoidal motion acceleration limits 092 */ 093 public final double trapezoidalLimitsAcceleration; 094 /** 095 * Trapezoidal constraints 096 */ 097 public final TrapezoidProfile.Constraints trapezoidalLimits; 098 /** 099 * Unit of default tolerances 100 */ 101 public final DistanceUnits defaultTolerancesUnit; 102 /** 103 * Default lower tolerance 104 */ 105 public final double defaultLowerTolerance; 106 /** 107 * Default upper tolerance 108 */ 109 public final double defaultUpperTolerance; 110 /** 111 * Motor configurations 112 */ 113 public final TalonFXConfiguration[] motorConfig; 114 115 /** 116 * 117 * @param name microsystem name 118 * @param motorTypes motor types 119 * @param motorDeviceIDs motor device IDs 120 * @param motorCanBus motor can bus 121 * @param motorDirection motor directions 122 * @param motorEnabledBrakeMode enabled brake mode for the motor 123 * @param motorDisabledBrakeMode disabled brake mode for the motor 124 * @param gearRatio gear ratio 125 * @param trapezoidalLengthUnit unit for trapezoidal length 126 * @param trapezoidalVelocityUnit unit for trapezoidal velocity 127 * @param trapezoidalLimitsVelocity trapezoidal velocity limits 128 * @param trapezoidalAccelerationUnit unit for trapezoidal acceleration 129 * @param trapezoidalLimitsAcceleration trapezoidal acceleration limits 130 * @param defaultTolerancesUnit unit for default tolerances 131 * @param defaultLowerTolerance default lower tolerance 132 * @param defaultUpperTolerance default upper tolerance 133 * @param currentLimitsConfigs configurations for current limits 134 * @param slot0 slot 0 configurations 135 * @param slot1 slot 1 configurations 136 * @param slot2 slot 2 configurations 137 * @param simSlot0 simulation slot 0 configurations 138 * @param simSlot1 simulation slot 1 configurations 139 * @param simSlot2 simulation slot 2 configurations 140 */ 141 public MicrosystemConf( 142 String name, 143 Motors[] motorTypes, 144 int[] motorDeviceIDs, 145 String[] motorCanBus, 146 InvertedValue[] motorDirection, 147 NeutralModeValue[] motorEnabledBrakeMode, 148 NeutralModeValue[] motorDisabledBrakeMode, 149 int[][] gearRatio, 150 DistanceUnits trapezoidalLengthUnit, 151 VelocityUnits trapezoidalVelocityUnit, 152 double trapezoidalLimitsVelocity, 153 AccelerationUnits trapezoidalAccelerationUnit, 154 double trapezoidalLimitsAcceleration, 155 DistanceUnits defaultTolerancesUnit, 156 double defaultLowerTolerance, 157 double defaultUpperTolerance, 158 CurrentLimitsConfigs currentLimitsConfigs, 159 PIDSGVAConstants slot0, 160 PIDSGVAConstants slot1, 161 PIDSGVAConstants slot2, 162 PIDSGVAConstants simSlot0, 163 PIDSGVAConstants simSlot1, 164 PIDSGVAConstants simSlot2 165 ) { 166 this.name = name; 167 168 this.motorTypes = motorTypes; 169 this.motorDeviceIDs = motorDeviceIDs; 170 this.motorCanBus = motorCanBus; 171 this.motorDirection = motorDirection; 172 this.motorEnabledBrakeMode = motorEnabledBrakeMode; 173 this.motorDisabledBrakeMode = motorDisabledBrakeMode; 174 175 numMotors = motorTypes.length; 176 assert (numMotors == motorDeviceIDs.length); 177 assert (numMotors == motorCanBus.length); 178 assert (numMotors == motorDirection.length); 179 assert (numMotors == motorEnabledBrakeMode.length); 180 assert (numMotors == motorDisabledBrakeMode.length); 181 182 this.motorToMechRatio = calculateGearRatio(gearRatio); 183 assert (this.motorToMechRatio > 0.0); 184 185 motorConfig = new TalonFXConfiguration[numMotors]; 186 for (int i = 0; i < numMotors; i++) { 187 motorConfig[i] = Motors.getDefaults(); 188 motorConfig[i].MotorOutput.Inverted = motorDirection[i]; 189 motorConfig[i].MotorOutput.NeutralMode = motorDisabledBrakeMode[i]; 190 motorConfig[i].Slot0 = IterativeRobotBase.isReal() ? slot0.toCTRESlot0Configuration() 191 : simSlot0.toCTRESlot0Configuration(); 192 motorConfig[i].Slot1 = IterativeRobotBase.isReal() ? slot1.toCTRESlot1Configuration() 193 : simSlot1.toCTRESlot1Configuration(); 194 motorConfig[i].Slot2 = IterativeRobotBase.isReal() ? slot2.toCTRESlot2Configuration() 195 : simSlot2.toCTRESlot2Configuration(); 196 motorConfig[i].CurrentLimits = currentLimitsConfigs; 197 } 198 199 this.trapezoidalLengthUnit = trapezoidalLengthUnit; 200 this.trapezoidalVelocityUnit = trapezoidalVelocityUnit; 201 this.trapezoidalAccelerationUnit = trapezoidalAccelerationUnit; 202 this.trapezoidalLimitsVelocity = trapezoidalLimitsVelocity; 203 this.trapezoidalLimitsAcceleration = trapezoidalLimitsAcceleration; 204 this.trapezoidalLimits = 205 new TrapezoidProfile.Constraints(trapezoidalLimitsVelocity, trapezoidalLimitsAcceleration); 206 207 this.defaultTolerancesUnit = defaultTolerancesUnit; 208 this.defaultLowerTolerance = defaultLowerTolerance; 209 this.defaultUpperTolerance = defaultUpperTolerance; 210 } 211 212 /** 213 * @param gearPairs Pairs of gear teeth representing the physical path to the motor. e.g if the 214 * motor is using a 14-tooth pinion to a 60-tooth gear, the input should be [[14, 60]] 215 * 216 * @return The resulting gear ratio coefficient. 217 * A reduction (target spinning slower than motor) results in a value g.t. 1 218 * An up-duction (target spinning faster than motor) will result in a value l.t. 1 219 */ 220 public static double calculateGearRatio(int[][] gearPairs) { 221 double ratio = 1.0; 222 for (int i = 0; i < gearPairs.length; i++) { 223 assert (gearPairs[i].length == 2); 224 ratio = ratio * gearPairs[i][1] / gearPairs[i][0]; 225 } 226 return ratio; 227 } 228}