Motor config

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

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;}
 }

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 <TalonException extends Throwable> 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()) {

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<FollowerConfig> followers = new ArrayList<>();
+    private List<PIDControllerConfig> 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<PIDControllerConfig> getPidControllers() {
+        return pidControllers;
+    }
+
+    public Inverted getInverted() {
+        return inverted;
+    }
+
+    public List<FollowerConfig> getFollowers() {
+        return followers;
+    }
 }

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);

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
 }

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 = ;
+}

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;
 		}
 	}

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<TalonSRX> {
 
@@ -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());
 	}