added autoroutines

src/main/java/frc/robot/commands/ArmWiggle.java

@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveTrain;
+import frc.robot.subsystems.Arm;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ArmWiggle extends CommandBase {
+
+//moves arm up and down twice
+
+private final Arm arm;
+private final Timer timer;
+
+  /** Creates a new DriveCwCircle. */
+  public ArmWiggle(Arm arm) {
+    timer = new Timer();
+    addRequirements(arm);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    for(int i = 0, i < 2; i++){
+      while(!arm.getLimitSwitch() && timer.get() < 2){
+        arm.moveUp();
+      }
+      while(timer.get() >= 2 && timer.get()< 4){
+        arm.moveDown();
+      }
+    }
+
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Autoroutine1.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class Autoroutine1 extends SequentialCommandGroup {
+  //drives in circles and spins a lot
+  /** Creates a new Autoroutine1. */
+  public Autoroutine1(DriveTrain drivetrain) {
+
+    addCommands(
+      new DriveCwCircle(drivetrain),
+      new DriveForward(drivetrain),
+      new DriveCcwCircle(drivetrain),
+      new Spin(drivetrain),
+      new TurnAround(drivetrain),
+      new DriveForward(drivetrain),
+      new TurnAround(drivetrain),
+      new Spin(drivetrain),
+    );
+  }
+}

src/main/java/frc/robot/commands/Autoroutine2.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class Autoroutine2 extends SequentialCommandGroup {
+  //Does a lot of arm moving up and down and also moves a bit
+  /** Creates a new Autoroutine2. */
+  public Autoroutine2(DriveTrain drivetrain, Arm arm) {
+
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+      new ArmWiggle(arm),
+      new DriveForward(drivetrain),
+      new ArmWiggle(arm),
+      new TurnAround(drivetrain),
+      new ArmWiggle(arm),
+      new DriveForward(drivetrain),
+      new TurnAround(drivetrain),
+      new ArmWiggle(arm)
+    );
+  }
+}

src/main/java/frc/robot/commands/DriveBackward.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.Timer;
+
+public class DriveBackwards extends CommandBase {
+  //drives robot backwards
+
+  private final DriveTrain drivetrain;
+  private final Timer timer;
+
+  /** Creates a new DriveCwCircle. */
+  public DriveBackwards(DriveTrain drivetrain) {
+    this.drivetrain = drivetrain;
+    timer = new Timer();
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(drivetrain);
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    timer.reset();
+    timer.start();
+    while(timer.get() < 2){
+      drivetrain.drive(-0.5,-0.5);
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
+

src/main/java/frc/robot/commands/DriveCcwCircle.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveCcwCircle extends CommandBase {
+  //makes robot drive in a circle counterclockwise
+
+  private final DriveTrain drivetrain;
+
+  /** Creates a new DriveCwCircle. */
+  public DriveCcwCircle(DriveTrain drivetrain) {
+    this.drivetrain = drivetrain;
+
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    @Override
+    public void initialize() {
+      double targetDegrees = 359;
+        while(drivetrain.getAngle() < targetDegrees){
+          drivetrain.drive(0.3,0.7);
+        }
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/DriveCwCircle.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveCwCircle extends CommandBase {
+  //makes robot drive in a circle clockwise
+  private final DriveTrain drivetrain;
+
+  /** Creates a new DriveCwCircle. */
+  public DriveCwCircle(DriveTrain drivetrain) {
+    this.drivetrain = drivetrain;
+
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    double targetDegrees = 359;
+      while(drivetrain.getAngle() < targetDegrees){
+        drivetrain.drive(0.7,0.3);
+      }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/DriveForward.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveForward extends CommandBase {
+  //makes robot go forward
+
+  private final DriveTrain drivetrain;
+  private final Timer timer;
+
+  /** Creates a new DriveCwCircle. */
+  public DriveForward(DriveTrain drivetrain) {
+    this.drivetrain = drivetrain;
+    timer  = new Timer();
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(drivetrain);
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    timer.reset();
+    timer.start();
+    while(timer.get() < 2){
+      drivetrain.drive(0.5,0.5);
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/GoBackAndForth.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class GoBackAndForth extends CommandBase {
+  //makes robot go backwards and forwards for 3 times
+  private final DriveTrain drivetrain;
+  private final Timer timer;
+
+  /** Creates a new GoBackAndForth. */
+  public GoBackAndForth(DriveTrain drivetrain) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.drivetrain = drivetrain;
+    timer = new Timer();
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    int direction = 1;
+
+    for(int i = 0; i < 6; i++){
+      timer.reset();
+      timer.start();
+      while(timer.get() < 2){
+        drivetrain.drive(direction*0.5, direction*0.5);
+        direction *= -1;
+      }
+    }
+
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}