diff --git a/src/main/deploy/death.chrp b/src/main/deploy/death.chrp new file mode 100644 index 0000000..dc8aab0 Binary files /dev/null and b/src/main/deploy/death.chrp differ diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ 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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index ed9e739..19221dc 100644 --- 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 new file mode 100644 index 0000000..58a2684 Binary files /dev/null and b/src/main/deploy/song1.chrp differ diff --git a/src/main/java/frc/robot/Commands/AnguladorController.java b/src/main/java/frc/robot/Commands/AnguladorController.java index ea5f7d8..72ccee5 100644 --- a/src/main/java/frc/robot/Commands/AnguladorController.java +++ b/src/main/java/frc/robot/Commands/AnguladorController.java @@ -17,13 +17,13 @@ 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 @@ -31,7 +31,7 @@ public void UpdateSetPoint(){ } 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 index 0d4d526..2510865 100644 --- 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 index 455c4ec..b785982 100644 --- 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 index 72de58d..823e9f5 100644 --- 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){ } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 994a4f3..d551e5d 100644 --- 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 index ff874aa..a3aeb95 100644 --- 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 turningLL = () -> this.lls.limelightStrafeProportional(); - Supplier RunningLL = () -> this.lls.LimelightRunProportional(); - Supplier zero = () -> 0.0; - Supplier back = () -> 0.1; - Supplier 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 index c432b2c..775ecda 100644 --- 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(); + } } diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java index 9f55c25..dbde27b 100644 --- a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java @@ -8,16 +8,13 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PS5Controller; +import edu.wpi.first.math.MathUtil; public class ElevatorSubsystem { public SparkMax cansparkMax = new SparkMax(56, MotorType.kBrushless); PIDController pidController = new PIDController(0.5, 0, 0); - Encoder caEncoder; private double elevatorSetPoint = 0; @@ -26,7 +23,7 @@ public class ElevatorSubsystem { - public void ElevatorSubsystem(){ + public ElevatorSubsystem(){ config .inverted(true) .idleMode(IdleMode.kBrake); @@ -34,17 +31,18 @@ public void ElevatorSubsystem(){ .positionConversionFactor(1) .velocityConversionFactor(1); config.closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(0.01, 0.0, 0); + .feedbackSensor(FeedbackSensor.kPrimaryEncoder); cansparkMax.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - public void SetPoint(){ - cansparkMax.set(pidController.calculate(cansparkMax.getEncoder().getPosition(), elevatorSetPoint ) * 0.05); + public void applySetPoint(){ + var pidOut = pidController.calculate(cansparkMax.getEncoder().getPosition(), elevatorSetPoint ) * 0.05; + var clampedOutput = MathUtil.clamp(pidOut, -3, 3); + cansparkMax.set(clampedOutput); } - public void UpdateSetPoint(double setpoint){ + public void updateSetPoint(double setpoint){ this.elevatorSetPoint = setpoint; } diff --git a/src/main/java/frc/robot/Subsystems/LimeLightSubsystem.java b/src/main/java/frc/robot/Subsystems/LimeLightSubsystem.java index c19f387..7b59a58 100644 --- a/src/main/java/frc/robot/Subsystems/LimeLightSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/LimeLightSubsystem.java @@ -9,8 +9,8 @@ import frc.robot.LimelightHelpers; public class LimeLightSubsystem { - public double maxSpeedX = 0.3; - public double maxSpeedY = 0.3; + double maxSpeedX = 0.3; + double maxSpeedY = 0.3; public double limelightStrafeProportional(){ // double kP = .015; diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index e627cd3..a5ef159 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -1,17 +1,24 @@ package frc.robot.Subsystems; import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; + public class SwerveSubsystem extends SubsystemBase { private final SwerveModule frontLeft = new SwerveModule( DriveConstants.kFrontLeftDriveMotorPort, @@ -69,6 +76,35 @@ public SwerveSubsystem() { } catch (Exception e) { } }).start(); + + + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + + + AutoBuilder.configure(this::getPose, + this::resetOdometry, + () -> DriveConstants.kDriveKinematics.toChassisSpeeds(frontLeft.getState(), + frontRight.getState(), + backLeft.getState(), + backRight.getState()), + (speeds, feedforward) -> driveRobotRelative(speeds), + new PPHolonomicDriveController( // PathPlanner`s lib for controlling drivetrains + new PIDConstants(5.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config , + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); + } catch (Exception e) { + e.printStackTrace(); + } } public void zeroHeading() { @@ -129,4 +165,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { backLeft.setDesiredState(desiredStates[2], backLeft.getRotation2d()); backRight.setDesiredState(desiredStates[3], backRight.getRotation2d()); } + + public void driveRobotRelative(ChassisSpeeds Mspeeds){ + SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(Mspeeds); + this.setModuleStates(moduleStates); + } } \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json new file mode 100644 index 0000000..d3f84e5 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.7.json", + "name": "PathplannerLib", + "version": "2025.2.7", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.7" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.7", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file