diff --git a/OffSeasonBot2019/gradle/wrapper/gradle-wrapper.properties b/OffSeasonBot2019/gradle/wrapper/gradle-wrapper.properties index c680a16..c79fd27 100644 --- a/OffSeasonBot2019/gradle/wrapper/gradle-wrapper.properties +++ b/OffSeasonBot2019/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sat Dec 28 11:57:23 PST 2019 +distributionUrl=https\://services.gradle.org/distributions/gradle-5.6.2-all.zip distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-5.6.2-bin.zip -zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists +zipStoreBase=GRADLE_USER_HOME diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/Inverted.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/Inverted.java index 6cbf2eb..302cced 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/Inverted.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/Inverted.java @@ -1,6 +1,11 @@ package com.team2813.lib.config; +import com.ctre.phoenix.motorcontrol.InvertType; + @SuppressWarnings("unused") public enum Inverted { - NONINVERTED, INVERTED, FOLLOW_LEADER; + NONINVERTED(InvertType.None), INVERTED(InvertType.InvertMotorOutput), + FOLLOW_LEADER(InvertType.FollowMaster), OPPOSE_LEADER(InvertType.OpposeMaster); + InvertType value; + Inverted(InvertType value){this.value = value;} } diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/MotorConfigs.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/MotorConfigs.java index a0963b8..7e7233c 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/MotorConfigs.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/MotorConfigs.java @@ -4,8 +4,12 @@ import com.fasterxml.jackson.dataformat.yaml.YAMLFactory; import com.team2813.lib.sparkMax.CANSparkMaxWrapper; import com.team2813.lib.sparkMax.SparkMaxException; +import com.team2813.lib.talon.CTREException; +import com.team2813.lib.talon.TalonWrapper; import com.team2813.lib.talon.VictorWrapper; +import com.team2813.lib.talon.PIDProfile; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Talon; import java.io.File; import java.io.IOException; @@ -37,6 +41,73 @@ public static void read() throws IOException { System.out.println("Successful!"); } + private static TalonWrapper initializeTalon(TalonConfig config) throws TalonException, CTREException, SparkMaxException { + for (Integer id : ids) + if (id == config.getDeviceNumber()){ + System.err.println("Tried to register talon with already used id"); + } + ids.add(config.getDeviceNumber()); + + System.out.println("Configuring" + config.getSubsystemName()); + + TalonWrapper talon = new TalonWrapper(config.getDeviceNumber(), config.getSubsystemName(), config.getMotorType().getValue()); + + talon.setFactoryDefaults(); + +// talon.setPeakCurrentDuration(config.getPeakCurrentDuration()); + talon.setCurrLimit(config.getPeakCurrentLimit()); + talon.enableVoltageCompensation(); + + + talon.setOpenLoopRamp(config.getOpenLoopRampRate()); + talon.setClosedLoopRamp(config.getClosedLoopRampRate()); + + talon.setPeriodicFrame(config.getStatusFrame(), config.getStatusFramePeriod()); +// talon.setSmartMotionMaxVelocity(config.motionCruiseVelocity()); // FIXME: 09/20/2019 need to change parameters/types +// talon.setSmartMotionMaxAccel(config.motionAcceleration()); // FIXME: 09/20/2019 need to change parameters/types + + talon.setSecondaryCurrLimit(config.getContinuousCurrentLimitAmps());// TODO check this is actually continuous limit + +// for (com.team2813.lib.talon.options.HardLimitSwitch hardLimitSwitch : field.getAnnotationsByType(com.team2813.lib.talon.options.HardLimitSwitch.class)) { +// System.out.println("\tconfiguring hard limit switch " + hardLimitSwitch.direction()); +// // FIXME remake limit switch stuff differently since it is called differently -- Grady 10/30 I'm not sure this is how it works for Spark Maxs +// } +// +// for (com.team2813.lib.talon.options.SoftLimit softLimit : field.getAnnotationsByType(com.team2813co.lib.talon.options.SoftLimit.class)) { +// System.out.println("\tconfiguring soft limit " + softLimit.direction()); +// +// //FIXME remake limit switch stuff differently +// } + + + for (PIDControllerConfig pidController : config.getPidControllers()) { + PIDProfile.Profile slotID = config.getPidControllers().indexOf(pidController) == 0 ? + PIDProfile.Profile.PRIMARY : PIDProfile.Profile.SECONDARY; + talon.setPIDF(slotID, pidController.getP(), pidController.getI(), + pidController.getD(), pidController.getF()); + talon.setMotionMagicCruiseVelocity((int) pidController.getMaxVelocity()); // FIXME: 1/3/2020 Casting because + // talon uses encoder ticks + // TODO deal with units issue + talon.setMotionMagicAcceleration((int) pidController.getMaxAcceleration()); // FIXME see above + // TODO: 1/3/2020 figure out min velocity with Talons / remove from PID controller so as not to have that attribute + } + + + Inverted inverted = config.getInverted(); + if (inverted != null) + talon.setInverted(inverted == Inverted.INVERTED); + + for (FollowerConfig followerConfig : config.getFollowers()) { + System.out.println( + "\tCreating follower w/ id of " + followerConfig.getId() + " on " + config.getSubsystemName() + ); + CANSparkMaxWrapper talonFollower = new CANSparkMaxWrapper(followerConfig.getId(), followerConfig.getType().getValue()); + talonFollower.follow(talon, followerConfig.getInverted().inverted); + } + + return talon; + } + private static CANSparkMaxWrapper initializeSpark(SparkConfig config) { for (Integer id : ids) if (id == config.getDeviceNumber()) { diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/TalonConfig.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/TalonConfig.java index a98c853..b62c918 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/config/TalonConfig.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/config/TalonConfig.java @@ -1,4 +1,160 @@ package com.team2813.lib.config; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod; +import com.team2813.lib.talon.BaseMotorControllerWrapper; +import java.util.ArrayList; +import java.util.List; + public class TalonConfig extends MotorConfig { + + private int deviceNumber; + private int peakCurrentDuration; + private int peakCurrentLimit; + private boolean enableVoltageCompensation; + private int compSaturationVoltage; + private int continuousCurrentLimitAmps; + private int motionAcceleration; + private int motionCruiseVelocity; + private double closedLoopRampRate; + private double openLoopRampRate; + private boolean invertSensorPhase; + private Inverted inverted; + private List followers = new ArrayList<>(); + private List pidControllers; + private PeriodicFrame statusFrame; // cannot serialize into PeriodicFrame (see getStatusFrame) + private int statusFramePeriod = 5; + private VelocityMeasPeriod velocityMeasurementPeriod; + private BaseMotorControllerWrapper.VelocityMeasurementWindow velocityMeasurementWindow; + + @Override + public int getDeviceNumber() { + return deviceNumber; + } + + @Override + public void setDeviceNumber(int deviceNumber) { + this.deviceNumber = deviceNumber; + } + + public int getPeakCurrentDuration() { + return peakCurrentDuration; + } + + public void setPeakCurrentDuration(int peakCurrentDuration) { + this.peakCurrentDuration = peakCurrentDuration; + } + + public int getPeakCurrentLimit() { + return peakCurrentLimit; + } + + public void setPeakCurrentLimit(int peakCurrentLimit) { + this.peakCurrentLimit = peakCurrentLimit; + } + + public boolean isEnableVoltageCompensation() { + return enableVoltageCompensation; + } + + public void setEnableVoltageCompensation(boolean enableVoltageCompensation) { + this.enableVoltageCompensation = enableVoltageCompensation; + } + + public int getCompSaturationVoltage() { + return compSaturationVoltage; + } + + public void setCompSaturationVoltage(int compSaturationVoltage) { + this.compSaturationVoltage = compSaturationVoltage; + } + + public int getContinuousCurrentLimitAmps() { + return continuousCurrentLimitAmps; + } + + public void setContinuousCurrentLimitAmps(int continuousCurrentLimitAmps) { + this.continuousCurrentLimitAmps = continuousCurrentLimitAmps; + } + + public int getMotionAcceleration() { + return motionAcceleration; + } + + public void setMotionAcceleration(int motionAcceleration) { + this.motionAcceleration = motionAcceleration; + } + + public int getMotionCruiseVelocity() { + return motionCruiseVelocity; + } + + public void setMotionCruiseVelocity(int motionCruiseVelocity) { + this.motionCruiseVelocity = motionCruiseVelocity; + } + + public double getClosedLoopRampRate() { + return closedLoopRampRate; + } + + public void setClosedLoopRampRate(double closedLoopRampRate) { + this.closedLoopRampRate = closedLoopRampRate; + } + + public double getOpenLoopRampRate() { + return openLoopRampRate; + } + + public void setOpenLoopRampRate(double openLoopRampRate) { + this.openLoopRampRate = openLoopRampRate; + } + + public boolean isInvertSensorPhase() { + return invertSensorPhase; + } + + public void setInvertSensorPhase(boolean invertSensorPhase) { + this.invertSensorPhase = invertSensorPhase; + } + + public PeriodicFrame getStatusFrame() { + return statusFrame; + } + + public void setStatusFrame(PeriodicFrame statusFrame) { + this.statusFrame = statusFrame; + } + + public int getStatusFramePeriod() { + return statusFramePeriod; + } + + public void setStatusFramePeriod(int statusFramePeriod) { + this.statusFramePeriod = statusFramePeriod; + } + + public VelocityMeasPeriod getVelocityMeasurementPeriod() { + return velocityMeasurementPeriod; + } + + public void setVelocityMeasurementPeriod(VelocityMeasPeriod velocityMeasurementPeriod) { + this.velocityMeasurementPeriod = velocityMeasurementPeriod; + } + + public BaseMotorControllerWrapper.VelocityMeasurementWindow getVelocityMeasurementWindow() { + return velocityMeasurementWindow; + } + + + public List getPidControllers() { + return pidControllers; + } + + public Inverted getInverted() { + return inverted; + } + + public List getFollowers() { + return followers; + } } diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/sparkMax/CANSparkMaxWrapper.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/sparkMax/CANSparkMaxWrapper.java index 793c07d..a4d3150 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/sparkMax/CANSparkMaxWrapper.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/sparkMax/CANSparkMaxWrapper.java @@ -7,6 +7,7 @@ import com.team2813.lib.talon.TalonWrapper; import com.team2813.lib.talon.VictorWrapper; + public class CANSparkMaxWrapper extends CANSparkMax { public String subsystemName; @@ -517,15 +518,15 @@ public double getPIDOutputMax(int slotID) { return getPIDController().getOutputMax(slotID); } - public void setSmartMotionMaxVelocity(double maxVel, int slotID) throws SparkMaxException { + public void setSmartMotionMaxVelocity(double maxVel, PIDProfile.Profile slotID) throws SparkMaxException { throwIfNotOk(getPIDController().setSmartMotionMaxVelocity(maxVel, slotID)); } - public void setSmartMotionMaxAccel(double maxAccel, int slotID) throws SparkMaxException { + public void setSmartMotionMaxAccel(double maxAccel, PIDProfile.Profile slotID) throws SparkMaxException { throwIfNotOk(getPIDController().setSmartMotionMaxAccel(maxAccel, slotID)); } - public void setSmartMotionMinOutputVelocity(double minVel, int slotID) throws SparkMaxException { + public void setSmartMotionMinOutputVelocity(double minVel, PIDProfile.Profile slotID) throws SparkMaxException { throwIfNotOk(getPIDController().setSmartMotionMinOutputVelocity(minVel, slotID)); } @@ -589,6 +590,10 @@ public void setReverseSoftLimit(double position) throws SparkMaxException { setSoftLimit(false, position); } + public void follow(TalonWrapper talon, boolean inverted) { + } + + //#endregion public void setInverted(Inverted isInverted) { if (isInverted == Inverted.INVERTED) super.setInverted(true); diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/BaseMotorControllerWrapper.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/BaseMotorControllerWrapper.java index dd102ee..161e185 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/BaseMotorControllerWrapper.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/BaseMotorControllerWrapper.java @@ -7,6 +7,9 @@ import com.ctre.phoenix.motion.TrajectoryPoint; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.team2813.lib.config.PeriodicFrame; +import com.team2813.lib.sparkMax.CANSparkMaxWrapper; + /** * @author Adrian Guerra @@ -101,9 +104,10 @@ public void setSensorPhaseInverted(boolean inverted) throws CTREException { * @throws CTREException * @see BaseMotorController#setInverted(InvertType) * @see InvertType + * @param type */ - public void setInverted(InvertType type) throws CTREException { - invertType = type; + public void setInverted(boolean type) throws CTREException { + boolean invertType = type; motorController.setInverted(type); throwLastError(); } @@ -785,5 +789,9 @@ private VelocityMeasurementWindow(int value) { } } + public void setPeriodicFrame(PeriodicFrame frameID, int periodMs){ + + } + // #endregion } diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/InvertType.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/InvertType.java new file mode 100644 index 0000000..9bf4cb2 --- /dev/null +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/InvertType.java @@ -0,0 +1,5 @@ +package com.team2813.lib.talon.options; + +public class InvertType { + public static final Object NORMAL = ; +} diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/PIDProfile.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/PIDProfile.java index 8d8ff7d..1d2e6e2 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/PIDProfile.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/PIDProfile.java @@ -20,7 +20,7 @@ public enum Profile { public final int id; - private Profile(int id) { + Profile(int id) { this.id = id; } } diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/TalonWrapper.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/TalonWrapper.java index a2a2cf7..88d8601 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/TalonWrapper.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/talon/TalonWrapper.java @@ -5,6 +5,8 @@ import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMaxLowLevel; + public class TalonWrapper extends BaseMotorControllerWrapper { @@ -20,7 +22,11 @@ public TalonWrapper(int deviceNumber) { this(deviceNumber, ""); } - public SensorCollection getSensorCollection() throws CTREException { + public TalonWrapper(int deviceNumber, String subsystemName, CANSparkMaxLowLevel.MotorType value) { + super(); + } + + public SensorCollection getSensorCollection() throws CTREException { return throwIfNotOkElseReturn(motorController.getSensorCollection()); }