Autos

Author: JoshuaAN

.vscode/launch.json

@@ -4,6 +4,13 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
+    {
+      "type": "java",
+      "name": "Launch Test",
+      "request": "launch",
+      "mainClass": "frc.robot.drive.commands.Test",
+      "projectName": "RapidReact"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",

src/main/java/frc/paths/FiveBallPartTwo.java

@@ -55,6 +55,7 @@
 import frc.robot.drive.Drivetrain;
 import frc.robot.drive.commands.AbsoluteOrientation;
 import frc.robot.drive.commands.JoystickDrive;
+import frc.robot.drive.commands.MotionProfileTurn;
 import frc.robot.drive.commands.RelativeOrientation;
 import frc.robot.drive.commands.ResetEncoders;
 import frc.robot.drive.commands.TurnToAngle;
@@ -342,9 +343,10 @@ public void configureButtonBindings() {
       //                       .alongWith(new TurnOffLEDs(mLimelight))
       //                       .alongWith(new IdleCommand()));
 
-      xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight));
+      // xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight));
+      xBoxX.whenHeld(new MotionProfileTurn(mDrive, Math.PI/2));
 
-      xBoxX.whenReleased(new StopShooter(mShooter));
+      // xBoxX.whenReleased(new StopShooter(mShooter));
 
       JoystickButton xBoxY = new JoystickButton(operator, X_BOX_Y);
       xBoxY.whenHeld(new PresetFlywheelController(mShooter, "TUR")

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

@@ -38,15 +38,14 @@
 public class FiveBallAuto extends SequentialCommandGroup {
   public FiveBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
     addCommands(
-      new ResetOdometry(drive, new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(-90.0))),
+      new ResetOdometry(drive, new Pose2d(new Translation2d(-0.7, 0), Rotation2d.fromDegrees(-90.0))),
       new ResetEncoder(shooter),
       new ParallelDeadlineGroup(
         new SequentialCommandGroup(
             new WaitCommand(0.8), // Give shooter time to spin up & hood to move
             new PullTrigger(shooter),
             new WaitCommand(0.5)),
         // new ActionCommand(new ImageAction("THfade.png", 0.01).brightness(0.7)),
-        new TrajectoryFollower(drive, new FiveBallPartOne()), // Turn to point at center
         new FlywheelController(shooter, 1810, 77.90)),
     new ParallelDeadlineGroup(
       new StopShooter(shooter),
@@ -65,27 +64,27 @@ public FiveBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
         new WaitCommand(4.0),
         new RetractIntake(intake))),
     new StopShooter(shooter),
-    new StopTrigger(shooter),
-    new ParallelDeadlineGroup(
-      new TrajectoryFollower(drive, new FiveBallPartThree()),
-      new FastIntake(intake)),
-    new WaitCommand(0.9), // Pick up balls 4 & 5
-    new ParallelDeadlineGroup(
-      new WaitCommand(4.5),
-      new SequentialCommandGroup(
-        new WaitCommand(1.75),
-        new FlywheelController(shooter, 1795, 77.60)),
-      new SequentialCommandGroup(
-        new WaitCommand(2.9),
-        new PullTrigger(shooter)),
-      new SequentialCommandGroup(
-        new WaitCommand(1.5),
-        new RetractIntake(intake)),
-      new TrajectoryFollower(drive, new FiveBallPartFour())),
-    new StopShooter(shooter),
-    new StopTrigger(shooter),
-    new ResetHood(shooter),
-    new SetColor(Status.getInstance(), Color.kBlack)
+    new StopTrigger(shooter)
+    // new ParallelDeadlineGroup(
+    //   new TrajectoryFollower(drive, new FiveBallPartThree()),
+    //   new FastIntake(intake)),
+    // new WaitCommand(0.9), // Pick up balls 4 & 5
+    // new ParallelDeadlineGroup(
+    //   new WaitCommand(4.5),
+    //   new SequentialCommandGroup(
+    //     new WaitCommand(1.75),
+    //     new FlywheelController(shooter, 1795, 77.60)),
+    //   new SequentialCommandGroup(
+    //     new WaitCommand(2.9),
+    //     new PullTrigger(shooter)),
+    //   new SequentialCommandGroup(
+    //     new WaitCommand(1.5),
+    //     new RetractIntake(intake)),
+    //   new TrajectoryFollower(drive, new FiveBallPartFour())),
+    // new StopShooter(shooter),
+    // new StopTrigger(shooter),
+    // new ResetHood(shooter),
+    // new SetColor(Status.getInstance(), Color.kBlack)
     );
   }
 }

src/main/java/frc/robot/auto/groups/FiveBallAuto.java

@@ -0,0 +1,52 @@
+// 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.drive.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.drive.Drivetrain;
+
+public class MotionProfileTurn extends CommandBase {
+  Drivetrain drive;
+  double offset;
+  double target;
+  ProfiledPIDController controller;
+
+  public MotionProfileTurn(Drivetrain drive, double offset) {
+    this.drive = drive;
+    this.offset = offset;
+    addRequirements(drive);
+    controller = new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(10, 10));
+    controller.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  @Override
+  public void initialize() {
+    target = offset + drive.getPose().getRotation().getRadians();
+  }
+
+  @Override
+  public void execute() {
+    double theta = drive.getPose().getRotation().getRadians();
+    double feedback = controller.calculate(theta, 0);
+    double feedforward = controller.getSetpoint().velocity;
+    drive.drive(new ChassisSpeeds(0, 0, feedforward), false);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drive.brake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/drive/commands/MotionProfileTurn.java

@@ -0,0 +1,12 @@
+package frc.robot.drive.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class Test {
+    public static void main(String[] args) {
+        ProfiledPIDController controller = new ProfiledPIDController(1,0,0,new TrapezoidProfile.Constraints(1, 1));
+        System.out.println(controller.calculate(1, 2));
+
+    }
+}

src/main/java/frc/robot/drive/commands/Test.java

@@ -85,23 +85,23 @@ def main():
     #                 [0,0.00000001,-1.85]],
     #                 "FiveBallPartOne")
 
-    # generator.generate([
-    #                 [0,0,-1.85],
-    #                 [-0.2,0.9,-1.85],
-    #                 [1.85,-0.5,-math.pi+0.6]
-    #                 ],
-    #                 "FiveBallPartTwo")
-
     generator.generate([
-                    [1.85,-0.5,-math.pi+0.6],
-                    [6.15,0.3,-math.pi+0.7]
+                    [-0.7,0,-math.pi/2],
+                    [-0.2,0.9,-math.pi/2+0.15],
+                    [1.85,-0.5,-math.pi+0.6]
                     ],
-                    "FiveBallPartThree")
+                    "FiveBallPartTwo")
 
-    generator.generate([
-                    [6.15,0.3,-math.pi+0.7],
-                    [0.10,-0.375,-math.pi+1.25]
-                    ],
-                    "FiveBallPartFour")
+    # generator.generate([
+    #                 [1.85,-0.5,-math.pi+0.6],
+    #                 [6.15,0.3,-math.pi+0.7]
+    #                 ],
+    #                 "FiveBallPartThree")
+
+    # generator.generate([
+    #                 [6.15,0.3,-math.pi+0.7],
+    #                 [0.10,-0.375,-math.pi+1.25]
+    #                 ],
+    #                 "FiveBallPartFour")
 
 main()