diff --git a/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java b/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java index 6f5c1f5..baee5a2 100644 --- a/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java +++ b/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java @@ -1,5 +1,7 @@ package com.team2648.iocore.commons; +import com.revrobotics.spark.config.SparkMaxConfig; + public class CommonMotorControllerConfig { private boolean isInverted; @@ -16,4 +18,11 @@ public class CommonMotorControllerConfig { return this; } + + public SparkMaxConfig getAsSparkMaxConfig() { + // TODO More config + SparkMaxConfig config = new SparkMaxConfig(); + config.inverted(isInverted); + return config; + } } diff --git a/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java b/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java new file mode 100644 index 0000000..40b1116 --- /dev/null +++ b/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java @@ -0,0 +1,84 @@ +package com.team2648.iocore.realio; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.team2648.iocore.abstracts.EncoderIO; +import com.team2648.iocore.abstracts.MotorControllerIO; +import com.team2648.iocore.commons.CommonMotorControllerConfig; +import com.team2648.iocore.enums.MotorControllerEncoderType; + +import edu.wpi.first.wpilibj.RobotController; + +public class SparkMaxIO extends MotorControllerIO { + private SparkMax motor; + + private String name; + + public SparkMaxIO(String name, int canID, MotorType type) { + super( + "/SparkMax/" + name, + ControlTypes.kDutyCycle, + ControlTypes.kPIDPosition, + ControlTypes.kPIDVelocity + ); + + motor = new SparkMax(canID, type); + + this.name = name; + } + + @Override + public double getOutputVoltage() { + return motor.getAppliedOutput() * RobotController.getBatteryVoltage(); + } + + @Override + public void applyConfig(CommonMotorControllerConfig config) { + motor.configure( + config.getAsSparkMaxConfig(), + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + } + + @Override + public void run(ControlTypes type, double controlValue) { + switch (type) { + case kDutyCycle: + motor.set(controlValue); + break; + case kPIDPosition: + motor.getClosedLoopController().setSetpoint( + controlValue, + ControlType.kPosition + ); + break; + case kPIDVelocity: + motor.getClosedLoopController().setSetpoint( + controlValue, + ControlType.kVelocity + ); + break; + default: + throw new UnsupportedOperationException("SparkMaxIO does not support ControlType " + type.name()); + } + } + + @Override + public EncoderIO getEncoder(MotorControllerEncoderType type) { + switch(type) { + case kInternal: + return new REVLIBRelativeEncoderIO(name, motor.getEncoder()); + case kAbsolute: + return new REVLIBAbsoluteEncoderIO(name, motor.getAbsoluteEncoder()); + case kExternal: + return new REVLIBRelativeEncoderIO(name, motor.getAlternateEncoder()); + default: + throw new UnsupportedOperationException("SparkMaxIO does not support MotorControllerEncoderType " + type.name()); + } + } + +} diff --git a/src/main/java/com/team2648/iocore/realio/VictorSPIO.java b/src/main/java/com/team2648/iocore/realio/VictorSPIO.java index eb1d6ef..c6b5ec3 100644 --- a/src/main/java/com/team2648/iocore/realio/VictorSPIO.java +++ b/src/main/java/com/team2648/iocore/realio/VictorSPIO.java @@ -14,7 +14,7 @@ public class VictorSPIO extends MotorControllerIO implements PWMFollowableMotor{ private VictorSP motor; public VictorSPIO(String motorName, int pwmPort) { - super("/" + motorName, ControlTypes.kDutyCycle); + super("/VictorSP/" + motorName, ControlTypes.kDutyCycle); motor = new VictorSP(pwmPort); }