From 52551db1343f536454900ef86b5ec69840e00b96 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Tue, 19 Mar 2019 23:10:58 -0600 Subject: [PATCH] final changes befor denver lets get this bread mm on wrist, height claibrations --- .../java/org/usfirst/frc4388/robot/Robot.java | 2 +- .../robot/commands/WristSetPositionMM.java | 3 +- .../robot/commands/presets/CargoHigh.java | 2 +- .../robot/commands/presets/CargoLow.java | 2 +- .../robot/commands/presets/CargoMid.java | 4 +- .../robot/commands/presets/HatchHigh.java | 4 +- .../robot/commands/presets/HatchLow.java | 2 +- .../robot/commands/presets/HatchMid.java | 4 +- .../robot/commands/presets/setWrist.java | 3 +- .../frc4388/robot/subsystems/Wrist.java | 62 ++++++++----------- 10 files changed, 39 insertions(+), 49 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 fe67815..2171131 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -112,7 +112,7 @@ public class Robot extends TimedRobot //drive.resetEncoders(); drive.endGyroCalibration(); arm.targetPositionInchesMM = arm.motor1.getPositionWorld(); - wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld(); + wrist.targetPositionInchesMM = wrist.wristMotor1.getPositionWorld(); updateStatus(); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java index ac0183b..b6e4b2e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java @@ -22,8 +22,7 @@ public class WristSetPositionMM extends Command { protected void initialize() { if (Math.abs(targetPositionInches - Robot.wrist.getPositionInches()) < MIN_DELTA_TARGET) { - isAtTarget = true; - System.out.println("Wrist is at target"); + isAtTarget = true; } else { isAtTarget = false; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java index 57996b9..58da5ab 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java @@ -21,7 +21,7 @@ public class CargoHigh extends CommandGroup { addSequential(new HatchFlip(false)); addParallel(new setWrist(2781)); addParallel(new DelayHatch()); - addSequential(new ArmSetPositionMM(3812)); + addSequential(new ArmSetPositionMM(4038)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java index 58dd417..5b57c0d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java @@ -18,7 +18,7 @@ public class CargoLow extends CommandGroup { */ public CargoLow() { - addParallel(new setWrist(2300)); + addParallel(new setWrist(2200)); addSequential(new ArmSetPositionMM(1300)); // Add Commands here: diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java index 2a48ef7..ae3dc76 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java @@ -18,8 +18,8 @@ public class CargoMid extends CommandGroup { */ public CargoMid() { - addParallel(new setWrist(2430)); - addSequential(new ArmSetPositionMM(2630)); + addParallel(new setWrist(2330)); + addSequential(new ArmSetPositionMM(2580)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java index b9bc0e8..9867d8e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java @@ -19,9 +19,9 @@ public class HatchHigh extends CommandGroup { */ public HatchHigh() { addSequential(new HatchFlip(false)); - addParallel(new setWrist(1144)); + addParallel(new setWrist(852)); addParallel(new DelayHatch()); - addSequential(new ArmSetPositionMM(3605)); + addSequential(new ArmSetPositionMM(3451)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java index da1b0ba..f0c3efe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java @@ -18,7 +18,7 @@ public class HatchLow extends CommandGroup { */ public HatchLow() { - addParallel(new setWrist(350)); + addParallel(new setWrist(200)); addSequential(new ArmSetPositionMM(550)); // Add Commands here: diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java index bbf9813..b38ba4e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java @@ -18,8 +18,8 @@ public class HatchMid extends CommandGroup { */ public HatchMid() { - addParallel(new setWrist(750)); - addSequential(new ArmSetPositionMM(2000)); + addParallel(new setWrist(525)); + addSequential(new ArmSetPositionMM(2050)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java index b4fba0c..c60f454 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java @@ -7,6 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; +import org.usfirst.frc4388.robot.commands.WristSetPositionMM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -18,7 +19,7 @@ public class setWrist extends CommandGroup { */ public setWrist(double wrist) { addSequential(new WaitCommand(1)); - addSequential(new WristSetPositionPID(wrist)); + addSequential(new WristSetPositionMM(wrist)); // Add Commands here: // addSequential(new Command2()); // these will run in order. 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 158f29d..1e42b45 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 @@ -46,7 +46,7 @@ public class Wrist extends Subsystem implements ControlLoopable 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 = 20; - public static final double JOYSTICK_INCHES_PER_MS_LO = 27; + public static final double JOYSTICK_INCHES_PER_MS_LO = 20; // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; @@ -87,12 +87,12 @@ 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 = 1; - public static final double I_Value = 0.0000; - public static final double D_Value = 000; + public static final double P_Value = 1.2; + public static final double I_Value = 0.002; + public static final double D_Value = 125; public static final double F_Value = 1; - public static final int CV_value = 100; - public static final int A_value = 100; + public static final int CV_value = 200; + public static final int A_value = 350; public static final double RampRate = 0.0; public static final double maxGravityComp = 0; private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later @@ -104,7 +104,7 @@ public class Wrist extends Subsystem implements ControlLoopable // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; + private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC; public double targetPositionInchesPID = 0; public double targetPositionInchesMM = 0; private boolean firstMpPoint; @@ -122,12 +122,6 @@ public class Wrist extends Subsystem implements ControlLoopable // 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); - wristMotor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); wristMotor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); wristMotor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); @@ -138,6 +132,12 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); wristMotor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); wristMotor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); + + 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) { @@ -175,11 +175,11 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } - public void setPositionMM(double targetPositionInches){ + wristMotor1.selectProfileSlot(MM_SLOT, 0); wristMotor1.set(ControlMode.MotionMagic, targetPositionInches); //System.err.println(wristMotor1.getControlMode()); - wristMotor1.selectProfileSlot(MM_SLOT, 0); + setWristControlMode(WristControlMode.MOTION_MAGIC); updatePositionMM(targetPositionInches); setFinished(false); @@ -219,7 +219,7 @@ public class Wrist extends Subsystem implements ControlLoopable double startPositionInches = wristMotor1.getPositionWorld(); //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); wristMotor1.set(ControlMode.Position, targetPositionInches); - wristMotor1.configClosedloopRamp(.02); + wristMotor1.configClosedloopRamp(.02); //TODO: Arm has this set to zero //wristMotor1.configPeakCurrentLimit(5); wristMotor1.configContinuousCurrentLimit(2); wristMotor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); @@ -318,23 +318,20 @@ public class Wrist extends Subsystem implements ControlLoopable // Do the update if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); - } else if (!isFinished) { if (wristControlMode == WristControlMode.MOTION_PROFILE) { isFinished = mpController.controlLoopUpdate(getPositionInches()); } + if (wristControlMode == WristControlMode.JOYSTICK_PID){ + controlPidWithJoystick(); + //System.err.println(wristMotor1.getControlMode()); + } if (wristControlMode == WristControlMode.MOTION_MAGIC){ controlMMWithJoystick(); //System.err.println(wristMotor1.getControlMode()); } - if (wristControlMode == WristControlMode.JOYSTICK_PID){ - //System.err.println(wristMotor1.getControlMode()); - controlPidWithJoystick(); - - - } /*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) { isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); @@ -347,25 +344,18 @@ 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 controlMMWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesMM = targetPositionInchesMM + deltaPosition; updatePositionMM(targetPositionInchesMM); //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); - - } - - - - private void controlPidWithJoystick() { - double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; - targetPositionInchesPID = targetPositionInchesPID + deltaPosition; - updatePositionPID(targetPositionInchesPID); - - } private void ControlWithJoystickhold(){