A bunch of changes + the first Subsystem

src/main/java/com/team2648/iocore/abstracts/MotorControllerIO.java

@@ -6,6 +6,9 @@ import java.util.function.DoubleSupplier;
 import com.team2648.iocore.commons.CommonMotorControllerConfig;
 import com.team2648.iocore.enums.MotorControllerEncoderType;
 
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
 public abstract class MotorControllerIO extends IOBase {
     public enum ControlTypes {
         kDutyCycle,

src/main/java/com/team2648/iocore/commons/CommonEncoderConfig.java

@@ -0,0 +1,39 @@
+package com.team2648.iocore.commons;
+
+public class CommonEncoderConfig {
+    private double positionConversionFactor;
+    private double velocityConversionFactor;
+
+    public CommonEncoderConfig() {
+        this(1);
+    }
+
+    public CommonEncoderConfig(double positionConversionFactor) {
+        this(positionConversionFactor, 1);
+    }
+
+    public CommonEncoderConfig(double positionConversionFactor, double velocityConversionFactor) {
+        this.positionConversionFactor = positionConversionFactor;
+        this.velocityConversionFactor = velocityConversionFactor;
+    }
+
+    public double getPositionConversionFactor() {
+        return positionConversionFactor;
+    }
+
+    public double getVelocityConversionFactor() {
+        return velocityConversionFactor;
+    }
+
+    public CommonEncoderConfig setPositionConversionFactor(double positionConversionFactor) {
+        this.positionConversionFactor = positionConversionFactor;
+
+        return this;
+    }
+
+    public CommonEncoderConfig setVelocityConversionFactor(double velocityConversionFactor) {
+        this.velocityConversionFactor = velocityConversionFactor;
+
+        return this;
+    }
+}

src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java

@@ -1,7 +1,11 @@
 package com.team2648.iocore.commons;
 
+import java.util.HashMap;
+import java.util.Optional;
+
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.team2648.iocore.enums.MotorControllerEncoderType;
 import com.team2648.iocore.enums.MotorIdleMode;
 
 public class CommonMotorControllerConfig {
@@ -12,12 +16,15 @@ public class CommonMotorControllerConfig {
 
     private MotorIdleMode mode;
 
+    private HashMap<MotorControllerEncoderType, CommonEncoderConfig> encoderConfigs;
 
     public CommonMotorControllerConfig() {
         isInverted = false;
         supplyCurrentLimit = -1;
         followCANID = -1;
         mode = MotorIdleMode.kCoast;
+
+        encoderConfigs = new HashMap<MotorControllerEncoderType, CommonEncoderConfig>();
     }
 
     public boolean isInverted() {
@@ -36,6 +43,14 @@ public class CommonMotorControllerConfig {
         return mode;
     }
 
+    public Optional<CommonEncoderConfig> getEncoderConfig(MotorControllerEncoderType type) {
+        if(encoderConfigs.containsKey(type)) {
+            return Optional.of(encoderConfigs.get(type));
+        } else {
+            return Optional.empty();
+        }
+    }
+
     public CommonMotorControllerConfig setInverted(boolean inverted) {
         this.isInverted = inverted;
 
@@ -60,6 +75,44 @@ public class CommonMotorControllerConfig {
         return this;
     }
 
+    public CommonMotorControllerConfig setEncoderConfig(MotorControllerEncoderType type, CommonEncoderConfig config) {
+        encoderConfigs.put(type, config);
+
+        return this;
+    }
+
+    public CommonMotorControllerConfig setEncoderPositionConversionFactor(MotorControllerEncoderType type, double positionConversionFactor) {
+        if(!encoderConfigs.containsKey(type)) {
+            encoderConfigs.put(type, new CommonEncoderConfig());
+        }
+
+        encoderConfigs.get(type).setPositionConversionFactor(positionConversionFactor);
+
+        return this;
+    }
+
+    public CommonMotorControllerConfig setEncoderVelocityConversionFactor(MotorControllerEncoderType type, double velocityConversionFactor) {
+        if(!encoderConfigs.containsKey(type)) {
+            encoderConfigs.put(type, new CommonEncoderConfig());
+        }
+
+        encoderConfigs.get(type).setVelocityConversionFactor(velocityConversionFactor);
+
+        return this;
+    }
+
+    public CommonMotorControllerConfig setEncoderConversionFactors(MotorControllerEncoderType type, double positionConversionFactor, double velocityConversionFactor) {
+        if(!encoderConfigs.containsKey(type)) {
+            encoderConfigs.put(type, new CommonEncoderConfig());
+        }
+
+        encoderConfigs.get(type)
+            .setPositionConversionFactor(positionConversionFactor)
+            .setVelocityConversionFactor(velocityConversionFactor);
+
+        return this;
+    }
+
     public SparkMaxConfig getAsSparkMaxConfig() {
         // TODO More config
         SparkMaxConfig config = new SparkMaxConfig();
@@ -73,7 +126,25 @@ public class CommonMotorControllerConfig {
         if(followCANID >= 0) {
             config.follow(followCANID);
         }
-        
+
+        if(encoderConfigs.containsKey(MotorControllerEncoderType.kInternal)) {
+            config.encoder
+                .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kInternal).getPositionConversionFactor())
+                .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kInternal).getVelocityConversionFactor());
+        }
+
+        if(encoderConfigs.containsKey(MotorControllerEncoderType.kAbsolute)) {
+            config.absoluteEncoder
+                .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kAbsolute).getPositionConversionFactor())
+                .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kAbsolute).getVelocityConversionFactor());
+        }
+
+        if(encoderConfigs.containsKey(MotorControllerEncoderType.kExternal)) {
+            config.alternateEncoder
+                .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kExternal).getPositionConversionFactor())
+                .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kExternal).getVelocityConversionFactor());
+        }
+
         return config;
     }
 }

src/main/java/com/team2648/iocore/realio/SparkMaxIO.java

@@ -79,6 +79,5 @@ public class SparkMaxIO extends MotorControllerIO {
             default:
                 throw new UnsupportedOperationException("SparkMaxIO does not support MotorControllerEncoderType " + type.name());
         }
-    }
-    
+    } 
 }

src/main/java/com/team2648/iocore/subsystems/SimpleDifferentialDrivetrain.java

@@ -0,0 +1,43 @@
+package com.team2648.iocore.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import com.team2648.iocore.abstracts.MotorControllerIO;
+import com.team2648.iocore.abstracts.MotorControllerIO.ControlTypes;
+
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SimpleDifferentialDrivetrain extends SubsystemBase {
+    private MotorControllerIO leftIO;
+    private MotorControllerIO rightIO;
+
+    private DifferentialDrive drive;
+
+    public SimpleDifferentialDrivetrain(MotorControllerIO leftIO, MotorControllerIO rightIO) {
+        this.leftIO = leftIO;
+        this.rightIO = rightIO;
+
+        drive = new DifferentialDrive(
+            (d) -> leftIO.run(ControlTypes.kDutyCycle, d), 
+            (d) -> rightIO.run(ControlTypes.kDutyCycle, d)
+        );
+    }
+
+    public Command driveArcade(DoubleSupplier xSpeed, DoubleSupplier zRotation) {
+        return run(() -> {
+            drive.arcadeDrive(xSpeed.getAsDouble(), zRotation.getAsDouble());
+        });
+    }
+
+    public Command driveTank(DoubleSupplier leftXSpeed, DoubleSupplier rightXSpeed) {
+        return run(() -> {
+            drive.tankDrive(leftXSpeed.getAsDouble(), rightXSpeed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return driveTank(() -> 0, () -> 0);
+    }
+}