diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java index 290820d..7e60123 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -1,5 +1,6 @@ package org.usfirst.frc4388.utility; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; @@ -32,35 +33,47 @@ public class CANTalonEncoder extends WPI_TalonSRX this.isRight = isRight; } - public double convertEncoderTicksToWorld(double encoderTicks) { + public double convertEncoderTicksToWorld(double encoderTicks) + { return encoderTicks / encoderTicksToWorld; + } + + public void setWorld(ControlMode mode, double worldValue) + { + this.set(mode, convertEncoderWorldToTicks(worldValue)); } - public double convertEncoderWorldToTicks(double worldValue) { + public double convertEncoderWorldToTicks(double worldValue) + { return worldValue * encoderTicksToWorld; } - public void setWorld(double worldValue) { + public void setWorld(double worldValue) + { //this.set(convertEncoderWorldToTicks(worldValue)); this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set? } - public void setPositionWorld(double worldValue) { + public void setPositionWorld(double worldValue) + { //this.setPosition(convertEncoderWorldToTicks(worldValue)); this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify } - public double getPositionWorld() { + public double getPositionWorld() + { //return convertEncoderTicksToWorld(this.getPosition()); return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" } - public void setVelocityWorld(double worldValue) { + public void setVelocityWorld(double worldValue) + { //this.set(convertEncoderWorldToTicks(worldValue) * 0.1); this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set? } - public double getVelocityWorld() { + public double getVelocityWorld() + { //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" }