From 2a3e42aeffa3e1d6ff407c3961d3f58105f0d664 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sat, 9 Mar 2019 14:23:23 -0700 Subject: [PATCH 1/4] Constants Adjustments --- .../java/org/usfirst/frc4388/robot/subsystems/Arm.java | 8 ++++---- .../java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) 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 3c82ae8..e5049ce 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 @@ -52,7 +52,7 @@ public class Arm extends Subsystem implements ControlLoopable 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 = 4096; + public static final double MAX_POSITION_INCHES = 3900; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -87,11 +87,11 @@ public class Arm extends Subsystem implements ControlLoopable public static final double KF_UP = 0.01; public static final double KF_DOWN = 0.0; public static final double P_Value = 2; - public static final double I_Value = 0.00300; + public static final double I_Value = 0.00030; public static final double D_Value = 200; public static final double RampRate = 0.0; 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.0; + public static final double PID_ERROR_INCHES = 5; LimitSwitchSource limitSwitchSource; // Pneumatics private Solenoid speedShift; @@ -175,7 +175,7 @@ public class Arm extends Subsystem implements ControlLoopable double startPositionInches = motor1.getPositionWorld(); //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); motor1.set(ControlMode.Position, targetPositionInches); - motor1.configClosedloopRamp(.02); + motor1.configClosedloopRamp(0); //motor1.configPeakCurrentLimit(5); motor1.configContinuousCurrentLimit(2); motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); 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 af02701..e405ce0 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 @@ -85,7 +85,7 @@ public class Wrist extends Subsystem implements ControlLoopable 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 = 2; + public static final double P_Value = .5; public static final double I_Value = 0.00300; public static final double D_Value = 200; public static final double RampRate = 0.0; From 387558d6355d6077a8781ad6c1e7a6de9adba66c Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 9 Mar 2019 14:54:54 -0700 Subject: [PATCH 2/4] Introduce Failsafe for if the robot position goes belows negative (Sets the negative position to the new 0) --- .../java/org/usfirst/frc4388/robot/subsystems/Arm.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 a2db85c..1abc253 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 @@ -171,7 +171,10 @@ public class Arm extends Subsystem implements ControlLoopable } public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(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); motor1.set(ControlMode.Position, targetPositionInches); @@ -309,8 +312,6 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); - - } private void ControlWithJoystickhold(){ From 7269697ac93d74d8bef64af76fd2b85496a34e56 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 9 Mar 2019 15:52:24 -0700 Subject: [PATCH 3/4] Update Limit Switch code in Arm.java --- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 115d8e7..7f67988 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 @@ -94,8 +94,6 @@ 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.0; LimitSwitchSource limitSwitchSource; - DigitalInput forwardLimitSwitch = new DigitalInput(1); - DigitalInput reverseLimitSwitch = new DigitalInput(2); // Pneumatics private Solenoid speedShift; @@ -274,7 +272,7 @@ public class Arm extends Subsystem implements ControlLoopable } lastControlLoopUpdateTimestamp = currentTimestamp; - if (reverseLimitSwitch.get()){ + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ motor1.setPosition(0); } From 0b025ff3677262f9ea22d0cb515b2c683f5f3e99 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 9 Mar 2019 16:29:28 -0700 Subject: [PATCH 4/4] Arm.java Cleanup --- .../src/main/java/org/usfirst/frc4388/robot/Robot.java | 2 +- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 82531b5..340d148 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -99,7 +99,7 @@ public class Robot extends TimedRobot drive.resetEncoders(); drive.resetGyro(); drive.setIsRed(getAlliance().equals(Alliance.Red)); - arm.resetencoder(); + arm.resetEncoder(); } 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 2da4864..6eaa1b8 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 @@ -142,7 +142,7 @@ public class Arm extends Subsystem implements ControlLoopable public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); } - public void resetencoder(){ + public void resetEncoder(){ motor1.setPosition(0); } @@ -175,7 +175,7 @@ public class Arm extends Subsystem implements ControlLoopable public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){ - resetencoder(); + resetEncoder(); } double startPositionInches = motor1.getPositionWorld(); //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); @@ -276,7 +276,7 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdateTimestamp = currentTimestamp; if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ - motor1.setPosition(0); + resetEncoder(); } // Do the update