Finished all commands

Only Pathplanner left

Author: Bachthoven

src/main/java/frc/robot/commands/RunSwerve.java

@@ -0,0 +1,41 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+import frc.robot.subsystems.SwerveBase;
+
+public class RunSwerve extends Command {
+    private final SwerveBase swerveBase;
+    private final CommandXboxController controller;
+    
+    public RunSwerve(SwerveBase swerve, CommandXboxController ctrl) {
+        this.swerveBase = swerve;
+        this.controller = ctrl;
+        addRequirements(swerveBase);
+    }
+
+    @Override
+    public void initialize() {
+        swerveBase.coast();
+    }
+
+    @Override
+    public void execute() {
+        swerveBase.setSwerve(controller.getLeftX(), controller.getLeftY(), controller.getRightX());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerveBase.brake();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/subsystems/SwerveBase.java

@@ -0,0 +1,72 @@
+// 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.subsystems;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SwerveBase extends SubsystemBase {
+    private final SwerveModule frontLeft;
+    private final SwerveModule frontRight;
+    private final SwerveModule backLeft;
+    private final SwerveModule backRight;
+    private final Pigeon2 gyro;
+
+    public SwerveBase() {
+        frontLeft = new SwerveModule(new TalonFX(8), new TalonFX(9), new CANcoder(10), 98.35);
+        frontRight = new SwerveModule(new TalonFX(11), new TalonFX(12), new CANcoder(13), 3.5);
+        backLeft = new SwerveModule(new TalonFX(14), new TalonFX(15), new CANcoder(16), 34.84);
+        backRight = new SwerveModule(new TalonFX(17), new TalonFX(18), new CANcoder(19), 74.19);
+        gyro = new Pigeon2(20);
+    }
+
+    public void setSwerve(double vx, double vy, double rx){
+        setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, rx, gyro.getRotation2d()));
+    }
+    public void setChassisSpeed(ChassisSpeeds chassisSpeeds) {
+        setSwerveStates(new SwerveDriveKinematics().toSwerveModuleStates(chassisSpeeds));
+    }
+
+    public void setSwerveStates(SwerveModuleState[] states){
+        backLeft.setSwerveState(states[0]); 
+        backRight.setSwerveState(states[1]);
+        frontLeft.setSwerveState(states[2]);
+        frontRight.setSwerveState(states[3]);
+    }
+
+    public void coast() {
+        frontLeft.coast();
+        frontRight.coast();
+        backLeft.coast();
+        backRight.coast();
+    }
+
+    public void brake() {
+        frontLeft.brake();
+        frontRight.brake();
+        backLeft.brake();
+        backRight.brake();
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Front Left Motor Angle", frontLeft.getAngle());
+        SmartDashboard.putNumber("Front Right Motor Angle", frontRight.getAngle());
+        SmartDashboard.putNumber("Back Left Motor Angle", backLeft.getAngle());
+        SmartDashboard.putNumber("Back Right Motor Angle", backRight.getAngle());
+
+        SmartDashboard.putNumber("Front Left Motor Speed", frontLeft.getSpeed());
+        SmartDashboard.putNumber("Front Right Motor Speed", frontRight.getSpeed());
+        SmartDashboard.putNumber("Back Left Motor Speed", backLeft.getSpeed());
+        SmartDashboard.putNumber("Back Right Motor Speed", backRight.getSpeed());
+    }
+}

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -0,0 +1,67 @@
+// 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.subsystems;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SwerveModule extends SubsystemBase {
+    private final TalonFX turnMotor;
+    private final TalonFX moveMotor;
+    private final PIDController pidController;
+    private final CANcoder encoder;
+    private double speed;
+
+    private final double angleOffset;
+
+    public SwerveModule(TalonFX turnMotor, TalonFX moveMotor, CANcoder encoder, double angleOffset) {
+        this.turnMotor = turnMotor;
+        this.moveMotor = moveMotor;
+        this.encoder = encoder;
+        this.angleOffset = angleOffset;
+        this.pidController = new PIDController(0.5, 0.5, 0.5);
+        this.pidController.setTolerance(1);
+        this.speed = 0;
+    }
+
+    public void setSwerveState(SwerveModuleState swerveState) {
+        speed = swerveState.speedMetersPerSecond;
+        moveMotor.set(speed);
+    
+        double adjustedAngle = swerveState.angle.getDegrees() - angleOffset;
+        pidController.setSetpoint(adjustedAngle);
+        pidController.calculate(turnMotor.getPosition().getValueAsDouble(), adjustedAngle);
+    }
+
+    public double getAngle() {
+        return encoder.getPosition().getValueAsDouble();
+    }
+
+    public double getSpeed() {
+        return speed;
+    }
+
+    public void coast() {
+        turnMotor.setNeutralMode(NeutralModeValue.Coast);
+        moveMotor.setNeutralMode(NeutralModeValue.Coast);
+    }
+
+    public void brake() {
+        turnMotor.setNeutralMode(NeutralModeValue.Brake);
+        moveMotor.setNeutralMode(NeutralModeValue.Brake);
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Turn Motor Temp", turnMotor.getDeviceTemp().getValueAsDouble()); 
+        SmartDashboard.putNumber("Move Motor Temp", moveMotor.getDeviceTemp().getValueAsDouble());
+    }
+}