diff --git a/2019robot/build.gradle b/2019robot/build.gradle index d4fbe1c..e9859af 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.4.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" 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 cec011f..8cec3ba 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -94,10 +94,12 @@ public class OI JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ + JoystickButton lowheight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + lowheight.whenPressed(new GrabFromLoadingSatation()); SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); - SmartDashboard.putData("run arm2", new ArmTest2()); + SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); //SmartDashboard.putData("PRE GAME!!!!", new PreGame()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java index b1c0654..ad87894 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java @@ -11,7 +11,7 @@ public class ArmSetPositionMM extends Command { private double targetPositionInches; private boolean isAtTarget; - private static final double MIN_DELTA_TARGET = 3; + private static final double MIN_DELTA_TARGET = 20; public ArmSetPositionMM(double targetPositionInches) { this.targetPositionInches = targetPositionInches; 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 new file mode 100644 index 0000000..eb7f0cc --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class GrabFromLoadingSatation extends CommandGroup { + /** + * Add your docs here. + */ + public GrabFromLoadingSatation() { + addSequential(new ArmSetPositionMM(600), 1); + //addParallel(new WaitCommand(1)); + addSequential(new WristSetPositionPID(450)); + // 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/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 81884da..00b9501 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 @@ -90,13 +90,13 @@ 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.0001;// 0.00030; - public static final double D_Value = 0;// 200; + public static final double I_Value = 0.0005;// 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; public static final double RampRate = 0;// 0.0; - public static final int A_value = 100; - public static final int CV_value = 340; + public static final int A_value = 450; + public static final int CV_value = 740; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 2e07a53..f10b70c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -680,7 +680,7 @@ public class Drive extends Subsystem implements ControlLoopable // m_steerInput = // OI.getInstance().getDriveTrainController().getRightXAxis(); m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); - m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); + m_steerInput = (OI.getInstance().getDriverController().getRightXAxis())*.8; if (controlMode == DriveControlMode.JOYSTICK) { m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, 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 4a9c63a..3f08e1f 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,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 = 2.3; - public static final double I_Value = 0.00004; - public static final double D_Value = 96; + public static final double P_Value = 3; + public static final double I_Value = 0.001; + public static final double D_Value = 200; 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;