Added commands for grabbing ball and hatch out of robot

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

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.usfirst.frc4388.robot.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class FlipIntake extends Command 
+{
+  public FlipIntake() 
+  {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() 
+  {
+    Robot.wrist.controlPIDFlipIntake();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() 
+  {
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() 
+  {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() 
+  {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() 
+  {
+  }
+}

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

@@ -8,33 +8,34 @@
 package org.usfirst.frc4388.robot.commands;
 
 import org.usfirst.frc4388.robot.Robot;
-import edu.wpi.first.wpilibj.command.CommandGroup;
 
-public class GrabBallOutOfRobot extends CommandGroup {
-  /**
-   * Add your docs here.
-   */
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabBallOutOfRobot extends CommandGroup 
+{
   public GrabBallOutOfRobot() 
   {
+    //Add rest of sequentials for this command group
+
+    addSequential(new FlipIntake());
+    //Move Arm until bar jump angle 
+
+    //Jump bar in robot
     if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle)
     {
-      
+      addParallel(new WaitCommand(2));
+      addSequential(new RunWristMotorJumpBar());
+      addSequential(new StopWristMotor());
     }
-    // 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.
+    //continue arm movement until ball is in intake ///SENSOR for ball in robot
 
-    // 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.
+    //Move ball out of robot to 360 degrees
+      //move arm and wrist in parallel
   }
 }

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

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabHatchOffWall extends CommandGroup 
+{
+  public GrabHatchOffWall() {
+    // 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/commands/GrabHatchOutOfFloorIntake.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;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabHatchOutOfFloorIntake extends CommandGroup 
+{
+  public GrabHatchOutOfFloorIntake() 
+  {
+    // 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/commands/RunWristMotorJumpBar.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.usfirst.frc4388.robot.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class RunWristMotorJumpBar extends Command 
+{
+  public RunWristMotorJumpBar() 
+  {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() 
+  {
+    Robot.wrist.jumpBar();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() 
+  {
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() 
+  {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() 
+  {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() 
+  {
+  }
+}

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

@@ -7,33 +7,41 @@
 
 package org.usfirst.frc4388.robot.commands;
 
+import org.usfirst.frc4388.robot.*;
+
 import edu.wpi.first.wpilibj.command.Command;
 
-public class WristAvoidBar extends Command {
-  public WristAvoidBar() {
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
+public class StopWristMotor extends Command 
+{
+  public StopWristMotor() 
+  {
+    requires(Robot.wrist);
   }
 
   // Called just before this Command runs the first time
   @Override
-  protected void initialize() {
+  protected void initialize() 
+  {
+    Robot.wrist.stopMotor();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
-  protected void execute() {
+  protected void execute() 
+  {
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
-  protected boolean isFinished() {
+  protected boolean isFinished() 
+  {
     return false;
   }
 
   // Called once after isFinished returns true
   @Override
-  protected void end() {
+  protected void end() 
+  {
   }
 
   // Called when another command which requires one or more of the same

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

@@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SensorCollection;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import edu.wpi.first.wpilibj.command.WaitCommand;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
@@ -105,6 +106,19 @@ public class Wrist extends Subsystem
     }
   }
 
+  //Jump bar by putting power to the motors for a specific amount of time
+  //Jump bar output
+  public void jumpBar()
+  {
+    wristRight.set(0.8);
+  }
+
+  //Stop wrist motor
+  public void stopMotor()
+  {
+    wristRight.set(0);
+  }
+
   //Method for setting the control mode for the wrist
   private synchronized void setWristControlMode(WristControlMode controlMode) 
   {