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 73d67f3..84d16ad 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 @@ -38,9 +38,9 @@ public class Arm extends Subsystem implements ControlLoopable public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; public static enum PlaceMode { HATCH, CARGO }; - // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks - public static final double ENCODER_TICKS_TO_INCHES = (1); - + // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks + public static final double ENCODER_TICKS_TO_INCHES = (1); + private double periodMs; private double lastControlLoopUpdatePeriod = 0.0; @@ -52,7 +52,7 @@ public class Arm extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_SPEED = -0.3; public static final double JOYSTICK_INCHES_PER_MS_HI = 35; public static final double JOYSTICK_INCHES_PER_MS_LO = 35; - + // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; public static final double ZERO_POSITION_INCHES = -0.25; @@ -72,16 +72,16 @@ public class Arm extends Subsystem implements ControlLoopable public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0; // Motion profile max velocities and accel times - public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; + public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; public static final double MP_T1 = 300; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 - + // Motor controllers - private ArrayList motorControllers = new ArrayList(); + private ArrayList motorControllers = new ArrayList(); public TalonSRXEncoder motor1; private TalonSRX motor2; - + // PID controller and params private MPTalonPIDController mpController; @@ -89,8 +89,8 @@ public class Arm extends Subsystem implements ControlLoopable public static int MM_SLOT = 1; public static int MP_SLOT = 2; - private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); - private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); + private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 1;//0.01; public static final double KF_DOWN = 0;// 0.0; public static final double P_Value = 0.5;// 2; @@ -108,12 +108,12 @@ public class Arm extends Subsystem implements ControlLoopable private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 5; LimitSwitchSource limitSwitchSource; - + // Pneumatics private Solenoid speedShift; // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; + public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC; public PlaceMode placeMode = PlaceMode.HATCH; @@ -123,20 +123,20 @@ public class Arm extends Subsystem implements ControlLoopable private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; - + public Arm() { try { motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); - - + + motor1.setInverted(true); motor2.setInverted(true); - + // if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } - + motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); @@ -155,8 +155,8 @@ public class Arm extends Subsystem implements ControlLoopable motor1.enableCurrentLimit(true); motorControllers.add(motor1); //motor1.setSelectedSensorPosition(0, , ); - - + + } catch (Exception e) { System.err.println("An error occurred in the Arm constructor"); @@ -166,21 +166,21 @@ public class Arm extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } - + } + public void resetEncoder(){ motor1.setPosition(0); targetPositionInchesMM = 0; targetPositionInchesPID = 0; } - + private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; } - + private synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } @@ -189,7 +189,7 @@ public class Arm extends Subsystem implements ControlLoopable motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); @@ -220,25 +220,25 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println(motor1.getControlMode()); motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); - + } - + public void setPositionPID(double targetPositionInches) { motor1.set(ControlMode.Position, targetPositionInches); mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set updatePositionPID(targetPositionInches); - setArmControlMode(ArmControlMode.JOYSTICK_PID); + setArmControlMode(ArmControlMode.JOYSTICK_PID); setFinished(false); } - + public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){ resetEncoder(); } double startPositionInches = motor1.getPositionWorld(); - //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); motor1.set(ControlMode.Position, targetPositionInches); motor1.configClosedloopRamp(0); //motor1.configPeakCurrentLimit(5); @@ -250,15 +250,15 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println(motor1.getControlMode()); //System.err.print(motor1.getClosedLoopError()); } - + public void setPositionMP(double targetPositionInches) { double startPositionInches = motor1.getPositionWorld(); - mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); setFinished(false); firstMpPoint = true; setArmControlMode(ArmControlMode.MOTION_PROFILE); } - + private double limitPosition(double targetPosition) { /*if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; @@ -266,7 +266,7 @@ public class Arm extends Subsystem implements ControlLoopable if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -320,7 +320,7 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdateTimestamp = currentTimestamp; dPadButtons(); - + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ resetEncoder(); } @@ -331,8 +331,8 @@ public class Arm extends Subsystem implements ControlLoopable } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { - isFinished = mpController.controlLoopUpdate(getPositionInches()); - + isFinished = mpController.controlLoopUpdate(getPositionInches()); + } if (armControlMode == ArmControlMode.JOYSTICK_PID){ controlPidWithJoystick(); @@ -342,13 +342,13 @@ public class Arm extends Subsystem implements ControlLoopable controlMMWithJoystick(); //System.err.println(motor1.getControlMode()); } - + /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } else if (armControlMode == ArmControlMode.ADAPTIVE_PURSUIT) { updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); }*/ } } @@ -358,7 +358,7 @@ public class Arm extends Subsystem implements ControlLoopable - + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; @@ -370,8 +370,10 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesMM = targetPositionInchesMM + deltaPosition; updatePositionMM(targetPositionInchesMM); - - + Wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + Robot.wrist.updatePositionPID(Wrist.targetPositionInchesPID); + + } private void ControlWithJoystickhold(){ @@ -379,7 +381,7 @@ public class Arm extends Subsystem implements ControlLoopable updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick((joyStickSpeed*.3)+.08); @@ -397,7 +399,7 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -405,31 +407,31 @@ public class Arm extends Subsystem implements ControlLoopable 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) { this.isFinished = isFinished; } - + public double getPeriodMs() { return periodMs; } - + public void updateStatus(Robot.OperationMode operationMode) { //System.err.println("the encoder is right after this"); try { - + SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); @@ -447,9 +449,9 @@ public class Arm extends Subsystem implements ControlLoopable catch (Exception e) { System.err.println("Arm update status error" +e.getMessage()); } - - } - + + } + public static Arm getInstance() { if(instance == null) { instance = new Arm(); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 90cfea4..7ebb2be 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -32,9 +32,9 @@ public class Wrist extends Subsystem implements ControlLoopable public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; - // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks - public static final double ENCODER_TICKS_TO_INCHES = (1); - + // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks + public static final double ENCODER_TICKS_TO_INCHES = (1); + private double periodMs; private double lastControlLoopUpdatePeriod = 0.0; @@ -46,7 +46,7 @@ public class Wrist extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_SPEED = -0.3; public static final double JOYSTICK_INCHES_PER_MS_HI = 20; public static final double JOYSTICK_INCHES_PER_MS_LO = 27; - + // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; public static final double ZERO_POSITION_INCHES = -0.25; @@ -66,23 +66,23 @@ public class Wrist extends Subsystem implements ControlLoopable public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0; // Motion profile max velocities and accel times - public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; + 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 ArrayList motorControllers = new ArrayList(); public TalonSRXEncoder wristMotor1; - + // PID controller and params private MPTalonPIDController mpController; public static int PID_SLOT = 0; public static int MP_SLOT = 1; - private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); - private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); + private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 0.01; public static final double KF_DOWN = 0.0; public static final double P_Value = 1; @@ -96,7 +96,7 @@ public class Wrist extends Subsystem implements ControlLoopable private Solenoid speedShift; // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; + public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; public double targetPositionInchesPID = 0; @@ -104,25 +104,25 @@ public class Wrist extends Subsystem implements ControlLoopable private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; - + public Wrist() { try { - wristMotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); + wristMotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); wristMotor1.setInverted(false); - + // if (wristMotor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } - + wristMotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); wristMotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); wristMotor1.setNeutralMode(NeutralMode.Brake); wristMotor1.enableCurrentLimit(true); //wristMotor1.setSensorPhase(true); motorControllers.add(wristMotor1); - - + + } catch (Exception e) { System.err.println("An error occurred in the Wrist constructor"); @@ -132,20 +132,20 @@ public class Wrist extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } + } public void resetencoder(){ wristMotor1.setPosition(0); targetPositionInchesPID = 0; } - - + + private synchronized void setWristControlMode(WristControlMode controlMode) { this.wristControlMode = controlMode; } - + private synchronized WristControlMode getWristControlMode() { return this.wristControlMode; } @@ -154,24 +154,24 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } - + public void setPositionPID(double targetPositionInches) { wristMotor1.set(ControlMode.Position, targetPositionInches); mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set updatePositionPID(targetPositionInches); - setWristControlMode(WristControlMode.JOYSTICK_PID); + setWristControlMode(WristControlMode.JOYSTICK_PID); setFinished(false); } - + public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); double startPositionInches = wristMotor1.getPositionWorld(); - //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); wristMotor1.set(ControlMode.Position, targetPositionInches); wristMotor1.configClosedloopRamp(.02); //wristMotor1.configPeakCurrentLimit(5); @@ -183,24 +183,24 @@ public class Wrist extends Subsystem implements ControlLoopable //System.err.println(wristMotor1.getControlMode()); //System.err.print(wristMotor1.getClosedLoopError()); } - + public void setPositionMP(double targetPositionInches) { double startPositionInches = wristMotor1.getPositionWorld(); - mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); setFinished(false); firstMpPoint = true; setWristControlMode(WristControlMode.MOTION_PROFILE); } - + private double limitPosition(double targetPosition) { - + /*if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; } else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; }*/ - + return targetPosition; } @Override @@ -224,26 +224,26 @@ public class Wrist extends Subsystem implements ControlLoopable @Override public void onStop(double timestamp) { // TODO Auto-generated method stub - + } @Override public void onLoop(double timestamp) { synchronized (Wrist.this) { switch( getElevatorControlMode() ) { - case JOYSTICK_PID: + case JOYSTICK_PID: controlPidWithJoystick(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); break; - case MOTION_PROFILE: + case MOTION_PROFILE: if (!isFinished()) { if (firstMpPoint) { mpController.setPIDSlot(MP_SLOT); firstMpPoint = false; } - setFinished(mpController.controlLoopUpdate()); + setFinished(mpController.controlLoopUpdate()); if (isFinished()) { mpController.setPIDSlot(PID_SLOT); } @@ -268,30 +268,30 @@ public class Wrist extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - + // Do the update if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); - + } else if (!isFinished) { if (wristControlMode == WristControlMode.MOTION_PROFILE) { - isFinished = mpController.controlLoopUpdate(getPositionInches()); - + isFinished = mpController.controlLoopUpdate(getPositionInches()); + } if (wristControlMode == WristControlMode.JOYSTICK_PID){ System.err.println(wristMotor1.getControlMode()); controlPidWithJoystick(); - - + + } - + /*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } else if (wristControlMode == WristControlMode.ADAPTIVE_PURSUIT) { updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); }*/ } } @@ -301,14 +301,14 @@ public class Wrist extends Subsystem implements ControlLoopable - + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); - - + + } private void ControlWithJoystickhold(){ @@ -316,7 +316,7 @@ public class Wrist extends Subsystem implements ControlLoopable updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); setSpeedJoystick(joyStickSpeed*.5); @@ -334,7 +334,7 @@ public class Wrist extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -342,34 +342,34 @@ public class Wrist extends Subsystem implements ControlLoopable public double getPositionInches() { return wristMotor1.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 synchronized boolean isFinished() { return isFinished; } - + public synchronized void setFinished(boolean isFinished) { this.isFinished = isFinished; } - + public double getPeriodMs() { return periodMs; } - + public void updateStatus(Robot.OperationMode operationMode) { //System.err.println("the encoder is right after this"); try { - + SmartDashboard.putNumber("Wrist Position Ticks", wristMotor1.getPositionWorld()); SmartDashboard.putNumber("Wrist Motor 1 Amps", wristMotor1.getOutputCurrent()); SmartDashboard.putNumber("wrist pid error", wristMotor1.getClosedLoopError()); SmartDashboard.putNumber("wrist motor output", wristMotor1.getMotorOutputPercent()); - + // SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); @@ -378,9 +378,9 @@ public class Wrist extends Subsystem implements ControlLoopable catch (Exception e) { System.err.println("Drivetrain update status error" +e.getMessage()); } - - } - + + } + public static Wrist getInstance() { if(instance == null) { instance = new Wrist();