Random auto stuff

2020Code/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.2.2"
+    id "edu.wpi.first.GradleRIO" version "2020.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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

@@ -7,6 +7,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants.  This class should not be used for any other purpose.  All constants should be
@@ -19,5 +21,16 @@ public final class Constants {
     public static final class DebugConstants {
         public static boolean debugMode = false;
     }
-
+    public static final class DriveConstants {
+        public static final double ksVolts = 0.22;
+        public static final double kvVoltSecondsPerInch = 1.98;
+        public static final double kaVoltSecondsSquaredPerInch = 0.2;
+        public static final double kPDriveVel = 8.5;
+        public static final double kTrackwidthInches = 0.69;
+        public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthInches);
+        public static final double kMaxSpeedInchesPerSecond = 3;
+        public static final double kMaxAccelerationInchesPerSecondSquared = 3;
+        public static final double kRamseteB = 2;
+        public static final double kRamseteZeta = 0.7;
+    }
 }

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

@@ -70,6 +70,8 @@ public void robotPeriodic() {
    */
   @Override
   public void disabledInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.PneumaticsT.setCompressor(true);
   }
 
   @Override
@@ -81,6 +83,9 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.ClimbT.setClimb(false);
+    frc.robot.RobotContainer.PneumaticsT.setCompressor(false);
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -98,12 +103,13 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    frc.robot.RobotContainer.ClimbT.setClimb(false);
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
     if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+      //m_autonomousCommand.cancel();
     }
   }
 

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

@@ -25,16 +25,15 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
+  public static DiskControl DiskControlT = new DiskControl();
+  public static Navx NavxT = new Navx();
   public static Drivetrain DrivetrainT = new Drivetrain();
   public static Pneumatics PneumaticsT = new Pneumatics();
   public static ShooterLimelight ShooterLimelightT = new ShooterLimelight();
-  public static DiskControl DiskControlT = new DiskControl();
-  public static Navx NavxT = new Navx();
   public static Intake IntakeT = new Intake();
   public static Indexer IndexerT = new Indexer();
   public static Climb ClimbT = new Climb();
   public static Shooter ShooterT = new Shooter();
-  public static BallLimelight BallLimelightT = new BallLimelight();
   //public static Lights LightsT = new Lights();
 
   public static XboxController m_driverController = new XboxController(0);
@@ -48,6 +47,8 @@ public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
     DrivetrainT.setDefaultCommand(new DriveWithJoystick());
