AlphaBots-Java · Galogico · Nov 19, 2025
diff --git a/src/main/deploy/death.chrp b/src/main/deploy/death.chrp
diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 7.0
+        "x": 2.8530737704918034,
+        "y": 7.06388319672131
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.536372950819672,
-        "y": 5.145850409836064
+        "x": 3.668237704918033,
+        "y": 5.42156762295082
       },
       "prevControl": {
-        "x": 3.404508196721311,
-        "y": 6.392571721311475
+        "x": 3.5483606557377048,
+        "y": 6.344620901639344
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -61.1134182330893
   },
   "reversed": false,
   "folder": null,

diff --git a/src/main/deploy/song1.chrp b/src/main/deploy/song1.chrp
diff --git a/src/main/java/frc/robot/Commands/AnguladorController.java b/src/main/java/frc/robot/Commands/AnguladorController.java
@@ -17,21 +17,21 @@ public void ControllAngulador(){
     }
 
     public void printAngler(){
-        SmartDashboard.putNumber("AnguladorAngle", angulador.canCoder.getPosition().getValueAsDouble());
+        SmartDashboard.putNumber("AnguladorAngle", angulador.getAnglerAbsEncoder());
     }
 
     public void UpdateSetPoint(){
         if(controller.getR1ButtonReleased()){
             //pegar peca
-            angulador.AngUpdateSetPoint(-0.52);
+         angulador.AngUpdateSetPoint(-0.52);
         }
         if(controller.getR2ButtonReleased()){
             //L3
             angulador.AngUpdateSetPoint(-0.05);
         }
         if(controller.getL2ButtonReleased()){
             //L2
-            angulador.AngUpdateSetPoint(-0.05);
+            angulador.AngUpdateSetPoint(-0.04);
         }
         if(controller.getL1ButtonReleased()){
             //L1
@@ -40,7 +40,7 @@ public void UpdateSetPoint(){
     }
 
     public void AnguladorL2(){
-        angulador.AngUpdateSetPoint(-0.05);
+        angulador.AngUpdateSetPoint(-0.04);
     }
     public void AnguladorL1(){
         angulador.AngUpdateSetPoint(0);

diff --git a/src/main/java/frc/robot/Commands/ElevatorController.java b/src/main/java/frc/robot/Commands/ElevatorController.java
@@ -7,7 +7,7 @@ public class ElevatorController {
      PS5Controller controller = new PS5Controller(1);
     ElevatorSubsystem elevator = new ElevatorSubsystem();
     public void ControllElevator(){
-        elevator.SetPoint();
+        elevator.applySetPoint();
 
 
     }
@@ -16,30 +16,30 @@ public void UpdateSetPoint(){
         // 1 setpoint = 7mm
         if(controller.getR1ButtonPressed()){
             //pegar peca
-            elevator.UpdateSetPoint(0);
+            elevator.updateSetPoint(0);
         }
         if(controller.getR2ButtonPressed()){
             //L3
-            elevator.UpdateSetPoint(70.8);
+            elevator.updateSetPoint(70.8);
         }
         if(controller.getL2ButtonPressed()){
             //L2
-            elevator.UpdateSetPoint(13);
+            elevator.updateSetPoint(11);
         }
         if(controller.getL1ButtonPressed()){
             //L1
-            elevator.UpdateSetPoint(0);
+            elevator.updateSetPoint(0);
         }
     }
 
     public void ElevatorL2(){
-        elevator.UpdateSetPoint(13);
+        elevator.updateSetPoint(13);
     }
     public void ElevatorL1(){
-        elevator.UpdateSetPoint(0);
+        elevator.updateSetPoint(0);
     }
     public void ElevatorL3(){
-        elevator.UpdateSetPoint(70);
+        elevator.updateSetPoint(70);
     }
 
 }
diff --git a/src/main/java/frc/robot/Commands/LimeLightCommand.java b/src/main/java/frc/robot/Commands/LimeLightCommand.java
@@ -91,7 +91,7 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        if (elapsed.get() < 9){return false;}else{return true;}
+        if (elapsed.get() < 3.5){return false;}else{return true;}
 
 
     }

diff --git a/src/main/java/frc/robot/Commands/SubidorController.java b/src/main/java/frc/robot/Commands/SubidorController.java
@@ -58,4 +58,4 @@ else if(controller.getPOV() == 90){
     }
 
 
-}
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,7 +5,13 @@
 package frc.robot;
 
 
+import java.io.Console;
+
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.hardware.TalonFX;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -15,18 +21,32 @@ public class Robot extends TimedRobot {
   private final RobotContainer m_robotContainer;
 
 
+  final TalonFX joe = new TalonFX(21);
+
   public Robot() {
     m_robotContainer = new RobotContainer();
+
+
+    m_Orchestra.addInstrument(joe);
+    var status = m_Orchestra.loadMusic("./death.chrp");
+    if (!status.isOK()) {
+      System.out.println("Error Loading Music");
+   }
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     m_robotContainer.PrintThings();
+
+    SmartDashboard.putBoolean("isPlaying", m_Orchestra.isPlaying());
+
   }
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+    m_Orchestra.stop();
+  }
 
   @Override
   public void disabledPeriodic() {}
@@ -51,12 +71,22 @@ public void autonomousPeriodic() {
   @Override
   public void autonomousExit() {}
 
+  Orchestra m_Orchestra = new Orchestra();
+
   @Override
   public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-  }
+  new Thread(() -> {
+            try {
+                Thread.sleep(1000);
+                m_Orchestra.play();
+            } catch (Exception e) {
+            }
+        }).start();
+
+}
 
   @Override
   public void teleopPeriodic() {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,8 @@
 import java.util.List;
 import java.util.function.Supplier;
 
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -96,59 +98,7 @@ private void configureBindings() {
   }
 
   public Command getAutonomousCommand() {
-        TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
-                AutoConstants.kMaxSpeedMetersPerSecond,
-                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
-                        .setKinematics(DriveConstants.kDriveKinematics);
-
-        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-                new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-                List.of(
-                        new Translation2d(-0.75, 0),
-                        new Translation2d(-0.75, -0.9)),
-                new Pose2d(-1.25, -0.9, Rotation2d.fromDegrees(0)),
-                trajectoryConfig);
-
-        PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0);
-        PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0);
-        ProfiledPIDController thetaController = new ProfiledPIDController(
-            0, 0, 0, AutoConstants.kThetaControllerConstraints);
-        thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-        SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
-                trajectory,
-                swerve::getPose,
-                DriveConstants.kDriveKinematics,
-                xController,
-                yController,
-                thetaController,
-                swerve::setModuleStates,
-                swerve);
-
-        Supplier<Double> turningLL = () -> this.lls.limelightStrafeProportional();
-        Supplier<Double> RunningLL = () -> this.lls.LimelightRunProportional();
-        Supplier<Double> zero = () -> 0.0;
-        Supplier<Double> back = () -> 0.1;
-        Supplier<Boolean> falso = () -> false;
-
-        LimeLightCommand limelightOriented = new LimeLightCommand(swerve, turningLL, RunningLL, zero, falso);
-        SwerveCommand backDrop = new SwerveCommand(swerve, back, zero, zero, falso);
-
-        return new SequentialCommandGroup(
-                //pos dir
-                //neg esq
-                new InstantCommand(() -> swerve.resetOdometry(trajectory.getInitialPose())),
-                swerveControllerCommand,
-                new InstantCommand(() -> elevator.ElevatorL2()),
-                new InstantCommand(() -> angulador.AnguladorL2()),
-                limelightOriented,
-                new InstantCommand(() -> sugador.Soltar()),
-                new WaitCommand(1),
-                new InstantCommand(() -> sugador.Parar()),
-                new WaitCommand(0.5),
-                backDrop,
-
-                new InstantCommand(() -> swerve.stopModules()));
-        // return null;
+    return new PathPlannerAuto("Example Auto");
   }
