A number of changes to accomodate some encoder stuff
This commit is contained in:
@@ -1,9 +0,0 @@
|
||||
package com.team2648.iocore.abstracts;
|
||||
|
||||
public abstract class BooleanSensorIO extends IOBase {
|
||||
public BooleanSensorIO(String sensorTypeName) {
|
||||
super("Sensors/Booleans/" + sensorTypeName);
|
||||
}
|
||||
|
||||
public abstract boolean get();
|
||||
}
|
||||
@@ -1,21 +0,0 @@
|
||||
package com.team2648.iocore.abstracts;
|
||||
|
||||
import edu.wpi.first.units.Unit;
|
||||
|
||||
public abstract class DoubleSensorIO<T extends Unit> extends IOBase {
|
||||
private T unitReference;
|
||||
|
||||
public DoubleSensorIO(String sensorTypeName, T unitReference) {
|
||||
super("Sensors/Doubles/" + sensorTypeName);
|
||||
|
||||
this.unitReference = unitReference;
|
||||
}
|
||||
|
||||
public abstract double get();
|
||||
|
||||
@SuppressWarnings("unchecked")
|
||||
public T getMeasure() {
|
||||
return (T) unitReference.of(get());
|
||||
}
|
||||
|
||||
}
|
||||
11
src/main/java/com/team2648/iocore/abstracts/EncoderIO.java
Normal file
11
src/main/java/com/team2648/iocore/abstracts/EncoderIO.java
Normal file
@@ -0,0 +1,11 @@
|
||||
package com.team2648.iocore.abstracts;
|
||||
|
||||
public abstract class EncoderIO extends IOBase {
|
||||
public EncoderIO(String encoderTypeName) {
|
||||
super("Encoders/" + encoderTypeName);
|
||||
}
|
||||
|
||||
public abstract double getPosition();
|
||||
public abstract double getVelocity();
|
||||
public abstract void setPosition(double position);
|
||||
}
|
||||
@@ -3,9 +3,10 @@ package com.team2648.iocore.abstracts;
|
||||
import java.util.List;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.team2648.iocore.config.CommonMotorConfig;
|
||||
import com.team2648.iocore.commons.CommonMotorControllerConfig;
|
||||
import com.team2648.iocore.enums.MotorControllerEncoderType;
|
||||
|
||||
public abstract class MotorIO extends IOBase {
|
||||
public abstract class MotorControllerIO extends IOBase {
|
||||
public enum ControlTypes {
|
||||
kDutyCycle,
|
||||
kPIDPosition,
|
||||
@@ -14,12 +15,12 @@ public abstract class MotorIO extends IOBase {
|
||||
|
||||
private List<ControlTypes> supportedControlTypes;
|
||||
|
||||
public MotorIO(String motorTypeName, ControlTypes... supportedControlTypes) {
|
||||
public MotorControllerIO(String motorTypeName, ControlTypes... supportedControlTypes) {
|
||||
this(motorTypeName, List.of(supportedControlTypes));
|
||||
}
|
||||
|
||||
public MotorIO(String motorTypeName, List<ControlTypes> supportedControlTypes) {
|
||||
super("Motor/" + motorTypeName);
|
||||
public MotorControllerIO(String motorTypeName, List<ControlTypes> supportedControlTypes) {
|
||||
super("/Motor/" + motorTypeName);
|
||||
|
||||
this.supportedControlTypes = supportedControlTypes;
|
||||
}
|
||||
@@ -30,11 +31,13 @@ public abstract class MotorIO extends IOBase {
|
||||
return supportedControlTypes.stream().anyMatch((t) -> t.equals(type));
|
||||
}
|
||||
|
||||
public abstract void applyConfig(CommonMotorConfig config);
|
||||
public abstract void applyConfig(CommonMotorControllerConfig config);
|
||||
|
||||
public void run(ControlTypes type, DoubleSupplier controlValueSource) {
|
||||
run(type, controlValueSource.getAsDouble());
|
||||
}
|
||||
|
||||
public abstract void run(ControlTypes type, double controlValue);
|
||||
|
||||
public abstract EncoderIO getEncoder(MotorControllerEncoderType type);
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
package com.team2648.iocore.commons;
|
||||
|
||||
public class CommonMotorControllerConfig {
|
||||
private boolean isInverted;
|
||||
|
||||
public CommonMotorControllerConfig() {
|
||||
|
||||
}
|
||||
|
||||
public boolean isInverted() {
|
||||
return isInverted;
|
||||
}
|
||||
|
||||
public CommonMotorControllerConfig setInverted(boolean inverted) {
|
||||
this.isInverted = inverted;
|
||||
|
||||
return this;
|
||||
}
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
package com.team2648.iocore.config;
|
||||
|
||||
public class CommonMotorConfig {
|
||||
private boolean isInverted;
|
||||
|
||||
public CommonMotorConfig() {
|
||||
|
||||
}
|
||||
|
||||
public boolean isInverted() {
|
||||
return isInverted;
|
||||
}
|
||||
|
||||
public CommonMotorConfig setInverted(boolean inverted) {
|
||||
this.isInverted = inverted;
|
||||
|
||||
return this;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
package com.team2648.iocore.enums;
|
||||
|
||||
public enum MotorControllerEncoderType {
|
||||
kInternal,
|
||||
kAbsolute,
|
||||
kExternal;
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
package com.team2648.iocore.realio;
|
||||
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.team2648.iocore.abstracts.EncoderIO;
|
||||
|
||||
public class REVLIBAbsoluteEncoderIO extends EncoderIO {
|
||||
private AbsoluteEncoder encoder;
|
||||
|
||||
public REVLIBAbsoluteEncoderIO(String name, AbsoluteEncoder encoder) {
|
||||
super("/REVLIB/Absolute/" + name);
|
||||
|
||||
this.encoder = encoder;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getPosition() {
|
||||
return encoder.getPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getVelocity() {
|
||||
return encoder.getVelocity();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setPosition(double position) {
|
||||
throw new UnsupportedOperationException("The position of an absolute encoder is not resettable");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
package com.team2648.iocore.realio;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.team2648.iocore.abstracts.EncoderIO;
|
||||
|
||||
public class REVLIBRelativeEncoderIO extends EncoderIO {
|
||||
private RelativeEncoder encoder;
|
||||
|
||||
public REVLIBRelativeEncoderIO(String name, RelativeEncoder encoder) {
|
||||
super("/REVLIB/Relative/" + name);
|
||||
|
||||
this.encoder = encoder;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getPosition() {
|
||||
return encoder.getPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getVelocity() {
|
||||
return encoder.getVelocity();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setPosition(double position) {
|
||||
encoder.setPosition(position);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,14 +1,16 @@
|
||||
package com.team2648.iocore.realio;
|
||||
|
||||
import com.team2648.iocore.abstracts.MotorIO;
|
||||
import com.team2648.iocore.config.CommonMotorConfig;
|
||||
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 com.team2648.iocore.interfaces.PWMFollowableMotor;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
||||
|
||||
public class VictorSPIO extends MotorIO implements PWMFollowableMotor{
|
||||
public class VictorSPIO extends MotorControllerIO implements PWMFollowableMotor{
|
||||
private VictorSP motor;
|
||||
|
||||
public VictorSPIO(String motorName, int pwmPort) {
|
||||
@@ -23,7 +25,7 @@ public class VictorSPIO extends MotorIO implements PWMFollowableMotor{
|
||||
}
|
||||
|
||||
@Override
|
||||
public void applyConfig(CommonMotorConfig config) {
|
||||
public void applyConfig(CommonMotorControllerConfig config) {
|
||||
motor.setInverted(config.isInverted());
|
||||
}
|
||||
|
||||
@@ -47,5 +49,9 @@ public class VictorSPIO extends MotorIO implements PWMFollowableMotor{
|
||||
public PWMMotorController getControllerObject() {
|
||||
return motor;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public EncoderIO getEncoder(MotorControllerEncoderType type) {
|
||||
throw new UnsupportedOperationException("VictorSP does not support any encoder type");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user