motortest

src/main/java/frc4388/robot/Constants.java

@@ -175,4 +175,4 @@ public final class Constants {
     public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
 
   }
-}
+}

src/main/java/frc4388/robot/Robot.java

@@ -84,7 +84,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
-    m_robotContainer.m_robotSwerveDrive.resetGyro();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -103,7 +102,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopInit() {
-    m_robotContainer.m_robotSwerveDrive.resetGyro();
+
 
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
@@ -112,7 +111,7 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.m_robotSwerveDrive.resetGyro();
+    
     m_robotTime.startMatchTime();
 
     m_robotContainer.m_robotMap.restart_motor_tests();

src/main/java/frc4388/robot/RobotContainer.java

@@ -11,6 +11,7 @@ import java.util.function.Consumer;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import com.revrobotics.CANSparkMax;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.revrobotics.REVLibError;
 import com.revrobotics.CANSparkMax.FaultID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -24,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Arm;
+import frc4388.robot.subsystems.MotorTest;
 import frc4388.robot.subsystems.Claw;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -50,11 +52,7 @@ public class RobotContainer {
     public final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+
-                                                                  m_robotMap.rightFront,
-                                                                  m_robotMap.leftBack,
-                                                                  m_robotMap.rightBack,
-                                                                  m_robotMap.gyro);
     
     public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
 
@@ -68,23 +66,13 @@ public class RobotContainer {
 
     /* Autos */
     public SendableChooser<Command> chooser = new SendableChooser<>();
-
+    // TODO:  PUT PARAMETERS FOR MOTORS
-    // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
+    MotorTest m_motortest = new MotorTest(new WPI_TalonFX(1), new WPI_TalonFX(1));
-    
-    // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
-    // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-    
-    // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
-    // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-
-    // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
-
     private PlaybackChooser playbackChooser;
 
     /* Commands */
     private Command emptyCommand = new InstantCommand();
-    private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
+   
-
     private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
 
     private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
@@ -92,78 +80,8 @@ public class RobotContainer {
     private boolean readyForPlacement = false;
     private Boolean isPole = null;
 
-    private SequentialCommandGroup alignToPole =
-        new SequentialCommandGroup(
-            new RotateToAngle(m_robotSwerveDrive, 0.0),
-            new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
-            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
-            new RotateToAngle(m_robotSwerveDrive, 0.0),
-            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
-            // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
-        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
 
-    // private SequentialCommandGroup alignToShelf = 
-    //     new SequentialCommandGroup(
-    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
-    //         new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
-    //         new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
-    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
-    //         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
-    //     ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
-    
-    private SequentialCommandGroup alignToShelf = 
-        new SequentialCommandGroup(
-            new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
-            new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
-            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
-            new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
-            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
-        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
 
-    public SequentialCommandGroup place = null;
-
-    private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
-        place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
-    };
-
-    // TODO: find actual values
-    private SequentialCommandGroup placeCubeHigh =
-        new SequentialCommandGroup(
-            // new InstantCommand(() -> System.out.println("Placing cone high")),
-            new PivotCommand(m_robotArm, 64 + 135),
-            new InstantCommand(() -> m_robotArm.setRotVel(0)),
-            new TeleCommand(m_robotArm, 95642),
-            toggleClaw.asProxy(),
-            armToHome.asProxy()
-        );
-
-    private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 70 + 135),
-        new TeleCommand(m_robotArm, 32866),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
-
-    private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
-
-    private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
-
-    private SequentialCommandGroup placeLow = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 0),
-        new TeleCommand(m_robotArm, 0),
-        toggleClaw.asProxy(),
-        armToHome.asProxy()
-    );
 
 
     /**
@@ -172,30 +90,14 @@ public class RobotContainer {
     public RobotContainer() {
         configureButtonBindings();
 
-        // * Default Commands
-        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-                m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
-                                                  getDeadbandedDriverController().getRight(),
-                                                  true);
-            }, m_robotSwerveDrive)
-            .withName("SwerveDrive DefaultCommand"));
 
-        m_robotArm.setDefaultCommand(new RunCommand(() -> {
+        m_motortest.setDefaultCommand(new RunCommand(() -> {
-            m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
+            m_motortest.motorSetSpeed(getDeadbandedDriverController().getLeftX());
-            m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
         }, m_robotArm)
         .withName("Arm DefaultCommand"));
         
         // * Auto Commands
-        chooser.setDefaultOption("NoAuto", emptyCommand);
-        chooser.addOption("alignToPole", alignToPole);
-        chooser.addOption("alignToShelf", alignToShelf);
 
-        chooser.addOption("placeConeHigh", placeConeHigh);
-        chooser.addOption("placeConeMid", placeConeMid);
-        chooser.addOption("placeCubeHigh", placeCubeHigh);
-        chooser.addOption("placeCubeMid", placeCubeMid);
-        chooser.addOption("placeLow", placeLow);
 
         // chooser.addOption("Blue1Path", blue1Path);
         // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
@@ -203,9 +105,7 @@ public class RobotContainer {
         // chooser.addOption("Red1Path", red1Path);
         // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
 
-        playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+ 
-            .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
-            .buildDisplay();
     }
 
     // here be dragons - enter if you dare
@@ -222,14 +122,8 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
         // * Driver Buttons
-        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+      // * Driver Buttons
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
+       
-        
-        // because closure reasons - ask Daniel Thomas
-        // final TapState tap = new TapState();
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
-            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.setToTurbo()))
-            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
             // .onTrue(new InstantCommand(() -> {
             //     tap.gearTapped = true;
             //     tap.gearTime   = System.currentTimeMillis();
@@ -247,85 +141,8 @@ public class RobotContainer {
             //         m_robotSwerveDrive.setToSlow();
             // }));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+ 
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
-
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(interruptCommand.asProxy()); // final
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //     .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
-        //                                     () -> getDeadbandedDriverController().getLeftX(),
-        //                                     () -> getDeadbandedDriverController().getLeftY(),
-        //                                     () -> getDeadbandedDriverController().getRightX(),
-        //                                     () -> getDeadbandedDriverController().getRightY(),
-        //                                     "Blue1Path.txt"))
-        //     .onFalse(new InstantCommand());
-
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
-        //     .onFalse(new InstantCommand()); 
-
-        // * Operator Buttons
-        
-        // align (pole)
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
-            .onTrue(alignToPole)
-            .onFalse(interruptCommand.asProxy());
-        
-        // align (shelf)
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
-            .onTrue(alignToShelf)
-            .onFalse(interruptCommand.asProxy());
-
-        // toggle claw
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
-            .onTrue(toggleClaw.asProxy());
-        
-        // kill soft limits
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
-            .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
-
-        // SmartDashboard.putBoolean("isn't SparkMAX", );
-
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
-            .onTrue  (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
-            .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
-        
-        //Arm to Home
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
-            .onTrue(armToHome.asProxy());
-
-        //Interupt Button
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
-            .onTrue(interruptCommand.asProxy());
-
-
-        // place high
-        new POVButton(getDeadbandedOperatorController(), 0)
-            .onTrue(new ConditionalCommand(
-                new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
-                emptyCommand.asProxy(),
-                () -> readyForPlacement == true)
-            );
-
-        // place mid
-        new POVButton(getDeadbandedOperatorController(), 270)
-            .onTrue(new ConditionalCommand(
-                new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
-                emptyCommand.asProxy(),
-                () -> readyForPlacement == true)
-            );
-
-        // place low
-        new POVButton(getDeadbandedOperatorController(), 180)
-            .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
-
-        // confirm
-        new POVButton(getDeadbandedOperatorController(), 90)
-            .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
 
     }
 

src/main/java/frc4388/robot/subsystems/MotorTest.java

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+public class MotorTest extends SubsystemBase {
+  /** Creates a new MotorTest. */
+  WPI_TalonFX motor1;
+  WPI_TalonFX motor2;
+  public MotorTest(WPI_TalonFX motor1, WPI_TalonFX motor2) {
+    this.motor1 = motor1;
+    this.motor2 = motor2;
+  }
+  
+
+  public double motorSetSpeed(double speed) {
+    motor1.set(speed);
+    motor2.set(speed);
+    return speed;
+  }
+  public void stop() {
+        motor1.set(0);
+        motor2.set(0);
+  }
+
+
+}