+
 }
diff --git a/src/main/java/frc/robot/Subsystems/AnguladorSubsystem.java b/src/main/java/frc/robot/Subsystems/AnguladorSubsystem.java
@@ -1,11 +1,9 @@
 package frc.robot.Subsystems;
 
-import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import org.opencv.core.Mat;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
@@ -15,34 +13,34 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PS5Controller;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class AnguladorSubsystem{
 
 
-    public SparkMax cansparkMaxAng = new SparkMax(57, MotorType.kBrushless);
-    public CANcoder canCoder = new CANcoder(46, "rio");
+    SparkMax cansparkMaxAng = new SparkMax(57, MotorType.kBrushless);
+    CANcoder canCoder = new CANcoder(46, "rio");
     PIDController pidControllerAng = new PIDController(1.6, 0, 0.1);
-    Encoder caEncoder;
     ArmFeedforward feedforward = new ArmFeedforward(0.3, 0.3, 12.24, 0.02);
     // ArmFeedforward feedforward = new ArmFeedforward(0, 0.4, 0, 0);
     private double AnguladorSetPoint;
 
+    private double kTrapezoidalProfilePeriod = 0.02;
+    private double kTrapezoidalProfileMaxVelocity = 0.6;
+    private double kTrapezoidalProfileMaxAcceleration = 0.4;
+
+
     // private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
 
     private final TrapezoidProfile.Constraints m_constraints =
-        new TrapezoidProfile.Constraints(0.6, 0.4);
+        new TrapezoidProfile.Constraints(kTrapezoidalProfileMaxVelocity, kTrapezoidalProfileMaxAcceleration); //rotations/second and rotations/second²
     private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
     private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
 
     private double angleOffsetDeg = 24;
 
     private final Timer m_timer = new Timer();
-    private PS5Controller cont = new PS5Controller(0);
 
 
 
@@ -51,15 +49,14 @@ public class AnguladorSubsystem{
 
 
 
-    public void AnguladorSubsystem(){
+    public AnguladorSubsystem(){
         config 
             .idleMode(IdleMode.kBrake);
             config.encoder
             .positionConversionFactor(1)
             .velocityConversionFactor(1);
             config.closedLoop
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-            .pid(1.5, 0.5, 1);
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
         cansparkMaxAng.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
 
@@ -69,11 +66,16 @@ public void AngSetPoint(){
         m_goal = new TrapezoidProfile.State(AnguladorSetPoint, 0);
         var profile = new TrapezoidProfile(m_constraints);
 
-        m_setpoint = profile.calculate(0.02, m_setpoint, m_goal);
+        m_setpoint = profile.calculate(kTrapezoidalProfilePeriod, m_setpoint, m_goal);
 
         SmartDashboard.putNumber("anguladorSetPoint", m_setpoint.position );
-        cansparkMaxAng.set(-feedforward.calculate((m_setpoint.position + 30/360) *2 * Math.PI, -m_setpoint.velocity)/ 18 + pidControllerAng.calculate(canCoder.getPosition().getValueAsDouble(), m_setpoint.position));
-        SmartDashboard.putNumber("feedforward", feedforward.calculate(m_setpoint.position *2 * Math.PI, -m_setpoint.velocity)/ 18);
+
+        var setPointDeg = m_setpoint.position * 360;
+        var adjustedSetPointRadians = (setPointDeg + angleOffsetDeg / 180.0 ) * Math.PI;
+        var setPointVelocityRadians = m_setpoint.velocity * 2 * Math.PI;
+
+        cansparkMaxAng.set(-feedforward.calculate(adjustedSetPointRadians, -setPointVelocityRadians) + pidControllerAng.calculate(canCoder.getPosition().getValueAsDouble(), m_setpoint.position)); //both the encoder and trapezoidal are in rotations, so they match here
+        SmartDashboard.putNumber("feedforward", feedforward.calculate(m_setpoint.position *2 * Math.PI, -m_setpoint.velocity * 2 * Math.PI));
 
     }
 
@@ -84,6 +86,8 @@ public void AngUpdateSetPoint(double Angsetpoint){
         m_timer.restart();
     }
 
-
+    public double getAnglerAbsEncoder(){
+        return canCoder.getPosition().getValueAsDouble();
+    }
 
 }
Original file line number	Diff line number	Diff line change
Expand Up		@@ -58,4 +58,4 @@ else if(controller.getPOV() == 90){
		}


		}
		}