Auto test

src/main/java/frc/robot/Constants.java

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
+import edu.wpi.first.wpilibj.XboxController.Button;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -12,4 +13,8 @@
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
-public final class Constants {}
+public final class Constants {
+    public static final int SWITCH_BUTTON = Button.kX.value;
+    public static final int OUT_TAKE =  Button.kRightBumper.value;
+    public static final int IN_TAKE =  Button.kLeftBumper.value;
+}

src/main/java/frc/robot/Robot.java

@@ -7,10 +7,8 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
-
   private RobotContainer m_robotContainer;
 
   @Override
@@ -21,6 +19,7 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+
   }
 
   @Override
@@ -35,14 +34,17 @@ public void disabledExit() {}
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
+    
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
   }
+
 
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+
+  }
 
   @Override
   public void autonomousExit() {}
@@ -53,9 +55,11 @@ public void teleopInit() {
       m_autonomousCommand.cancel();
     }
   }
-
+  
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+
+  }
 
   @Override
   public void teleopExit() {}

src/main/java/frc/robot/RobotContainer.java

@@ -5,16 +5,30 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.Autonomous;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Flywheel;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class RobotContainer {
+  public Flywheel flywheel = new Flywheel();
+  public XboxController controller = new XboxController(0);
+  private Drivetrain drivetrain = new Drivetrain(controller);
   public RobotContainer() {
     configureBindings();
   }
 
-  private void configureBindings() {}
+  private void configureBindings() {
+      new JoystickButton(controller, Constants.SWITCH_BUTTON).onTrue(new InstantCommand(()->{drivetrain.swap();}));
+      new JoystickButton(controller, Constants.OUT_TAKE).onTrue(new InstantCommand(()->{flywheel.out_take(1);}));
+      new JoystickButton(controller, Constants.OUT_TAKE).onFalse(new InstantCommand(() -> {flywheel.out_take(0);}));
+      new JoystickButton(controller, Constants.IN_TAKE).onTrue(new InstantCommand(()->{flywheel.intake(1);}));
+      new JoystickButton(controller, Constants.IN_TAKE).onFalse(new InstantCommand(() -> {flywheel.out_take(0);}));
+  }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return new Autonomous(drivetrain);
   }
 }

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

@@ -0,0 +1,47 @@
+// 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.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Drivetrain;
+public class Autonomous extends CommandBase {
+  /** Creates a new Autonomous. */
+  private Drivetrain drivetrain;
+  private final Timer timer = new Timer();
+  public Autonomous(Drivetrain dtrain) {
+    this.drivetrain = dtrain;
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drivetrain.isAuto = true;
+    timer.start();
+    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) {
+    timer.stop();
+    drivetrain.isAuto = false;
+    drivetrain.drive(0, 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (timer.get() > 5) {
+      return true;
+    }
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -0,0 +1,44 @@
+package frc.robot.subsystems;
+import org.carlmontrobotics.lib199.MotorControllerFactory;
+import org.carlmontrobotics.lib199.MotorConfig;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class Drivetrain extends SubsystemBase {
+    public boolean isAuto;
+    public boolean isTank = false;
+    private XboxController controller;
+    CANSparkMax rightMotor = MotorControllerFactory.createSparkMax(1,MotorConfig.NEO);
+    CANSparkMax leftMotor = MotorControllerFactory.createSparkMax(2,MotorConfig.NEO);
+
+    public Drivetrain(XboxController ctrlr){
+        controller = ctrlr;
+    }
+
+    public void drive(double left,double right) {
+        rightMotor.set(right);
+        leftMotor.set(left);
+    }
+
+    public void swap() {
+        isTank = !isTank;
+    }
+
+    @Override
+    public void periodic(){
+      if (!isAuto) {
+        double speed = -controller.getLeftY();
+        if (isTank) {
+          double turn = controller.getLeftX();
+          double left = speed - turn;
+          double right = speed + turn;
+          drive(left, right);
+        } else {
+          double rightYAxis = -controller.getRightY();
+          drive(speed,rightYAxis);
+        }
+      }
+    }
+}

src/main/java/frc/robot/subsystems/Flywheel.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems;
+
+import org.carlmontrobotics.lib199.MotorConfig;
+import org.carlmontrobotics.lib199.MotorControllerFactory;
+
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Flywheel extends SubsystemBase{
+    CANSparkMax flyWheel1 = MotorControllerFactory.createSparkMax(3,MotorConfig.NEO);
+    CANSparkMax flyWheel2 = MotorControllerFactory.createSparkMax(4, MotorConfig.NEO);
+    public void intake(double speed) {
+        flyWheel1.set(-speed);
+        flyWheel2.set(speed);
+    }
+    public void out_take(double speed) {
+        flyWheel1.set(speed);
+        flyWheel2.set(-speed);
+    }
+}