Skip to content
This repository has been archived by the owner on Nov 10, 2024. It is now read-only.

Commit

Permalink
odo
Browse files Browse the repository at this point in the history
  • Loading branch information
@MorphaxTheDeveloper
MorphaxTheDeveloper committed Mar 11, 2024
1 parent 22dc6ee commit d21da9f
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 127 deletions.
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -244,7 +244,7 @@ public Command getAutonomousCommand() {
xController,
yController,
thetaController,
swerveSubsystem::setModuleStates,
swerveSubsystem::OtosetModuleStates,
swerveSubsystem);

return swerveControllerCommand;
Expand Down
54 changes: 34 additions & 20 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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);
Expand All @@ -61,39 +63,37 @@ 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();
}

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<Double> absoluteencoderradian = absoluteEncoder.getAbsolutePosition();
double angle = absoluteencoderradian.getValueAsDouble();
Expand Down Expand Up @@ -122,16 +122,30 @@ 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",
(state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond) * 3);
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());
}
}
Loading

0 comments on commit d21da9f

Please sign in to comment.