added arm auto pos

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"

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());

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;

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.
+  }
+}

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;
 
 
 

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,

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;