From d21da9fd30f6e94341da1853ef49a836ec2d4098 Mon Sep 17 00:00:00 2001 From: MorphaxTheDeveloper Date: Mon, 11 Mar 2024 16:24:14 +0300 Subject: [PATCH] odo --- src/main/java/frc/robot/Constants.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 14 +- .../frc/robot/subsystems/SwerveModule.java | 54 +++-- .../frc/robot/subsystems/SwerveSubsystem.java | 187 +++++++++--------- 4 files changed, 140 insertions(+), 127 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8144031..3fe07d5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -98,14 +98,14 @@ public static final class AutoConstants { DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; - public static final double kPXController = 1.5; - public static final double kPYController = 1.5; - public static final double kPThetaController = 3; + public static final double kPXController = 0.05; + public static final double kPYController = 0.05; + public static final double kPThetaController = 0.1; - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = // new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, - kMaxAngularAccelerationRadiansPerSecondSquared); + 1.5, + 2); } public static class ports { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8062d1b..5167d0d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,20 +218,20 @@ public Command getAutonomousCommand() { // .setKinematics(DriveConstants.kDriveKinematics); TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) + 3, + 1) .setKinematics(DriveConstants.kDriveKinematics); var trajectoryOne = TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, Rotation2d.fromDegrees(0)), - List.of(new Translation2d(1, 1), new Translation2d(2, -1)), - new Pose2d(3, 0, Rotation2d.fromDegrees(0)), + List.of(new Translation2d(-1, 0), new Translation2d(-3, 1)), + new Pose2d(-2, 0, Rotation2d.fromDegrees(0)), new TrajectoryConfig(Units.feetToMeters(0.5), Units.feetToMeters(0.5))); - PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0); - PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0); + PIDController xController = new PIDController(5, 0, 0); + PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0); ProfiledPIDController thetaController = new ProfiledPIDController( AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); @@ -244,7 +244,7 @@ public Command getAutonomousCommand() { xController, yController, thetaController, - swerveSubsystem::setModuleStates, + swerveSubsystem::OtosetModuleStates, swerveSubsystem); return swerveControllerCommand; diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 29119d5..b8d5bfc 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ModuleConstants; -import frc.robot.Constants; import frc.robot.Constants.DriveConstants; public class SwerveModule extends SubsystemBase { @@ -31,10 +30,13 @@ public class SwerveModule extends SubsystemBase { boolean absoluteEncoderReversed; double absoluteEncoderOffsetRad; + int driveMotorId; + public SwerveModule(int driveMotorId, int turningMotorId, boolean driveMotorReversed, boolean turningMotorReversed, int absoluteEncoderId, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { this.absoluteEncoderOffsetRad = absoluteEncoderOffset; + this.driveMotorId = driveMotorId; this.absoluteEncoderReversed = absoluteEncoderReversed; absoluteEncoder = new CANcoder(absoluteEncoderId); @@ -61,18 +63,30 @@ public SwerveModule(int driveMotorId, int turningMotorId, boolean driveMotorReve } public double getDrivePosition() { + SmartDashboard.putNumber("swervePos"+ driveMotorId, driveEncoder.getPosition()); return driveEncoder.getPosition(); } - public double getDrivePositionForSwerve() { - return driveEncoder.getPosition() * (Math.sqrt(Constants.ModuleConstants.kWheelDiameterMeters / 2) * Math.PI) / (6.75 * 2048.0); + public SwerveModulePosition getPosition(){ + + return new SwerveModulePosition( + driveEncoder.getPosition() , + new Rotation2d(turningEncoder.getPosition())); + } public double getTurningPosition() { - return turningEncoder.getPosition(); + SmartDashboard.putNumber("swervePosTur"+ driveMotorId, turningEncoder.getPosition()); + return turningEncoder.getPosition(); + } + + public double getTurningPosition2() { + return turningEncoder.getPosition() * 50; } public double getDriveVelocity() { + SmartDashboard.putNumber("swerveVelo"+ driveMotorId, driveEncoder.getVelocity()); + return driveEncoder.getVelocity(); } @@ -80,20 +94,6 @@ public double getTurningVelocity() { return turningEncoder.getVelocity(); } - public double getDegrees(){ - return turningEncoder.getPosition() % 180; - } - - public Rotation2d getRot2dAngle(){ - return Rotation2d.fromDegrees( - getDegrees() - ); - } - - public SwerveModulePosition getModulePosition(){ - return new SwerveModulePosition(getDrivePositionForSwerve(), getRot2dAngle()); - } - public double getAbsoluteEncoderRad() { StatusSignal absoluteencoderradian = absoluteEncoder.getAbsolutePosition(); double angle = absoluteencoderradian.getValueAsDouble(); @@ -122,7 +122,7 @@ public void setDesiredState(SwerveModuleState state) { return; } state = SwerveModuleState.optimize(state, getState().angle); - driveMotor.set(3 * (state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + driveMotor.set(2 * (state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); turningMotor.set((turningPidController.calculate(getTurningPosition(), state.angle.getRadians()))); SmartDashboard.putString("Swerve[" + absoluteEncoder.getDeviceID() + "] state", state.toString()); SmartDashboard.putNumber("status", @@ -130,8 +130,22 @@ public void setDesiredState(SwerveModuleState state) { SmartDashboard.putNumber("digeri", turningPidController.calculate(getTurningPosition(), state.angle.getRadians())); } + public void OtosetDesiredState(SwerveModuleState state) { + if (Math.abs(state.speedMetersPerSecond) < 0.001) { + stop(); + return; + } + state = SwerveModuleState.optimize(state, getState().angle); + driveMotor.set(3*(state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + turningMotor.set((turningPidController.calculate(getTurningPosition(), state.angle.getRadians()))); + SmartDashboard.putString("Swerve[" + absoluteEncoder.getDeviceID() + "] state", state.toString()); + SmartDashboard.putNumber("OHOOHOHOHOHOHOOHOHOOHOHOOHO", + (state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + SmartDashboard.putNumber("ASDSKMA SKDASDMASPKDASDAPKMP", (turningPidController.calculate(getTurningPosition(), state.angle.getRadians()))); + } + @Override public void periodic() { - SmartDashboard.putNumber("pozisssyonlar", getTurningPosition() * 50); + SmartDashboard.putString("POSSSSS"+ driveMotorId, getPosition().toString()); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 1530415..ebe40d4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,9 +1,9 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.mechanisms.swerve.SimSwerveDrivetrain.SimSwerveModule; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -61,47 +61,21 @@ public class SwerveSubsystem extends SubsystemBase { DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad, DriveConstants.kBackRightDriveAbsoluteEncoderReversed); - - SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[] { - frontLeft.getModulePosition(), - frontRight.getModulePosition(), - backLeft.getModulePosition(), - backRight.getModulePosition() - }; - public AutoBuilder autobuilder = new AutoBuilder(); - private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); - - private final SwerveDriveOdometry odometer = new - SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(0), - swerveModulePositions, - new Pose2d(0.0, 0.0, new Rotation2d(0.0))); - - private SwerveModule[] modules = {frontLeft, frontRight, backLeft, backRight}; - - public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[modules.length]; - for (int i = 0; i < modules.length; i++) { - states[i] = modules[i].getState(); - } - - return states; - } - - private SwerveDriveKinematics kinematics = DriveConstants.kDriveKinematics; - - - public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { - ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); - SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); - } - - - public ChassisSpeeds getSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); - } + private SwerveModule[] modules = {frontLeft, frontRight, backLeft, backRight}; + + private SwerveDriveKinematics kinematics = DriveConstants.kDriveKinematics; +public static AutoBuilder autoBuilder = new AutoBuilder(); + private final SwerveDriveOdometry odometer = new + SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(0), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }, new Pose2d(0, 0, new Rotation2d())); + public SwerveSubsystem() { @@ -113,43 +87,46 @@ public SwerveSubsystem() { } }).start(); - autobuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - new PIDConstants(0.05, 0.0, 0.0), // Translation PID constants - new PIDConstants(0.05, 0.0, 0.0), // Rotation PID constants - 0.5, // Max module speed, in m/s - 0.2, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements -); - + // autoBuilder.configureHolonomic( + // this::getPose, // Robot pose supplier + // this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) + // this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + // this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + // new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + // new PIDConstants(0.5, 0.0, 0.0), // Translation PID constants + // new PIDConstants(0.5, 0.0, 0.0), // Rotation PID constants + // 4.5, // Max module speed, in m/s + // 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + // new ReplanningConfig() // Default path replanning config. See the API for the options here + // ), + // () -> { + // // Boolean supplier that controls when the path will be mirrored for the red alliance + // // This will flip the path being followed to the red side of the field. + // // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + // return false; + // }, + // this // Reference to this subsystem to set requirements + // ); } + public void zeroHeading() { gyro.reset(); } + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } public double getHeading() { return Math.IEEEremainder(gyro.getAngle(), 360); } - + public Rotation2d getRotation2d() { return Rotation2d.fromDegrees(getHeading()); } @@ -159,10 +136,12 @@ public Pose2d getPose() { return odometer.getPoseMeters(); } + - public void resetOdometry(Pose2d pose) { - odometer.resetPosition(getRotation2d(),swerveModulePositions,pose); - + public void resetOdometry(Pose2d pose) { + odometer.resetPosition(getRotation2d(), new SwerveModulePosition[] { + frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()} + , getPose()); } @@ -181,34 +160,54 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { backRight.setDesiredState(desiredStates[3]); } - @Override - public void periodic() { - /* - * odometer.update(getRotation2d(), frontLeft.getState(), frontRight.getState(), - * backLeft.getState(), - * backRight.getState()); - * SmartDashboard.putNumber("Robot Heading", getHeading()); - * SmartDashboard.putString("Robot Location", - * getPose().getTranslation().toString()); - */ + public void OtosetModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + frontLeft.OtosetDesiredState(desiredStates[0]); + frontRight.OtosetDesiredState(desiredStates[1]); + backLeft.OtosetDesiredState(desiredStates[2]); + backRight.OtosetDesiredState(desiredStates[3]); + } - SmartDashboard.putNumber("gyro", getHeading()); + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[modules.length]; + for (int i = 0; i < modules.length; i++) { + states[i] = modules[i].getState(); + } + return states; + } - swerveModulePositions = new SwerveModulePosition[] { - frontLeft.getModulePosition(), - frontRight.getModulePosition(), - backLeft.getModulePosition(), - backRight.getModulePosition() - }; - + public SwerveModulePosition[] getPositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[modules.length]; + for (int i = 0; i < modules.length; i++) { + positions[i] = modules[i].getPosition() ; + } + return positions; + } + + public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); + + SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(targetStates); + } - odometer.update(getRotation2d(), swerveModulePositions); - SmartDashboard.putString("Lokasyon", getPose().getTranslation().toString()); -SmartDashboard.putString("statess", getModuleStates()[0].toString()); -SmartDashboard.putString("statess2", getModuleStates()[1].toString()); -SmartDashboard.putString("statess3", getModuleStates()[2].toString()); -SmartDashboard.putString("statess4", getModuleStates()[3].toString()); + @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 Anglue Value", getRotation2d().toString()); + SmartDashboard.putString("Robot Location", + getPose().getTranslation().toString()); + + SmartDashboard.putNumber("gyro", getHeading()); } }