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 e736050..b08f4fb 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -98,12 +98,16 @@ public class OI JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ + JoystickButton Height1 = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.RIGHT_JOYSTICK_BUTTON); + Height1.whenPressed(new ArmToHeight1()); + + JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON); lowHeight.whenPressed(new GrabFromLoadingSatation()); JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); stow.whenPressed(new StowArm()); - stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_RED)); + stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); SmartDashboard.putData("switch to manuel", new SetManual()); // SmartDashboard.putData("run turn test", new TestTurn()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java new file mode 100644 index 0000000..14d38a4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.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; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class ArmToHeight1 extends CommandGroup { + /** + * Add your docs here. + */ + public ArmToHeight1() { + addSequential(new HatchFlip(false)); + addParallel(new WristPlacement(true)); + addParallel(new ArmSetPositionMM(1300), 4); + addSequential(new WaitCommand(1)); + addSequential(new WristSetPositionPID(2300), 2); + // 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/GrabFromLoadingSatation.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java index eb7f0cc..9878e62 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java @@ -15,7 +15,7 @@ public class GrabFromLoadingSatation extends CommandGroup { * Add your docs here. */ public GrabFromLoadingSatation() { - addSequential(new ArmSetPositionMM(600), 1); + addSequential(new ArmSetPositionMM(550), 1); //addParallel(new WaitCommand(1)); addSequential(new WristSetPositionPID(450)); // e.g. addSequential(new Command1()); 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 239bee8..1f2c239 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 @@ -9,6 +9,7 @@ package org.usfirst.frc4388.robot.commands; import edu.wpi.first.wpilibj.command.CommandGroup; + public class StowArm extends CommandGroup { /** * Add your docs here. @@ -16,8 +17,8 @@ public class StowArm extends CommandGroup { public StowArm() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addParallel(new WristSetPositionPID(250), 2); - addSequential(new ArmSetPositionMM(10), 4); + addParallel(new WristSetPositionPID(200), 2); + addSequential(new ArmSetPositionMM(-10), 4); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); 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..44733a9 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 @@ -46,14 +46,14 @@ public class Arm extends Subsystem implements ControlLoopable public static final double TEST_SPEED_UP = 0.3; 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 = 20; + public static final double JOYSTICK_INCHES_PER_MS_HI = 35; + public static final double JOYSTICK_INCHES_PER_MS_LO = 35; // Defined positions 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 = 0.0; + public static final double MIN_POSITION_INCHES = -10; public static final double MAX_POSITION_INCHES = 3900; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; @@ -69,7 +69,7 @@ public class Arm extends Subsystem implements ControlLoopable // Motion profile max velocities and accel times public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; - public static final double MP_T1 = 400; // Fast = 300 + public static final double MP_T1 = 300; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 // Motor controllers @@ -90,7 +90,7 @@ public class Arm extends Subsystem implements ControlLoopable public static final double KF_UP = 1;//0.01; public static final double KF_DOWN = 0;// 0.0; public static final double P_Value = 0.5;// 2; - public static final double I_Value = 0.0005;// 0.00030; + public static final double I_Value = 0.0008;// 0.00030; public static final double D_Value = 100;// 200; public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms) public static final double maxGravityComp = 0.08; 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 de816df..07ccd34 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 @@ -45,7 +45,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 = 20; + public static final double JOYSTICK_INCHES_PER_MS_LO = 27; // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; @@ -85,9 +85,9 @@ 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 = 3; - public static final double I_Value = 0.001; - public static final double D_Value = 300; + public static final double P_Value = 1; + public static final double I_Value = 0.0001; + public static final double D_Value = 100; public static final double RampRate = 0.0; private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 150;