From e48da51e7089987b9139ea96ddba82a990eb48d4 Mon Sep 17 00:00:00 2001 From: HFocus Date: Mon, 18 Mar 2019 17:45:55 -0600 Subject: [PATCH] Create Presets --- .../java/org/usfirst/frc4388/robot/OI.java | 1 + .../robot/commands/presets/CargoHigh.java | 41 +++++++++++++++++++ .../robot/commands/presets/CargoLow.java | 41 +++++++++++++++++++ .../robot/commands/presets/CargoMid.java | 41 +++++++++++++++++++ .../robot/commands/presets/HatchHigh.java | 41 +++++++++++++++++++ .../robot/commands/presets/HatchLow.java | 41 +++++++++++++++++++ .../robot/commands/presets/HatchMid.java | 41 +++++++++++++++++++ .../{ => presets}/SetPositionArmWrist.java | 5 ++- .../robot/commands/{ => presets}/StowArm.java | 7 +++- .../usfirst/frc4388/robot/subsystems/Arm.java | 22 ++++++---- 10 files changed, 271 insertions(+), 10 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ => presets}/SetPositionArmWrist.java (88%) rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ => presets}/StowArm.java (83%) 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 cea2dc6..fe06745 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -7,6 +7,7 @@ import buttons.XBoxTriggerButton; import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.commands.presets.SetPositionArmWrist; import org.usfirst.frc4388.robot.constants.LEDPatterns; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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 new file mode 100644 index 0000000..920b755 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class CargoHigh extends CommandGroup { + /** + * Add your docs here. + */ + public CargoHigh() { + + addParallel(new WristSetPositionPID(3243)); + addSequential(new ArmSetPositionMM(4298)); + + // 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/presets/CargoLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java new file mode 100644 index 0000000..d636586 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class CargoLow extends CommandGroup { + /** + * Add your docs here. + */ + public CargoLow() { + + addParallel(new WristSetPositionPID(2500)); + addSequential(new ArmSetPositionMM(1388)); + + // 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/presets/CargoMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java new file mode 100644 index 0000000..ec71323 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class CargoMid extends CommandGroup { + /** + * Add your docs here. + */ + public CargoMid() { + + addParallel(new WristSetPositionPID(2830)); + addSequential(new ArmSetPositionMM(2830)); + + // 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/presets/HatchHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java new file mode 100644 index 0000000..a78c8f3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class HatchHigh extends CommandGroup { + /** + * Add your docs here. + */ + public HatchHigh() { + + addParallel(new WristSetPositionPID(1144)); + addSequential(new ArmSetPositionMM(3605)); + + // 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/presets/HatchLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java new file mode 100644 index 0000000..fd85278 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class HatchLow extends CommandGroup { + /** + * Add your docs here. + */ + public HatchLow() { + + addParallel(new WristSetPositionPID(450)); + addSequential(new ArmSetPositionMM(590)); + + // 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/presets/HatchMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java new file mode 100644 index 0000000..b5660ac --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class HatchMid extends CommandGroup { + /** + * Add your docs here. + */ + public HatchMid() { + + addParallel(new WristSetPositionPID(750)); + addSequential(new ArmSetPositionMM(2000)); + + // 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/presets/SetPositionArmWrist.java similarity index 88% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java index 6eecaab..fe64532 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java @@ -5,7 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package org.usfirst.frc4388.robot.commands; +package org.usfirst.frc4388.robot.commands.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java similarity index 83% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java index 526bc8c..544ba95 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java @@ -5,7 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package org.usfirst.frc4388.robot.commands; +package org.usfirst.frc4388.robot.commands.presets; + +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.HatchFlip; +import org.usfirst.frc4388.robot.commands.WristPlacement; +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; 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 941092f..fc151f4 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 @@ -5,8 +5,14 @@ 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.robot.commands.presets.CargoHigh; +import org.usfirst.frc4388.robot.commands.presets.CargoLow; +import org.usfirst.frc4388.robot.commands.presets.CargoMid; +import org.usfirst.frc4388.robot.commands.presets.HatchHigh; +import org.usfirst.frc4388.robot.commands.presets.HatchLow; +import org.usfirst.frc4388.robot.commands.presets.HatchMid; +import org.usfirst.frc4388.robot.commands.presets.SetPositionArmWrist; +import org.usfirst.frc4388.robot.commands.presets.StowArm; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPTalonPIDController; @@ -283,24 +289,24 @@ public class Arm extends Subsystem implements ControlLoopable int dPadAngle = Robot.oi.getOperatorController().getDpadAngle(); if (placeMode == PlaceMode.HATCH){ if (dPadAngle == 0 && lastDPadAngle == -1){ - new SetPositionArmWrist(3605, 1144).start(); + new HatchHigh().start(); } if (dPadAngle == 90 && lastDPadAngle == -1){ - new SetPositionArmWrist(2000, 750).start(); + new HatchMid().start(); } if (dPadAngle == 180 && lastDPadAngle == -1){ - new SetPositionArmWrist(590, 450).start(); + new HatchLow().start(); } } if (placeMode == PlaceMode.CARGO) { if (dPadAngle == 0 && lastDPadAngle == -1){ - new SetPositionArmWrist(4298, 3243).start(); + new CargoHigh().start(); } if (dPadAngle == 90 && lastDPadAngle == -1){ - new SetPositionArmWrist(2830, 2830).start(); + new CargoMid().start(); } if (dPadAngle == 180 && lastDPadAngle == -1){ - new SetPositionArmWrist(1388, 2500).start(); + new CargoLow().start(); } }