From 21493bac4675ccac259a612e867a27cde070d901 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Sat, 26 Jan 2019 12:32:44 -0800 Subject: [PATCH] Converting position in inches to degrees Changed wrist subsystem and added to constants --- .../org/usfirst/frc4388/robot/Constants.java | 7 ++- .../frc4388/robot/commands/WristSetMode.java | 4 +- .../frc4388/robot/subsystems/Wrist.java | 45 +++++++++++-------- .../frc4388/utility/CANTalonEncoder.java | 2 +- 4 files changed, 36 insertions(+), 22 deletions(-) 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 21b73c4..ebdf7fe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -34,7 +34,12 @@ public class Constants { public static double kArmInchesOfTravelPerRev = 3.75; public static double kArmEncoderTicksPerInch = 126.36; public static double kArmBasicPercentOutputUp = -0.85; - public static double kArmBasicPercentOutputDown =.7; + public static double kArmBasicPercentOutputDown =.7; + + //Wrist + public static int kWristEncoderticksPerRev = 4096; + public static double kWristDegreesOfTravel = 306; + public static double kWristEncoderTicksPerDegree = 11.38; // CONTROL LOOP GAINS 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 4e2a196..7b9e956 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.kArmEncoderTicksPerInch; + 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) {