Merge pull request #28 from Team6593/CommandRewrite

final day changes
Team6593 · Mar 22, 2023 · 268e5d0 · 268e5d0
2 parents 464fc97 + e642379
commit 268e5d0
Show file tree

Hide file tree

Showing 6 changed files with 25 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -126,7 +126,7 @@ public static final class DoubleSolenoidChannels{
 
     }
     public static final class Autonomous {
-        public final double encoderDistanceToChargeStation = 1000; //couple feet or so, change later
+        public final double encoderDistanceToChargeStation = -600; //couple feet or so, change later
         public final double levelDegrees = -71;
         public final double limelightLensHeightInches = 52;
         public final double limelightMountAngleDegrees = 0;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -47,6 +47,7 @@
 
 import frc.robot.commands.autonomous.BalanceOnChargeStation;
 import frc.robot.commands.autonomous.DriveToChargeStation;
+import frc.robot.commands.autonomous.TaxiWithGyro;
 import frc.robot.commands.drivetrain.DriveTrain_DefaultCommnad;
 import frc.robot.commands.drivetrain.HighGear;
 import frc.robot.commands.drivetrain.LowGear;
@@ -223,15 +224,15 @@ private void configureButtonBindings() {
    */
   public Command getAutonomousCommand() {
     //return new DoNothing();
-    return new AutonomousScoring(reeler, elevator, arm);
+    //return new AutonomousScoring(reeler, elevator, arm);
     //.andThen(new DriveToChargeStation(driveTrain, autonomous.encoderDistanceToChargeStation))
     //.andThen(new BalanceOnChargeStation(driveTrain, navX));
 
-     //return new DriveToChargeStation(driveTrain, autonomous.encoderDistanceToChargeStation)
+     //return new DriveToChargeStation(driveTrain, autonomous.encoderDistanceToChargeStation);
      //.andThen(new BalanceOnChargeStation(driveTrain, navX));
 
-    // IF THEzzsd ABOVE AUTON COMMAND DOESN'T WORK USE THE OLD COMMAND HERE:
-    //new TaxiWithGyro(driveTrain, .2); 
+    // IF THE ABOVE AUTON COMMAND DOESN'T WORK USE THE OLD COMMAND HERE:
+    return new TaxiWithGyro(driveTrain, .16); 
     // taxi backwards for 5 seconds then stop
     // might have to invert motorspeed to a negative
 

diff --git a/src/main/java/frc/robot/commands/DoNothing.java b/src/main/java/frc/robot/commands/DoNothing.java
@@ -33,6 +33,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return true;
   }
 }
diff --git a/src/main/java/frc/robot/commands/RefactoredCommands/HumanStation.java b/src/main/java/frc/robot/commands/RefactoredCommands/HumanStation.java
@@ -47,7 +47,7 @@ public void execute() {
       elevator.elevatorStop();
       elevator.elevatorBrake();
     } else if (elevator.maxHeightLimitSwitch.get() == true) {
-      reeler.reelArmUp(.16 * 3.0);
+      reeler.reelArmUp(.22 * 3.0);
       arm.stopArmMotor();
       elevator.elevate(-.18 * 3.0);
     }

diff --git a/src/main/java/frc/robot/commands/autonomous/DriveToChargeStation.java b/src/main/java/frc/robot/commands/autonomous/DriveToChargeStation.java
@@ -41,6 +41,10 @@ public void execute() {
     if (!done) {
       if (rightMotorPosition_RECU < encoderDistance) {
         driveTrain.drive(.3);
+      } else if (encoderDistance < 0) {
+        if (rightMotorPosition_RECU > encoderDistance) {
+          driveTrain.drive(-.3);
+        } else {driveTrain.stopAllMotors();}
       } else {
         driveTrain.stopAllMotors();
         done = true;

diff --git a/src/main/java/frc/robot/commands/autonomous/TaxiWithGyro.java b/src/main/java/frc/robot/commands/autonomous/TaxiWithGyro.java
@@ -7,12 +7,10 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DriveTrain;
-import frc.robot.subsystems.NavX;
 
 public class TaxiWithGyro extends CommandBase {
 
   private DriveTrain driveTrain;
-  private NavX navX;
   double motorSpeed;
   Timer timer = new Timer();
   double startTime;
@@ -30,9 +28,9 @@ public TaxiWithGyro(DriveTrain driveTrain, double motorSpeed) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    driveTrain.dtInit();
     startTime = Timer.getFPGATimestamp();
     //driveTrain.dtInit();
-    navX.reset();
     //timer.reset();
     timer.start();
   }
@@ -41,17 +39,18 @@ public void initialize() {
   @Override
   public void execute() {
 
-    // double time = Timer.getFPGATimestamp();
-    // if (timer.getFPGATimestamp() - startTime < 5) {
-    //   System.out.println(timer.getFPGATimestamp());
-    //   // negative if backwards
-    //   // positive if forwards
-    //   driveTrain.autonDrive(-motorSpeed);
-    // } else {
-    //   System.out.println(timer.getFPGATimestamp());
-    //   driveTrain.stopAllMotors();
-    //   System.out.println(" auton stopped");
-    // }
+    double time = Timer.getFPGATimestamp();
+
+    if (timer.getFPGATimestamp() - startTime < 5) {
+      System.out.println(timer.getFPGATimestamp());
+      // negative if backwards
+      // positive if forwards
+      driveTrain.drive(motorSpeed);
+    } else {
+      System.out.println(timer.getFPGATimestamp());
+      driveTrain.stopAllMotors();
+      System.out.println(" auton stopped");
+    }
   }
 
   // Called once the command ends or is interrupted.