package frc.robot;
import java.util.List;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.commands.SwerveJoystickCmd;
import frc.robot.subsystems.SwerveSubsystem;
//import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
public class RobotContainer {
//coloque seus subsystems aqui
private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
private final Joystick driverJoytick = new Joystick(OIConstants.kDriverControllerPort);
static XboxController driverJoystick = new XboxController(OIConstants.kDriverControllerPort);
public RobotContainer() {
swerveSubsystem.setDefaultCommand(new SwerveJoystickCmd(//essa linha de código seta o command do swerve para sempre atuar
swerveSubsystem,
() -> -driverJoytick.getRawAxis(OIConstants.kDriverYAxis),// driverJoystick.getLeftY(),
() -> driverJoytick.getRawAxis(OIConstants.kDriverXAxis),//driverJoystick.getLeftX(),
() -> driverJoytick.getRawAxis(OIConstants.kDriverRotAxis),//driverJoystick.getRightX(),
() -> !driverJoytick.getRawButton(OIConstants.kDriverFieldOrientedButtonIdx) /* driverJoystick.getAButton()*/));
configureButtonBindings();
}
private void configureButtonBindings() {
new JoystickButton(driverJoytick, 2).whenPressed(() -> swerveSubsystem.zeroHeading());//arrumar depois
}
public Command getAutonomousCommand() {
// 1. Create trajectory settings
TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics);
// 2. Generate trajectory
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),//ponto inicial
List.of(
new Translation2d(1, 0),//pontos intermediarios
new Translation2d(1, -1)),
new Pose2d(2, -1, Rotation2d.fromDegrees(180)),//ponto final
trajectoryConfig);
// 3. Define PID controllers for tracking trajectory
PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0);
PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0);
ProfiledPIDController thetaController = new ProfiledPIDController(
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// 4. Construct command to follow trajectory
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
trajectory,
swerveSubsystem::getPose,
DriveConstants.kDriveKinematics,
xController,
yController,
thetaController,
swerveSubsystem::setModuleStates,
swerveSubsystem);
// 5. Add some init and wrap-up, and return everything
return new SequentialCommandGroup(
new InstantCommand(() -> swerveSubsystem.resetOdometry(trajectory.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> swerveSubsystem.stopModules()));
}
}
above is robotcontainer
package frc.robot.subsystems;
//import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.DriveConstants;
//import com.ctre.phoenix6.configs.Pigeon2Configuration;
//yhimport com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.hardware.Pigeon2;
public class SwerveSubsystem extends SubsystemBase {
private final SwerveModule frontLeft = new SwerveModule(
DriveConstants.kFrontLeftDriveMotorPort,
DriveConstants.kFrontLeftTurningMotorPort,
DriveConstants.kFrontLeftDriveEncoderReversed,
DriveConstants.kFrontLeftTurningEncoderReversed,
DriveConstants.kFrontLeftDriveAbsoluteEncoderPort,
DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetRad,
DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed);
private final SwerveModule frontRight = new SwerveModule(
DriveConstants.kFrontRightDriveMotorPort,
DriveConstants.kFrontRightTurningMotorPort,
DriveConstants.kFrontRightDriveEncoderReversed,
DriveConstants.kFrontRightTurningEncoderReversed,
DriveConstants.kFrontRightDriveAbsoluteEncoderPort,
DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetRad,
DriveConstants.kFrontRightDriveAbsoluteEncoderReversed);
private final SwerveModule backLeft = new SwerveModule(
DriveConstants.kBackLeftDriveMotorPort,
DriveConstants.kBackLeftTurningMotorPort,
DriveConstants.kBackLeftDriveEncoderReversed,
DriveConstants.kBackLeftTurningEncoderReversed,
DriveConstants.kBackLeftDriveAbsoluteEncoderPort,
DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetRad,
DriveConstants.kBackLeftDriveAbsoluteEncoderReversed);
private final SwerveModule backRight = new SwerveModule(
DriveConstants.kBackRightDriveMotorPort,
DriveConstants.kBackRightTurningMotorPort,
DriveConstants.kBackRightDriveEncoderReversed,
DriveConstants.kBackRightTurningEncoderReversed,
DriveConstants.kBackRightDriveAbsoluteEncoderPort,
DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad,
DriveConstants.kBackRightDriveAbsoluteEncoderReversed);
private final Pigeon2 gyro = new Pigeon2(5, "rio");
private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(0),
new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(),backRight.getPosition()});//(DriveConstants.kDriveKinematics, // arrumar isso aqui (se eu coloquei no chat gpt, provavelmente isso deu erro)
// new Rotation2d(0));
public SwerveSubsystem() {
new Thread(() -> {
try {
Thread.sleep(1000);
zeroHeading();
} catch (Exception e) {
}
}).start();
}
public void zeroHeading() {
gyro.reset();
}
public double getHeading() {
return Math.IEEEremainder(gyro.getAngle(), 360);
}
public Rotation2d getRotation2d() {
return Rotation2d.fromDegrees(getHeading());
}
public Pose2d getPose() {
return odometer.getPoseMeters();
}
public void resetOdometry(Pose2d pose) {
odometer.resetPosition(getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(),
backRight.getPosition()}, pose)/*(pose, getRotation2d())*/; //também tem q arrumar isso
}
@Override
public void periodic() {
odometer.update(getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(),
backRight.getPosition()});//
SmartDashboard.putNumber("Robot Heading", getHeading());
SmartDashboard.putString("Robot Location", getPose().getTranslation().toString());
}
public void stopModules() {
frontLeft.stop();
frontRight.stop();
backLeft.stop();
backRight.stop();
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
frontLeft.setDesiredState(desiredStates[0]);
frontRight.setDesiredState(desiredStates[1]);
backLeft.setDesiredState(desiredStates[2]);
backRight.setDesiredState(desiredStates[3]);
}
}
above is the swervesubsystem