diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index b362c7b..9e41553 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -8,36 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.564640410958912, - "y": 2.6351241438356174 + "x": 7.789895007209807, + "y": -1.860191735760635 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9517551369863013, - "y": 5.42990154109589 + "x": 4.612885273979555, + "y": 6.076005993145134 }, "prevControl": { - "x": 7.7935855263157885, - "y": 6.622861842105262 - }, - "nextControl": { - "x": 2.2635141527995155, - "y": 4.905671091395238 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.753253424657535, - "y": 2.66517551369863 - }, - "prevControl": { - "x": 7.716611842105262, - "y": 7.190542763157895 + "x": 7.152226027404213, + "y": 5.685338184925955 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index bc087bb..e572aa2 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -3,10 +3,10 @@ "front": -11.5, "left": 11.5 }, - "absoluteEncoderOffset": 0, + "absoluteEncoderOffset": 164.5, "drive": { "type": "sparkflex_vortex", - "id": 63, + "id": 53, "canbus": "rio" }, "inverted": { @@ -15,7 +15,7 @@ }, "angle": { "type": "sparkmax_neo", - "id": 73, + "id": 43, "canbus": "rio" }, "encoder": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index a0e0797..3f07d3f 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -3,10 +3,10 @@ "front": -11.5, "left": -11.5 }, - "absoluteEncoderOffset": 0, + "absoluteEncoderOffset": 118, "drive": { "type": "sparkflex_vortex", - "id": 64, + "id": 54, "canbus": "rio" }, "inverted": { @@ -15,7 +15,7 @@ }, "angle": { "type": "sparkmax_neo", - "id": 74, + "id": 44, "canbus": "rio" }, "encoder": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 58b201f..0a789da 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -3,10 +3,10 @@ "front": 11.5, "left": 11.5 }, - "absoluteEncoderOffset": 0, + "absoluteEncoderOffset": 211.2, "drive": { "type": "sparkflex_vortex", - "id": 61, + "id": 51, "canbus": "rio" }, "inverted": { @@ -15,12 +15,12 @@ }, "angle": { "type": "sparkmax_neo", - "id": 71, + "id": 41, "canbus": "rio" }, "encoder": { "type": "thrifty", - "id": 1, + "id": 2, "canbus": null }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 339d706..35857ad 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -3,10 +3,10 @@ "front": 11.5, "left": -11.5 }, - "absoluteEncoderOffset": 0, + "absoluteEncoderOffset": 42.6, "drive": { "type": "sparkflex_vortex", - "id": 62, + "id": 52, "canbus": "rio" }, "inverted": { @@ -15,12 +15,12 @@ }, "angle": { "type": "sparkmax_neo", - "id": 72, + "id": 42, "canbus": "rio" }, "encoder": { "type": "thrifty", - "id": 2, + "id": 1, "canbus": null }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 3834a36..79aa48f 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -1,16 +1,16 @@ { "drive": { - "p": 0.0020645, + "p": 0.04, "i": 0, - "d": 0, - "f": 0, + "d": 0.000125, + "f": 0.01, "iz": 0 }, "angle": { - "p": 0.0020645, + "p": 0, "i": 0, "d": 0, "f": 0, "iz": 0 } -} \ No newline at end of file +} diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index a4cdb4b..a838fe6 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,7 +1,7 @@ { "imu": { "type": "pigeon2", - "id": 51, + "id": 31, "canbus": "rio" }, "invertedIMU": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a79e045..cafa423 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,7 +11,7 @@ public static class OperatorConstants { } public static class GeneralConstants { - public static final int LED_LENGTH = 30; - public static final int LED_PWM_PORT = 1; + public static final int LED_LENGTH = 73; + public static final int LED_PWM_PORT = 8; } } diff --git a/src/main/java/frc/robot/PIDDebugger.java b/src/main/java/frc/robot/PIDDebugger.java new file mode 100644 index 0000000..7aef2a7 --- /dev/null +++ b/src/main/java/frc/robot/PIDDebugger.java @@ -0,0 +1,22 @@ +package frc.robot; + + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class PIDDebugger { + + public PIDDebugger() { + + } + + public void setSparkPIDControllerToDashboard(String prefix, double kP, double kI, double kD, double kMinOutput, + double kIz, double kMaxOutput) { + SmartDashboard.putNumber(prefix + "P", kP); + SmartDashboard.putNumber(prefix + "I", kI); + SmartDashboard.putNumber(prefix + "D", kD); + SmartDashboard.putNumber(prefix + "MinOutput", kMinOutput); + SmartDashboard.putNumber(prefix + "MaxOutput", kMaxOutput); + SmartDashboard.putNumber(prefix + "Iz", kIz); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0180b23..8215b9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,8 +38,8 @@ public class RobotContainer { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverController.getLeftY() * -1, - () -> driverController.getLeftX() * -1) + () -> driverController.getLeftY(), + () -> driverController.getLeftX()) .withControllerRotationAxis(driverController::getRightX) .deadband(OperatorConstants.DEADBAND) .scaleTranslation(0.8) @@ -73,6 +73,11 @@ private void configureBindings() { driverController.button(5).whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); driverController.button(10).onTrue((Commands.runOnce(drivebase::zeroGyro))); driverController.button(4).whileTrue(drivebase.centerModulesCommand()); + driverController.button(2).whileTrue( + drivebase.driveToPose( + new Pose2d(new Translation2d(7, 4), Rotation2d.fromDegrees(0))) + ); + driverController.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); //driverController.button(3).whileTrue(drivebase.pa); Go barge command } diff --git a/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleConstants.java b/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleConstants.java new file mode 100644 index 0000000..9b6a24a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleConstants.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.AnkleSubsystem; + +public class AnkleConstants { + public final static int RedlineID = 1; + public final static int SparkID = 1; + public final static double maxAngle = 0/360; + public final static double minAngle = 0/360; + public static final double MaxOutput = 0; + public static final double MinOutput = 0; + public static double kP = 0.04; + public static double kI = 0; + public static double kD = 0; + public static double gearRatio = 4; + +} diff --git a/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleSubsystem.java b/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleSubsystem.java new file mode 100644 index 0000000..1531f78 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleSubsystem.java @@ -0,0 +1,111 @@ +package frc.robot.subsystems.AnkleSubsystem; + +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class AnkleSubsystem extends SubsystemBase { + private SparkMax m_motor; + private WPI_VictorSPX m_wheelMotor; + private SparkClosedLoopController closedLoopController; + private SparkMaxConfig motorConfig; + private RelativeEncoder encoder; + private double currentAngleSetpoint; + private double kP, kI, kD, kIz, kMaxOutput, kMinOutput; + + public AnkleSubsystem() { + m_motor = new SparkMax(AnkleConstants.SparkID, MotorType.kBrushless); + m_wheelMotor = new WPI_VictorSPX(AnkleConstants.RedlineID); + encoder = m_motor.getEncoder(); + closedLoopController = m_motor.getClosedLoopController(); + motorConfig = new SparkMaxConfig(); + m_motor.configure(configCreator(motorConfig), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + encoder.setPosition(0); + } + + private void setAngle(double angle) { + if (isAngleInRange(angle)) { + currentAngleSetpoint = angle; + closedLoopController.setReference(angle * AnkleConstants.gearRatio, ControlType.kMAXMotionPositionControl, + ClosedLoopSlot.kSlot0); + } else { + setDefault(); + } + } + + private void setAngleTest() { + double angle = SmartDashboard.getNumber("testAngleAnkle", 0); + if (isAngleInRange(angle)) { + closedLoopController.setReference(angle * AnkleConstants.gearRatio, ControlType.kMAXMotionPositionControl, + ClosedLoopSlot.kSlot0); + } else { + setDefault(); + } + } + + private void setDefault() { + closedLoopController.setReference(0, ControlType.kMAXMotionPositionControl, + ClosedLoopSlot.kSlot0); + currentAngleSetpoint = 0; + if (encoder.getPosition() == 0) { + stopAngleMotor(); + } + } + + private boolean isAngleInRange(double angle) { + return angle <= AnkleConstants.maxAngle && angle >= AnkleConstants.minAngle; + } + + private void stopAngleMotor() { + m_motor.set(0); + } + + private void setWheelMotor(double velocity) { + m_wheelMotor.set(velocity); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Current Angle (Ankle)", currentAngleSetpoint); + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + + private SparkMaxConfig configCreator(SparkMaxConfig motorConfig) { + String prefix = "ankle"; + double kP = SmartDashboard.getNumber(prefix + "P", AnkleConstants.kP); + double kI = SmartDashboard.getNumber(prefix + "I", AnkleConstants.kI); + double kD = SmartDashboard.getNumber(prefix + "D", AnkleConstants.kD); + double kMinOutput = SmartDashboard.getNumber(prefix + "MinOutput", AnkleConstants.MinOutput); + double kMaxOutput = SmartDashboard.getNumber(prefix + "MaxOutput", AnkleConstants.MaxOutput); + motorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(kP) + .i(kI) + .d(kD) + .outputRange(kMinOutput, kMaxOutput); + + motorConfig.closedLoop.maxMotion + .maxVelocity(1000) + .maxAcceleration(1000) + .allowedClosedLoopError(1); + motorConfig.smartCurrentLimit(50); + + return motorConfig; + } + +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/Arm.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/Arm.java new file mode 100644 index 0000000..3922bc4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem/Arm.java @@ -0,0 +1,94 @@ +package frc.robot.subsystems.ArmSubsystem; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + + private final SparkMax m_motor; + private final SparkMax m_followerMotor; + private final SparkMaxConfig followerMotorConfig, m_motorConfig; + // Ayarlanan açı + private double currentAngle; + + public Arm() { + m_motor = new SparkMax(ArmConstants.masterNeoID, MotorType.kBrushless); + m_followerMotor = new SparkMax(ArmConstants.followerNeoID, MotorType.kBrushless); + followerMotorConfig = new SparkMaxConfig(); + m_motorConfig = new SparkMaxConfig(); + followerMotorConfig.follow(m_motor); + m_motor.configure(configCreator(m_motorConfig), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + m_followerMotor.configure(followerMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + currentAngle = 0; + } + + // Arm'ı belirli bir açıya ayarlama fonksiyonu + private void setAngle(double angle) { + if (isAngleInRange(angle)) { + currentAngle = angle; + + } else { + stopMotor(); + } + } + + // Test için arm'ı sabit bir açıya ayarlama + private void setAngleTest() { + double testAngle = SmartDashboard.getNumber("testAngle", 0); + setAngle(testAngle); + } + + // Varsayılan pozisyona geri dönme (0 derece) + private void setDefault() { + setAngle(ArmConstants.defaultAngle); + } + + private boolean isAngleInRange(double angle) { + return angle >= ArmConstants.defaultAngle && angle <= ArmConstants.maxAngle; + } + + // Motoru durdurma + private void stopMotor() { + m_motor.setVoltage(0); + m_followerMotor.setVoltage(0); + } + + // Robotun her döngüsünde çalışacak fonksiyon + @Override + public void periodic() { + SmartDashboard.putNumber("Current Angle (Arm)", currentAngle); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + + private SparkMaxConfig configCreator(SparkMaxConfig motorConfig) { + String prefix = "arm"; + double kP = SmartDashboard.getNumber(prefix + "P", ArmConstants.kP); + double kI = SmartDashboard.getNumber(prefix + "I", ArmConstants.kI); + double kD = SmartDashboard.getNumber(prefix + "D", ArmConstants.kD); + double kMinOutput = SmartDashboard.getNumber(prefix + "MinOutput", ArmConstants.MinOutput); + double kMaxOutput = SmartDashboard.getNumber(prefix + "MaxOutput", ArmConstants.MaxOutput); + motorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(kP) + .i(kI) + .d(kD) + .outputRange(kMinOutput, kMaxOutput); + + motorConfig.closedLoop.maxMotion + .maxVelocity(1000) + .maxAcceleration(1000) + .allowedClosedLoopError(1); + return motorConfig; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmConstants.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmConstants.java new file mode 100644 index 0000000..b3ad59b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmConstants.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.ArmSubsystem; + +public class ArmConstants { + public final static int masterNeoID = 0; + public final static int followerNeoID = 0; + public final static double defaultAngle = 0; + public final static double maxAngle = 274.165 - 19.165; + public final static int gearRatio = 24; + public static final double MaxOutput = 0; + public static final double MinOutput = 0; + public static double kP = 0.05; + public static double kI = 0; + public static double kD = 0; +} + diff --git a/src/main/java/frc/robot/subsystems/Drive/Swerve.java b/src/main/java/frc/robot/subsystems/Drive/Swerve.java index 21cb8f3..b161683 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Drive/Swerve.java @@ -348,7 +348,7 @@ public Command driveWithSetpointGeneratorFieldRelative(Supplier f public Command sysIdDriveMotorCommand() { return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( + SwerveDriveTest.setDriveSysIdRoutine( new Config(), this, swerveDrive, 12, true), 3.0, 5.0, 3.0); diff --git a/src/main/java/frc/robot/subsystems/Drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/Drive/SwerveConstants.java index f00cfad..e30a01c 100644 --- a/src/main/java/frc/robot/subsystems/Drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/Drive/SwerveConstants.java @@ -2,6 +2,6 @@ public class SwerveConstants { - public final static double MAX_SPEED = 3; + public final static double MAX_SPEED = 5; public final static double WHEEL_LOCK_TIME = 10; // seconds } diff --git a/src/main/java/frc/robot/subsystems/RobotStatusManager.java b/src/main/java/frc/robot/subsystems/RobotStatusManager.java index 35fa653..e7764c6 100644 --- a/src/main/java/frc/robot/subsystems/RobotStatusManager.java +++ b/src/main/java/frc/robot/subsystems/RobotStatusManager.java @@ -40,7 +40,7 @@ public enum RobotStatus { private static final int LED_LENGTH = GeneralConstants.LED_LENGTH; public RobotStatusManager() { - this.currentStatus = RobotStatus.Alignment; + this.currentStatus = RobotStatus.Climbing; // LED Şeridi Baslat ledStrip = new AddressableLED(GeneralConstants.LED_PWM_PORT); @@ -144,3 +144,4 @@ public void periodic() { } } } +