arm changes

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -49,8 +49,8 @@ public class Robot extends IterativeRobot
     private Command RRautonomousCommand;
     private Command RLautonomousCommand;
     private Command LRautonomousCommand;
-    private Command LLautonomousCommand;
-    
+	private Command LLautonomousCommand;
+
     public void robotInit() 
     {
     	//Printing game data to riolog
@@ -70,16 +70,14 @@ public class Robot extends IterativeRobot
 	    operationModeChooser.addDefault("Test", OperationMode.TEST);
 	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
 		SmartDashboard.putData("Operation Mode", operationModeChooser);
-		
-		
-		
-		
-		
-		
 
-		
+		//Initializing the angle of the arm to be 
+		arm.setInitAngle();
+
 		//ledLights.setAllLightsOn(false);
-      } catch (Exception e) {
+	  } 
+	  catch (Exception e) 
+	  {
     		System.err.println("An error occurred in robotInit()");
       }
     }

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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 GrabBallOutOfRobot extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public GrabBallOutOfRobot() 
+  {
+    // 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.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java

@@ -28,6 +28,10 @@ public class Arm extends Subsystem implements Loop
 	// One revolution of the 1-3 GEAR RATION ON THE ARM	* 4096 ticks 
 	public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI);   
 	
+	public double ARM_ANGLE_DEGREES = 0;
+
+	// Defined speeds
+	public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
 	
 	// Defined positions
 	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
@@ -83,6 +87,15 @@ public class Arm extends Subsystem implements Loop
 		}
 	}
 
+	//Set the degree to negative angle after initializing 
+	public void setInitAngle()
+	{
+	  double armAngleToHoriz = 70;
+	  double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz;
+	  
+	  ARM_ANGLE_DEGREES = initAngle;
+	}
+
 	@Override
 	public void initDefaultCommand() {
 	}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java

@@ -15,6 +15,7 @@ import org.usfirst.frc4388.utility.CANTalonEncoder;
 import org.usfirst.frc4388.utility.ControlLoopable;
 import org.usfirst.frc4388.utility.PIDParams;
 import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
+import org.usfirst.frc4388.robot.subsystems.Arm;
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
@@ -32,7 +33,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
 public class Wrist extends Subsystem
 {
   //Control Mode Array
-  public static enum WristControlMode {PID, JOYSTICK_MANUAL};
+  public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL};
 
   //Motor Controllers
   private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -60,18 +61,20 @@ public class Wrist extends Subsystem
 	public static final double MIN_POSITION_INCHES = 0.0;
   public static final double MAX_POSITION_INCHES = 83.4;
 
-  public static final double MIN_ANGLE_DEGREES = 0.0;   ////FIND ANGLE VALUES
-  public static final double MAX_ANGLE_DEGREES = 0.0;
+  public static final double MIN_ANGLE_DEGREES = -3;   ////FIND ANGLE VALUES
+  public static final double MAX_ANGLE_DEGREES = 3;
 
   public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
   public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
   public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
+
+  public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES;
   
   //Misc
   private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
   private boolean isFinished;
   private double targetPositionInchesPID = 0;
-  private double targetAngleDegreesPID = 0;
+  private double targetAngleDegreesPID = -(180 - armAngleDegrees);
 
   private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
   private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;