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 a933550..c10bb0d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -36,11 +36,13 @@ public class Constants { public static double kArmBasicPercentOutputUp = -0.85; public static double kArmBasicPercentOutputDown =.7; + - // Wrist - public static int kWristEncoderTicksPerRev = 4096; - public static double kWristDegreesOfTravelPerRev = 360; + //Wrist + public static int kWristEncoderticksPerRev = 4096; + public static double kWristDegreesOfTravel = 360; public static double kWristEncoderTicksPerDegree = 11.38; + // CONTROL LOOP GAINS // PID gains for drive velocity loop (LOW GEAR) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java index 3d54006..b86576a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java @@ -20,10 +20,10 @@ public class WristSetMode extends Command { // Called just before this Command runs the first time protected void initialize() { if (controlMode == WristControlMode.PID) { - Robot.wrist.setPositionPID(Robot.wrist.getPositionInches()); + Robot.wrist.setPositionPID(Robot.wrist.getPositionDegrees()); } else if (controlMode == WristControlMode.JOYSTICK_MANUAL) { - Robot.wrist.setSpeed(0); + Robot.wrist.setSpeedJoystick(0); } } 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 248276a..5edbeb9 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 @@ -41,6 +41,7 @@ public class Wrist extends Subsystem //Encoder ticks to inches for encoders public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; + public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI; // PID controller and params private MPTalonPIDController mpController; @@ -58,22 +59,29 @@ public class Wrist extends Subsystem // Defined positions public static final double MIN_POSITION_INCHES = 0.0; public static final double MAX_POSITION_INCHES = 83.4; + + public static final double MIN_ANGLE_DEGREES = 0.0; ////FIND ANGLE VALUES + public static final double MAX_ANGLE_DEGREES = 0.0; + 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; + public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; private boolean isFinished; private double targetPositionInchesPID = 0; + private double targetAngleDegreesPID = 0; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; + private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; public Wrist() { try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); } catch(Exception e) { @@ -101,36 +109,37 @@ public class Wrist extends Subsystem } //Setting the target position for the PID loop and set the control mode to PID - public void setPositionPID(double targetPositionInches) + public void setPositionPID(double targetAngleInches) { mpController.setPIDSlot(PID_SLOT); - updatePositionPID(targetPositionInches); + updatePositionPID(targetAngleInches); setWristControlMode(WristControlMode.PID); setFinished(false); } //Setting range for target position - private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { - return MIN_POSITION_INCHES; + private double limitPosition(double targetAngle) + { + if (targetAngle < MIN_ANGLE_DEGREES) { + return MIN_ANGLE_DEGREES; } - else if (targetPosition > MAX_POSITION_INCHES) { - return MAX_POSITION_INCHES; + else if (targetAngle > MAX_ANGLE_DEGREES) { + return MAX_ANGLE_DEGREES; } - return targetPosition; + return targetAngle; } //Method for updating the PID target position - public void updatePositionPID(double targetPositionInches) + public void updatePositionPID(double targetAngleDegrees) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = wristRight.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + targetAngleDegreesPID = limitPosition(targetAngleDegrees); + double startAngleDegrees = wristRight.getPositionWorld(); + mpController.setTarget(targetPositionInchesPID, targetAngleDegreesPID > startAngleDegrees ? KF_UP : KF_DOWN); } //Getting the current encoder position - public double getPositionInches() + public double getPositionDegrees() { return wristRight.getPositionWorld(); } @@ -161,11 +170,11 @@ public class Wrist extends Subsystem private void controlPID() { - double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; + double joystickAngle = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaAngle = joystickAngle * joystickDegreesPerMs; - targetPositionInchesPID = targetPositionInchesPID + deltaPosition; - updatePositionPID(targetPositionInchesPID); + targetAngleDegreesPID = targetAngleDegreesPID + deltaAngle; + updatePositionPID(targetAngleDegreesPID); } //Method for controlling the motor with the joystick 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 7e60123..73373e8 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -64,7 +64,7 @@ public class CANTalonEncoder extends WPI_TalonSRX { //return convertEncoderTicksToWorld(this.getPosition()); return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" - } + } public void setVelocityWorld(double worldValue) {