From e0947b51d918bb5f59f3d8218c39666c7cb3b4cd Mon Sep 17 00:00:00 2001 From: HFocus Date: Sat, 16 Mar 2019 14:17:15 -0600 Subject: [PATCH 1/5] Make Wrist move with Arm --- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 4 +++- .../main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 2 +- 2 files changed, 4 insertions(+), 2 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 07db110..3b288c6 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 @@ -113,7 +113,7 @@ public class Arm extends Subsystem implements ControlLoopable private boolean isFinished; private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC; private double targetPositionInchesPID = 0; - private double targetPositionInchesMM = 0; + public static double targetPositionInchesMM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -334,6 +334,8 @@ 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); } 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 855f456..3289a07 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 @@ -99,7 +99,7 @@ public class Wrist extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; - private double targetPositionInchesPID = 0; + public static double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; From 10c538d831c55cabbe129c872aff0dfcdc782c8c Mon Sep 17 00:00:00 2001 From: HFocus Date: Sun, 17 Mar 2019 20:12:30 -0600 Subject: [PATCH 2/5] The One Commit to Rule Them All --- .../java/org/usfirst/frc4388/robot/OI.java | 23 +++++-- .../java/org/usfirst/frc4388/robot/Robot.java | 2 + .../robot/commands/ResetArmEncoder.java | 46 ++++++++++++++ .../robot/commands/ResetWristEncoder.java | 46 ++++++++++++++ .../frc4388/robot/commands/SetMMAndPID.java | 39 ++++++++++++ .../robot/commands/SetPositionArmWrist.java | 38 ++++++++++++ .../frc4388/robot/commands/StowArm.java | 5 +- .../robot/commands/SwitchArmPlaceMode.java | 53 ++++++++++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 62 +++++++++++++++---- .../frc4388/robot/subsystems/Wrist.java | 10 +-- 10 files changed, 300 insertions(+), 24 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index b08f4fb..9e19634 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.*; import org.usfirst.frc4388.robot.subsystems.*; +import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; @@ -74,9 +75,19 @@ public class OI safteySwitch.whenReleased(new setClimberSafety(false)); safteySwitch.whenReleased(new setLEDPattern(LEDPatterns.FOREST_WAVES)); + JoystickButton manual = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); + manual.whenPressed(new SetManual()); + manual.whenReleased(new SetMMAndPID()); + JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS); JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS); + JoystickButton resetArmEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.B_BUTTON); + resetArmEncoder.whenPressed(new ResetArmEncoder()); + + JoystickButton resetWristEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON); + resetWristEncoder.whenPressed(new ResetWristEncoder()); + /** DEPRICATED, TRIGGERS ON THE DRIVER JOYSTICK COVER FOR THIS BUTTON */ //JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON); //ratchetFlip.whenPressed(new ratchetFlip(0.5)); @@ -103,13 +114,15 @@ public class OI JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON); - lowHeight.whenPressed(new GrabFromLoadingSatation()); + lowHeight.whenPressed(new SetPositionArmWrist(550, 450)); - JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - stow.whenPressed(new StowArm()); - stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); + JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO)); + cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH)); + //stow.whenPressed(new StowArm()); + //stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); - SmartDashboard.putData("switch to manuel", new SetManual()); + //SmartDashboard.putData("switch to manuel", new SetManual()); // SmartDashboard.putData("run turn test", new TestTurn()); SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); 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 864d3ce..896a60f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -118,6 +118,8 @@ public class Robot extends TimedRobot //System.err.println("TeleopInit after resetEncoders"); drive.endGyroCalibration(); System.err.println("TeleopInit after endGyroCalibrations"); + arm.targetPositionInchesMM = arm.motor1.getPositionWorld(); + wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld(); updateStatus(); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java new file mode 100644 index 0000000..dcdb5b9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ResetArmEncoder extends Command { + public ResetArmEncoder() { + requires(Robot.arm); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.arm.resetEncoder(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java new file mode 100644 index 0000000..8cdf431 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ResetWristEncoder extends Command { + public ResetWristEncoder() { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.wrist.resetencoder(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java new file mode 100644 index 0000000..e67d86c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class SetMMAndPID extends CommandGroup { + /** + * Add your docs here. + */ + public SetMMAndPID() { + addSequential(new ArmSetMode(ArmControlMode.MOTION_MAGIC)); + addParallel(new WristSetMode(WristControlMode.JOYSTICK_PID)); + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java new file mode 100644 index 0000000..6eecaab --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class SetPositionArmWrist extends CommandGroup { + /** + * Add your docs here. + */ + public SetPositionArmWrist(double arm, double wrist) { + + addParallel(new WristSetPositionPID(wrist)); + addSequential(new ArmSetPositionMM(arm)); + + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index 1f2c239..4465ea7 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -17,8 +17,9 @@ public class StowArm extends CommandGroup { public StowArm() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addParallel(new WristSetPositionPID(200), 2); - addSequential(new ArmSetPositionMM(-10), 4); + addParallel(new WristSetPositionPID(110), 2); + addSequential(new ArmSetPositionMM(420), 4); + addSequential(new ArmSetPositionMM(5)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java new file mode 100644 index 0000000..d02370d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode; + +import edu.wpi.first.wpilibj.command.Command; + + + +public class SwitchArmPlaceMode extends Command { + + public PlaceMode placeMode; + + public SwitchArmPlaceMode(PlaceMode placeMode) { + requires(Robot.arm); + this.placeMode = placeMode; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.arm.placeMode = placeMode; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} 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 44733a9..73d67f3 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 @@ -4,6 +4,9 @@ import java.util.ArrayList; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.SetPositionArmWrist; +import org.usfirst.frc4388.robot.commands.StowArm; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPTalonPIDController; @@ -33,6 +36,7 @@ public class Arm extends Subsystem implements ControlLoopable private static Arm instance; 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); @@ -53,8 +57,8 @@ public class Arm extends Subsystem implements ControlLoopable public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; 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 = -10; - public static final double MAX_POSITION_INCHES = 3900; + public static final double MIN_POSITION_INCHES = -25; + public static final double MAX_POSITION_INCHES = 4400; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -75,7 +79,7 @@ public class Arm extends Subsystem implements ControlLoopable // Motor controllers private ArrayList motorControllers = new ArrayList(); - private TalonSRXEncoder motor1; + public TalonSRXEncoder motor1; private TalonSRX motor2; // PID controller and params @@ -112,8 +116,9 @@ public class Arm extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC; - private double targetPositionInchesPID = 0; - private double targetPositionInchesMM = 0; + public PlaceMode placeMode = PlaceMode.HATCH; + public double targetPositionInchesPID = 0; + public double targetPositionInchesMM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -165,8 +170,11 @@ public class Arm extends Subsystem implements ControlLoopable public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); } + public void resetEncoder(){ motor1.setPosition(0); + targetPositionInchesMM = 0; + targetPositionInchesPID = 0; } private synchronized void setArmControlMode(ArmControlMode controlMode) { @@ -193,9 +201,6 @@ public class Arm extends Subsystem implements ControlLoopable setArmControlMode(ArmControlMode.MOTION_MAGIC); updatePositionMM(targetPositionInches); setFinished(false); - - - } public double calcGravityCompensationAtCurrentPosition() { @@ -255,10 +260,10 @@ public class Arm extends Subsystem implements ControlLoopable } private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { + /*if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; - } - else if (targetPosition > MAX_POSITION_INCHES) { + }*/ + if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } @@ -273,9 +278,38 @@ public class Arm extends Subsystem implements ControlLoopable this.periodMs = periodMs; } + private int lastDPadAngle = -1; + public void dPadButtons(){ + int dPadAngle = Robot.oi.getOperatorController().getDpadAngle(); + if (placeMode == PlaceMode.HATCH){ + if (dPadAngle == 0 && lastDPadAngle == -1){ + new SetPositionArmWrist(3605, 1144).start(); + } + if (dPadAngle == 90 && lastDPadAngle == -1){ + new SetPositionArmWrist(2000, 750).start(); + } + if (dPadAngle == 180 && lastDPadAngle == -1){ + new SetPositionArmWrist(590, 450).start(); + } + } + if (placeMode == PlaceMode.CARGO) { + if (dPadAngle == 0 && lastDPadAngle == -1){ + new SetPositionArmWrist(4298, 3243).start(); + } + if (dPadAngle == 90 && lastDPadAngle == -1){ + new SetPositionArmWrist(2830, 2830).start(); + } + if (dPadAngle == 180 && lastDPadAngle == -1){ + new SetPositionArmWrist(1388, 2500).start(); + } + } - - + if (dPadAngle == 270 && lastDPadAngle == -1){ + new StowArm().start(); + } + SmartDashboard.putNumber("DPad Angle", dPadAngle); + lastDPadAngle = dPadAngle; + } public synchronized void controlLoopUpdate() { // Measure *actual* update period @@ -284,6 +318,8 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; + + dPadButtons(); if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ resetEncoder(); 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 07ccd34..90cfea4 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 @@ -73,7 +73,7 @@ public class Wrist extends Subsystem implements ControlLoopable // Motor controllers private ArrayList motorControllers = new ArrayList(); - private TalonSRXEncoder wristMotor1; + public TalonSRXEncoder wristMotor1; // PID controller and params private MPTalonPIDController mpController; @@ -99,7 +99,7 @@ public class Wrist extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; - private double targetPositionInchesPID = 0; + public double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -138,6 +138,7 @@ public class Wrist extends Subsystem implements ControlLoopable } public void resetencoder(){ wristMotor1.setPosition(0); + targetPositionInchesPID = 0; } @@ -192,12 +193,13 @@ public class Wrist extends Subsystem implements ControlLoopable } private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { + + /*if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; } else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; - } + }*/ return targetPosition; } From df88b29380a88d2a1ec10e8def04d3be6ffc277b Mon Sep 17 00:00:00 2001 From: HFocus Date: Sun, 17 Mar 2019 20:17:01 -0600 Subject: [PATCH 3/5] fix compile errors --- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 84d16ad..2a70058 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 @@ -370,8 +370,8 @@ 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); + Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); } From 4b90f82ec10add569c949d9c3132405391789273 Mon Sep 17 00:00:00 2001 From: HFocus Date: Mon, 18 Mar 2019 16:58:49 -0600 Subject: [PATCH 4/5] Reenable old code --- 2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java | 2 +- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 2 +- .../main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 9e19634..7df5f21 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -122,7 +122,7 @@ public class OI //stow.whenPressed(new StowArm()); //stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); - //SmartDashboard.putData("switch to manuel", new SetManual()); + SmartDashboard.putData("switch to manuel", new SetManual()); // SmartDashboard.putData("run turn test", new TestTurn()); SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); 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 2a70058..e1f0cb2 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 @@ -370,7 +370,7 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesMM = targetPositionInchesMM + deltaPosition; updatePositionMM(targetPositionInchesMM); - Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); 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 7ebb2be..4e27452 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 @@ -194,12 +194,12 @@ public class Wrist extends Subsystem implements ControlLoopable private double limitPosition(double targetPosition) { - /*if (targetPosition < MIN_POSITION_INCHES) { + if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; } else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; - }*/ + } return targetPosition; } From e418c5b943fd1bb299775805d5b308aaa0b054bf Mon Sep 17 00:00:00 2001 From: HFocus Date: Mon, 18 Mar 2019 17:08:49 -0600 Subject: [PATCH 5/5] Merge edits --- .../frc4388/robot/commands/SetMMAndPID.java | 39 ------------------- .../frc4388/robot/commands/StowArm.java | 3 +- 2 files changed, 1 insertion(+), 41 deletions(-) delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java deleted file mode 100644 index e67d86c..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java +++ /dev/null @@ -1,39 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; -import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class SetMMAndPID extends CommandGroup { - /** - * Add your docs here. - */ - public SetMMAndPID() { - addSequential(new ArmSetMode(ArmControlMode.MOTION_MAGIC)); - addParallel(new WristSetMode(WristControlMode.JOYSTICK_PID)); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index 4465ea7..526bc8c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -18,8 +18,7 @@ public class StowArm extends CommandGroup { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); addParallel(new WristSetPositionPID(110), 2); - addSequential(new ArmSetPositionMM(420), 4); - addSequential(new ArmSetPositionMM(5)); + addSequential(new ArmSetPositionMM(10)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2());