+    IntakeT.setDefaultCommand(new ControlIntake());
+    ShooterT.setDefaultCommand(new TurretWithJoystick());
   }
 
   /**
@@ -60,12 +61,16 @@ private void configureButtonBindings() {
     //final JoystickButton a = new JoystickButton(m_joystick, 1);
     //final JoystickButton b = new JoystickButton(m_joystick, 2);
     // final JoystickButton x = new JoystickButton(m_joystick, 3);
-    final JoystickButton y = new JoystickButton(m_joystick, 4);
+    final JoystickButton leftBumper = new JoystickButton(m_joystick, 4);
+    final JoystickButton climbButtonUp = new JoystickButton(m_driverController, 8);
+    final JoystickButton climbButtonDown = new JoystickButton(m_driverController, 7);
 
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
-    // x.whenPressed(new AutoTest());
-    y.toggleWhenPressed(new TargetPort());
+    //x.whenPressed(new AutoTest());
+    leftBumper.toggleWhenPressed(new TargetPort());
+    climbButtonUp.whenPressed(new SetClimb(true));
+    climbButtonDown.whenPressed(new SetClimb(false));
   }
 
 
@@ -75,7 +80,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return new AutoTest();
+    return new AutoTest(); //.withTimeout(20);
   }
 }

2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class AutoControlIntake extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double m_intakeSpeed;
+  double m_indexerSpeed;
+  boolean m_intakeState;
+  public AutoControlIntake(double intakeSpeed, double indexerSpeed, boolean intakeState) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.IntakeT);
+    addRequirements(frc.robot.RobotContainer.IndexerT);
+    m_intakeSpeed = intakeSpeed;
+    m_indexerSpeed = indexerSpeed;
+    m_intakeState = intakeState;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    frc.robot.RobotContainer.IntakeT.setPiston(m_intakeState);
+    if(m_intakeSpeed < 0 && frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(0);
+    } 
+    else if(m_intakeSpeed < 0 && !frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(0);
+    }
+    else{
+      frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
+    }
+    frc.robot.RobotContainer.IntakeT.setMotor(m_intakeSpeed);
+  }
+
+  // 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;
+  }
+}

2020Code/src/main/java/frc/robot/commands/AutoTest.java

@@ -11,11 +11,64 @@
 
 public class AutoTest extends SequentialCommandGroup {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  public static final double DriveDist = 150; //scale 106%
+  public static final double TurnAngle = 90;
+
+
   public AutoTest() {
     addCommands(
-      new DriveDistance(100),
-      new TurnAngle(-90),
-      new DriveArc(40, 60)
+      new DriveDistance(150), //move forward first step
+      new DriveArc(30, 360)
+      //new TurnAngle(90), //turn right 90 degrees 
+
+      // new DriveDistance(50), //move forward 
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(70), //drive forward
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(70), //drive forward
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(175), //move forward 
+
+      // new TurnAngle(-90), //turn left 90 degrees
+
+      // new DriveDistance(50), //drive forward
+
+      // new TurnAngle(-90), //turn left 90 degrees 
+
+      // new DriveDistance(50), //drive forward 
+
+      // new TurnAngle(-90), //turn left 90 degrees
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 90 
+
+      // new DriveDistance(100), //move forward
+
+      // new TurnAngle(90), //turn right 90 
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 90
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left
+
+      // new DriveDistance(250) //back to the start!
+
+     //  new DriveArc(20, 360)
     );
   }
 }

2020Code/src/main/java/frc/robot/commands/AutoTest2.java

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class AutoTest2 extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+    public static final double DriveDist = 100; 
+
+    public AutoTest2() {
+        addCommands(
+            new DriveArc(55, -68), //90 degree circle counterclockwise
+            new DriveArc(15, 59), //90 degree circle clockwise
+            new DriveDistance(100), //move forward 125 inches
+            new DriveArc(45, 65),  //90 degree circle clockwise
+            new DriveArc(45, 280), //full circle 360 degrees 
+            new DriveArc(45, -65), //90 degree circle counterclockwise
+            new DriveDistance(100), //move forward 125 inches
+            new DriveArc(15, 59), //90 degree circle clockwise
+            new DriveArc(55, -68)  //90 degree circle counterclockwise
+            ); 
+        }   
+    }

2020Code/src/main/java/frc/robot/commands/AutoTest3.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class AutoTest3 extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+    public static final double DriveDistance = 65; 
+
+    public AutoTest3() {
+        addCommands(
+           new DriveArc(45, -90), //90 degree circle counterclowise
+           new TurnAngle(171), //170 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(40, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new TurnAngle(180), //Turn 180 degrees
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(45, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new TurnAngle(180), //Turn 180 degrees
+           new DriveArc(35, -90) //90 degrees counterclockwise
+            ); 
+        }   
+    }

2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class BarrelRunTest extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+
+    public BarrelRunTest() {
+        addCommands(
+                new DriveDistance(90), //move forward 7.5 feet
+                new DriveArc(30, 340), //do a full circle clockwise
+                new DriveDistance(90), //move forward 90 inches
+                new DriveArc(40, -315), //rotate 315 degrees counterclockwise
+                new DriveDistance(85), //move forward 85 inches
+                new DriveArc(22.5, -225), //rotate 225 degrees counterclockwise
+                new DriveDistance(240) //move forward 240 feet
+            ); 
+        }   
+    }

2020Code/src/main/java/frc/robot/commands/ChooseAutoTest.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+public class ChooseAutoTest extends CommandBase {
+        private final Command m_BarrelRunTest = new BarrelRunTest();
+        private final Command m_SlalomPath = new AutoTest2();
+        private final Command m_BouncePath = new AutoTest3();
+
+        SendableChooser<Command> pathChooser = new SendableChooser<Command>();
+
+    public ChooseAutoTest(){
+        pathChooser.setDefaultOption("Barrel Run", m_BarrelRunTest);
+        pathChooser.addOption("Slalom Path", m_SlalomPath);
+        pathChooser.addOption("Bounce Path", m_BouncePath);
+        SmartDashboard.putData(pathChooser);
+
+    }
+}
+
+
+