Testing

Author: JoshuaAN

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

@@ -172,6 +172,7 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+    mRobotContainer.enableLights();
   }
 
   /** This function is called periodically during autonomous. */

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

@@ -163,7 +163,8 @@ public Command getAutonomousCommand() {
       */
     }
     // return autoCommand;
-    return new NewAuto(mDrive, mShooter, mIntake);
+    return new NewAuto(mDrive, mShooter, mIntake, mLimelight, joysticks
+    );
   }
 
   public void displaySwitch() {

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.lib.HelixJoysticks;
 import frc.paths.NewAutoPartFour;
 import frc.paths.NewAutoPartOne;
 import frc.paths.NewAutoPartThree;
@@ -18,6 +19,7 @@
 import frc.robot.drive.Drivetrain;
 import frc.robot.drive.commands.ResetOdometry;
 import frc.robot.drive.commands.TrajectoryFollower;
+import frc.robot.drive.commands.TurnToAngle;
 import frc.robot.intake.Intake;
 import frc.robot.intake.commands.FastIntake;
 import frc.robot.intake.commands.RetractIntake;
@@ -29,19 +31,20 @@
 import frc.robot.shooter.commands.StopTrigger;
 import frc.robot.status.actions.ImageAction;
 import frc.robot.status.commands.ActionCommand;
+import frc.robot.vision.Limelight;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class NewAuto extends SequentialCommandGroup {
   /** Creates a new NewAuto. */
-  public NewAuto(Drivetrain drive, Shooter shooter, Intake intake) {
+  public NewAuto(Drivetrain drive, Shooter shooter, Intake intake, Limelight limelight, HelixJoysticks joysticks) {
     addCommands(
       new ResetOdometry(drive, new Pose2d(new Translation2d(0,0), new Rotation2d(-2.35))),
       new ResetEncoder(shooter),
       new ParallelDeadlineGroup(
         new WaitCommand(2.9), 
-        new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()),
+        // new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()),
         new TrajectoryFollower(drive, new NewAutoPartOne()),
         new FastIntake(intake),
         new SequentialCommandGroup(
@@ -56,17 +59,24 @@ public NewAuto(Drivetrain drive, Shooter shooter, Intake intake) {
       ),
       new WaitCommand(0.35),
       new ParallelDeadlineGroup(
-        new WaitCommand(3.9),
-        new TrajectoryFollower(drive, new NewAutoPartThree()),
+        new WaitCommand(4.5),
         new SequentialCommandGroup(
-          new WaitCommand(1.25),
+          new WaitCommand(1.75),
           new FlywheelController(shooter, 1795, 76.0)
         ),
         new SequentialCommandGroup(
-          new WaitCommand(2.9),
+          new TrajectoryFollower(drive, new NewAutoPartThree()),
+          new TurnToAngle(drive, limelight, joysticks)
+        ),
+        new SequentialCommandGroup(
+          new WaitCommand(3.4),
           new PullTrigger(shooter)
         ),
-        new RetractIntake(intake)),
+        new SequentialCommandGroup(
+          new WaitCommand(0.25),
+          new RetractIntake(intake)
+        )
+      ),
       new ParallelDeadlineGroup(
         new WaitCommand(3.0),
         new TrajectoryFollower(drive, new NewAutoPartFour()),

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

@@ -16,6 +16,9 @@ public class MotionProfileTurn extends CommandBase {
   Drivetrain drive;
   double offset;
   double target;
+  double velocity;
+  double dt;
+  double maxAcceleration, maxVelocity;
   ProfiledPIDController controller;
 
   public MotionProfileTurn(Drivetrain drive, double offset) {
@@ -28,22 +31,42 @@ public MotionProfileTurn(Drivetrain drive, double offset) {
   public void initialize() {
     // target = offset + drive.getPose().getRotation().getRadians();
     SmartDashboard.putString("Turning", "Enabled");
-    target = 0;
-    controller = new ProfiledPIDController(0.35, 0.0, 0.0, new TrapezoidProfile.Constraints(10, 10), 0.02);
-    controller.enableContinuousInput(-Math.PI, Math.PI);
+    controller = new ProfiledPIDController(0.35, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 1), 0.02);
+    // controller.enableContinuousInput(-Math.PI, Math.PI);
+    velocity = 0;
+    maxAcceleration = 0.5;
+    maxVelocity = 1;
+    dt = 0.02;
   }
 
   @Override
   public void execute() {
     double theta = drive.getPose().getRotation().getRadians();
-    double feedback = controller.calculate(theta, 1);
-    double feedforward = controller.getSetpoint().velocity;
-    SmartDashboard.putNumber("Theta Angle", theta);
-    SmartDashboard.putNumber("Turning Feedforward", feedforward);
+    double error = shortAngle(0, theta);
+    double feedback = controller.calculate(0, shortAngle(0, theta));
+    double feedforward = solveVelocity(maxVelocity, maxAcceleration, error, dt);
+    SmartDashboard.putNumber("Error", error);
+    SmartDashboard.putNumber("Velocity", feedforward);
     drive.drive(new ChassisSpeeds(0, 0, feedforward), false);
-    target++;
-    SmartDashboard.putNumber("Loops", target);
-    // drive.drive(new ChassisSpeeds(0, 0, 1), false);
+  }
+
+  public double solveVelocity(double maxVelocity, double maxAcceleration, double error, double dt) {
+    double accel = 0;
+    if (Math.sqrt(2*maxAcceleration*Math.abs(error)) > Math.abs(velocity)) {
+      accel = -maxAcceleration * Math.signum(error);
+    } else {
+      accel = maxAcceleration * Math.signum(error);
+    }
+    SmartDashboard.putNumber("Accel", accel);
+    velocity = Math.min(maxVelocity, Math.max(-maxVelocity, velocity + accel * dt));
+    // velocity += accel * dt;
+    return velocity;
+  }
+
+  public double shortAngle(double targetAngle, double currentAngle) {
+    double diff = (targetAngle - currentAngle + Math.PI) % (2* Math.PI) - Math.PI;
+    diff = diff < -Math.PI ? diff + 2 * Math.PI : diff;
+    return diff;
   }
 
   // Called once the command ends or is interrupted.