From 1b24f7b0ed5e3a774ba49f357dc3fe25748d3571 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sat, 26 Jan 2019 15:52:00 -0700 Subject: [PATCH] Arm added, All IDS updated --- .../org/usfirst/frc4388/robot/Constants.java | 1 + .../org/usfirst/frc4388/robot/RobotMap.java | 22 +- .../frc4388/robot/commands/ArmBasic.java | 99 ------ .../usfirst/frc4388/robot/subsystems/Arm.java | 298 +++++++++--------- .../org/usfirst/frc4388/utility/Loop.java | 44 +++ .../frc4388/utility/TalonSRXEncoder.java | 84 +++++ 6 files changed, 302 insertions(+), 246 deletions(-) delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java index c10bb0d..6319a55 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -9,6 +9,7 @@ package org.usfirst.frc4388.robot; public class Constants { + public static double kLooperDt = 0.01; public static double kDriveWheelDiameterInches = 6.04; public static double kTrackLengthInches = 10; public static double kTrackWidthInches = 26.5; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 388444e..d13fe44 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -18,19 +18,29 @@ public class RobotMap { public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; //carriage motors - public static final int WRIST_LEFT_MOTOR_CAN_ID = 8; - public static final int WRITST_RIGHT_MOTOR_CAN_ID = 9; + public static final int WRIST_LEFT_MOTOR_CAN_ID = 6; + public static final int INTAKE_MOTOR = 7; //Arm Motors - public static final int ARM_MOTOR1_ID = 6; - public static final int ARM_MOTOR2_ID = 7; + public static final int ARM_MOTOR1_ID = 8; + public static final int ARM_MOTOR2_ID = 9; public static final int CLIMBER_CAN_ID = 10; + public static final int FLIP_OUT_CLIMBER = 11; + public static final int CLIMBER_LEFT = 12; + public static final int CLIMBER_RIGHT = 13; + // Pneumatics public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; - public static final int OPEN_INTAKE_PCM_ID = 4; - public static final int CLOSE_INTAKE_PCM_ID = 5; + public static final int OPEN_BALL_INTAKE_PCM_ID = 2; + public static final int CLOSE_BALL_INTAKE_PCM_ID = 3; + public static final int LIFT_HATCH_PCM_ID = 4; + public static final int LOWER_HATCH_INTAKE_PCM_ID = 5; + public static final int END_EFECTOR_EXPAND_PCM_ID = 6; + public static final int END_EFECTOR_CONTRACT_PCM_ID = 7; + + } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java deleted file mode 100644 index 8d724c4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class ArmBasic extends Command { - private double m_targetHeightInchesAboveFloor; - private double m_speed; - private boolean m_goingUp; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - public static boolean isfinishedElevatorBasic; - - public ArmBasic(double targetHeightInchesAboveFloor) { - requires(Robot.arm); - m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; - } - - // Called just before this Command runs the first time - protected void initialize() - { - Robot.arm.setControlMode(DriveControlMode.RAW); - - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - // start out at half speed, as crude way to reduce slippage - m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); -System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); - if (m_goingUp) { - m_speed = Constants.kArmBasicPercentOutputUp; - } - else { - m_speed = Constants.kArmBasicPercentOutputDown; - } - m_commandInitTimestamp = Timer.getFPGATimestamp(); - - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - Robot.arm.rawSetOutput(m_speed); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); -System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); - if (remaining < 0.0) { - finished = true; - - } - /*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - }*/ - - - if (!finished) { - SmartDashboard.putNumber("EB Dist", currentHeight); - } else { - SmartDashboard.putNumber("EB finDist", currentHeight); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp); - - isfinishedElevatorBasic = isFinished(); - - Robot.arm.rawSetOutput(0.0); - - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index a145532..70a0ea4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -1,49 +1,62 @@ - package org.usfirst.frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.command.Subsystem; - import java.util.ArrayList; -import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.utility.MPTalonPIDController; -import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.Loop; +import org.usfirst.frc4388.utility.MPTalonPIDController; import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.SoftwarePIDPositionController; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SensorCollection; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import org.usfirst.frc4388.utility.TalonSRXEncoder; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; -/** - * Add your docs here. - */ -public class Arm extends Subsystem +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Arm extends Subsystem implements Loop { - //Control Mode Array - public static enum ArmControlMode {PID, JOYSTICK_MANUAL}; + private static Arm instance; - //Motor Controllers - private ArrayList motorControllers = new ArrayList(); + public static enum ArmControlMode {PID, JOYSTICK_MANUAL }; + - private CANTalonEncoder arm1; + // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks + public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI); + + // Defined speeds + public static final double CLIMB_SPEED = -1.0; + public static final double TEST_SPEED_UP = 0.3; + public static final double TEST_SPEED_DOWN = -0.3; + public static final double AUTO_ZERO_SPEED = -0.3; + public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; + + // Defined positions + public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; + public static final double ZERO_POSITION_INCHES = -0.25; + public static final double NEAR_ZERO_POSITION_INCHES = 0.0; + public static final double MIN_POSITION_INCHES = 0.0; + public static final double MAX_POSITION_INCHES = 83.4; + public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; - - // PID controller and params + + // Motion profile max velocities and accel times + public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; + public static final double MP_T1 = 400; // Fast = 300 + public static final double MP_T2 = 150; // Fast = 150 + + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + + private CANTalonEncoder motor1; + private TalonSRX motor2; + + // PID controller and params private MPTalonPIDController mpController; public static int PID_SLOT = 0; @@ -54,64 +67,68 @@ public class Arm extends Subsystem private PIDParams pidPIDParamsLoGear = new PIDParams(0.45, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 0.005; public static final double KF_DOWN = 0.0; - public static final double PID_ERROR_INCHES = 1.0; + public static final double PID_ERROR_INCHES = 1.0; + private long periodMs = (long)(Constants.kLooperDt * 1000.0); - // Defined positions - public static final double MIN_POSITION_INCHES = 0.0; - public static final double MAX_POSITION_INCHES = 83.4; - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; - public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; - - //Misc - private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; - private boolean isFinished; - private double targetPositionInchesPID = 0; - private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; - - public Arm() - { - try - { - //PID arm encoder and talon - arm1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - } - catch(Exception e) - { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); - } - } - - //Method for setting the control mode for the arm - private synchronized void setArmControlMode(ArmControlMode controlMode) - { - this.armControlMode = controlMode; - } - - //Getting the control mode for the arm - private synchronized ArmControlMode getArmControlMode() - { - return this.armControlMode; - } - - //Setting the speed for the motor on the arm along with setting the control mode to manual - public void setSpeed(double speed) - { - arm1.set(ControlMode.PercentOutput, speed); - setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + // Misc + public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; + private boolean isFinished; + private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; + private double targetPositionInchesPID = 0; + private boolean firstMpPoint; + + + private Arm() { + try { + motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + //motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID); + + + + } + catch (Exception e) { + System.err.println("An error occurred in the DriveTrain constructor"); + } } - //Setting the target position for the PID loop and set the control mode to PID - public void setPositionPID(double targetPositionInches) - { - mpController.setPIDSlot(PID_SLOT); + @Override + public void initDefaultCommand() { + } + + public void resetZeroPosition(double position) { + mpController.resetZeroPosition(position); + } + + private synchronized void setArmControlMode(ArmControlMode controlMode) { + this.armControlMode = controlMode; + } + + private synchronized ArmControlMode getArmControlMode() { + return this.armControlMode; + } + + + public void setSpeedJoystick(double speed) { + motor1.set(ControlMode.PercentOutput, speed); + setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + } + + public void setPositionPID(double targetPositionInches) { + mpController.setPIDSlot(PID_SLOT); updatePositionPID(targetPositionInches); setArmControlMode(ArmControlMode.PID); setFinished(false); - } - - //Setting range for target position - private double limitPosition(double targetPosition) { + } + + public void updatePositionPID(double targetPositionInches) { + targetPositionInchesPID = limitPosition(targetPositionInches); + double startPositionInches = motor1.getPositionWorld(); + mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + } + + + private double limitPosition(double targetPosition) { if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; } @@ -121,35 +138,25 @@ public class Arm extends Subsystem return targetPosition; } - - //Method for updating the PID target position - public void updatePositionPID(double targetPositionInches) - { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = arm1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - } + + @Override + public void onStart(double timestamp) { + mpController.setPIDSlot(PID_SLOT); + mpController.setPIDSlot(PID_SLOT); + } - //Getting the current encoder position - public double getPositionInches() - { - return arm1.getPositionWorld(); - } - - //Setting the speed for the motors in manual mode - public void setSpeedJoystick(double speed) - { - arm1.set(ControlMode.PercentOutput, speed); - setArmControlMode(armControlMode.JOYSTICK_MANUAL); - } + @Override + public void onStop(double timestamp) { + // TODO Auto-generated method stub + + } - //@Override - public void onLoop(double timestamp) - { + @Override + public void onLoop(double timestamp) { synchronized (Arm.this) { - switch(getArmControlMode() ) { + switch( getArmControlMode() ) { case PID: - controlPID(); + controlPidWithJoystick(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); @@ -158,52 +165,61 @@ public class Arm extends Subsystem break; } } - } - - private void controlPID() - { + } + + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; - + double deltaPosition = joystickPosition *.5; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; - updatePositionPID(targetPositionInchesPID); + updatePositionPID(targetPositionInchesPID); } - - //Method for controlling the motor with the joystick - private void controlManualWithJoystick() - { - double joystickSpeed; - - joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick(joystickSpeed); + + private void controlManualWithJoystick() { + double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + setSpeedJoystick(joyStickSpeed); } + - public synchronized boolean isFinished() - { + public double getPositionInches() { + return motor1.getPositionWorld(); + } + +// public double getAverageMotorCurrent() { +// return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3; +// } + + public double getAverageMotorCurrent() { + return (motor1.getOutputCurrent() + motor2.getOutputCurrent() / 2); + } + + public synchronized boolean isFinished() { return isFinished; } - - public synchronized void setFinished(boolean isFinished) - { + + public synchronized void setFinished(boolean isFinished) { this.isFinished = isFinished; } - - @Override - public void initDefaultCommand() - { - } - - public void updateStatus(Robot.OperationMode operationMode) - { - if (operationMode == Robot.OperationMode.TEST) - { - try - { + + public double getPeriodMs() { + return periodMs; + } + + public void updateStatus(Robot.OperationMode operationMode) { + if (operationMode == Robot.OperationMode.TEST) { + try { + SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld()); + SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); + SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); } - catch (Exception e) - { - System.err.println("Arm update status error"); + catch (Exception e) { } } + } + + public static Arm getInstance() { + if(instance == null) { + instance = new Arm(); + } + return instance; } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java new file mode 100644 index 0000000..429ee7f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.utility; + +import edu.wpi.first.wpilibj.command.Command; + +public class Loop extends Command { + public Loop() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java new file mode 100644 index 0000000..c7ef11a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java @@ -0,0 +1,84 @@ +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; + +public class TalonSRXEncoder extends WPI_TalonSRX +{ + public static int TIMEOUT_MS = 0; + public static int PID_IDX = 0; + + private double encoderTicksToWorld; + private boolean isRight = true; + + public TalonSRXEncoder(int deviceId, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { + this(deviceId, encoderTicksToWorld, false, feedbackDevice); + } + + public TalonSRXEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { + super(deviceNumber); + this.configSelectedFeedbackSensor(feedbackDevice, PID_IDX, TIMEOUT_MS); + this.encoderTicksToWorld = encoderTicksToWorld; + this.isRight = isRight; + } + + public boolean isRight() { + return isRight; + } + + public void setRight(boolean isRight) { + this.isRight = isRight; + } + + public void setPID(int slotId, double kP, double kI, double kD) { + this.setPIDF(slotId, kP, kI, kD, 0); + } + + public void setPIDF(int slotId, double kP, double kI, double kD, double kF) { + this.config_kP(slotId, kP, TIMEOUT_MS); + this.config_kI(slotId, kI, TIMEOUT_MS); + this.config_kD(slotId, kD, TIMEOUT_MS); + this.config_kF(slotId, kF, TIMEOUT_MS); + } + + public void setPIDFIZone(int slotId, double kP, double kI, double kD, double kF, int iZone) { + this.config_kP(slotId, kP, TIMEOUT_MS); + this.config_kI(slotId, kI, TIMEOUT_MS); + this.config_kD(slotId, kD, TIMEOUT_MS); + this.config_kF(slotId, kF, TIMEOUT_MS); + this.config_IntegralZone(slotId, iZone, TIMEOUT_MS); + } + + public double convertEncoderTicksToWorld(double encoderTicks) { + return encoderTicks / encoderTicksToWorld; + } + + public int convertEncoderWorldToTicks(double worldValue) { + return (int)(worldValue * encoderTicksToWorld); + } + + public void setWorld(ControlMode mode, double worldValue) { + this.set(mode, convertEncoderWorldToTicks(worldValue)); + } + + public void setPosition(int value) { + this.setSelectedSensorPosition(value, PID_IDX, TIMEOUT_MS); + } + + public void setPositionWorld(double worldValue) { + this.setSelectedSensorPosition(convertEncoderWorldToTicks(worldValue), PID_IDX, TIMEOUT_MS); + } + + public double getPositionWorld() { + return convertEncoderTicksToWorld(this.getSelectedSensorPosition(PID_IDX)); + } + + public void setVelocityWorld(double worldValue) { + this.set(ControlMode.Velocity, convertEncoderWorldToTicks(worldValue) * 0.1); + } + + public double getVelocityWorld() { + return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1); + } +} \ No newline at end of file