diff --git a/src/main/java/com/team1816/lib/subsystems/drivetrain/IDrivetrain.java b/src/main/java/com/team1816/lib/subsystems/drivetrain/IDrivetrain.java index 70aac426..09fdad27 100644 --- a/src/main/java/com/team1816/lib/subsystems/drivetrain/IDrivetrain.java +++ b/src/main/java/com/team1816/lib/subsystems/drivetrain/IDrivetrain.java @@ -22,10 +22,7 @@ public interface IDrivetrain extends ITestableSubsystem { double maxSpd = (config.kinematics.maxDriveRPS / config.kinematics.driveGearing) * 2 * Math.PI * config.kinematics.wheelRadius; double cof = config.kinematics.wheelCOF; double gearing = config.kinematics.driveGearing; - double wheelCircumference = 2 * Math.PI * whlRad; - // MOI = mass * (width^2 + length^2) / 12, with 1.5x multiplier to account for - // concentrated perimeter mass (swerve modules, bumpers, battery) double MOI = 1.5 * massKG * (config.kinematics.wheelbaseWidth * config.kinematics.wheelbaseWidth + config.kinematics.wheelbaseLength * config.kinematics.wheelbaseLength) / 12; boolean isAllowedToPathPlannerPath = false; diff --git a/src/main/java/com/team1816/lib/subsystems/drivetrain/Swerve.java b/src/main/java/com/team1816/lib/subsystems/drivetrain/Swerve.java index 28d366c3..411962c7 100644 --- a/src/main/java/com/team1816/lib/subsystems/drivetrain/Swerve.java +++ b/src/main/java/com/team1816/lib/subsystems/drivetrain/Swerve.java @@ -78,28 +78,24 @@ public void periodic() { } private SwerveRequest GetSwerverCommand(SwerveRequest.FieldCentric drive) { + double deadBand = 0.05; // 1. Get raw joystick values (-1.0 to +1.0) double rawX = -controller.getLeftY(); // forward/back (negative because forward is usually negative Y) double rawY = -controller.getLeftX(); // strafe left/right double rawRot = -controller.getRightX(); // rotation - // 2. Deadband (remove drift) - rawX = Math.abs(rawX) < 0.08 ? 0 : rawX; - rawY = Math.abs(rawY) < 0.08 ? 0 : rawY; - rawRot = Math.abs(rawRot) < 0.08 ? 0 : rawRot; + // 2. Deadband (remove drift, yet still allow speeds just above 0) + double x = Math.abs(rawX) < deadBand ? 0 : (rawX-deadBand)/(1-deadBand); + double y = Math.abs(rawY) < deadBand ? 0 : (rawY-deadBand)/(1-deadBand); + double rot = Math.abs(rawRot) < deadBand ? 0 : (rawRot-deadBand)/(1-deadBand); - // 3. Cube the inputs → insane precision at low speed, full power at full stick - double x = rawX * rawX * rawX; - double y = rawY * rawY * rawY; - double rot = rawRot * rawRot * rawRot; + // 3. Slew rate limit → buttery smooth acceleration +// x = xLimiter.calculate(x); +// y = yLimiter.calculate(y); +// rot = rotLimiter.calculate(rot); - // 4. Slew rate limit → buttery smooth acceleration - x = xLimiter.calculate(x); - y = yLimiter.calculate(y); - rot = rotLimiter.calculate(rot); - - // 5. Multipliers to slow down movement based on driver preference. Slow mode and normal mode. + // 4. Multipliers to slow down movement based on driver preference. Slow mode and normal mode. if (controller.leftTrigger().getAsBoolean()) { x *= SLOW_MODE_TRANSLATIONAL_MULTIPLIER; y *= SLOW_MODE_TRANSLATIONAL_MULTIPLIER; diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 73c7c365..f878430b 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -131,23 +131,24 @@ subsystems: kinematics: driveGearing: 6.03 steerGearing: 26.0909 - wheelbaseLength: .505 # meters - wheelbaseWidth: .505 # meters - robotMass: 74.088 # kg + wheelbaseLength: 0.4305 # meters + wheelbaseWidth: 0.4305 # meters + robotMass: 56.7 # kg wheelRadius: .04938 # meters (1.944 in) - maxDriveRPS: 98 # measured motor max speed rps - maxAngularRate: 8.285 # rad/sec - wheelCOF: 1.7 + maxDriveRPS: 80 # measured motor max speed rps + maxAngularRate: 1.3 # rot/sec + wheelCOF: 1 constants: translationKp: 20 translationKd: 0 rotationKp: 10 rotationKi: 0 rotationKd: 0 + #Driver preference options normalModeTranslationalMultiplier: 1 - normalModeRotationalMultiplier: 0.4 - slowModeTranslationalMultiplier: 0.35 - slowModeRotationalMultiplier: 0.15 + normalModeRotationalMultiplier: 1 + slowModeTranslationalMultiplier: 0.5 + slowModeRotationalMultiplier: 1 ledManager: implemented: false devices: @@ -258,10 +259,10 @@ subsystems: motorType: NEO550_JST remoteSensor: RemoteCANcoder remoteSensorId: 29 - id: -1 + id: 27 motorRotation: Clockwise_Positive - reverseSoftLimit: 0.0405 - forwardSoftLimit: 0.109 + reverseSoftLimit: 0.052 + forwardSoftLimit: 0.1196 statorCurrentLimit: 35 pidConfig: slot0: @@ -295,12 +296,12 @@ subsystems: shooterOffsetYMeters: -0.112268 shooterOffsetZMeters: 0.508 inclineDuckingLimitRotations: 0.052 - distanceOneInclineAngleRotations: 0.0405 + distanceOneInclineAngleRotations: 0.052 distanceOneLaunchVelocityRPS: 35 - distanceTwoInclineAngleRotations: 0.0555 + distanceTwoInclineAngleRotations: 0.08 distanceTwoLaunchVelocityRPS: 47 - distanceThreeInclineAngleRotations: 0.07425 - distanceThreeLaunchVelocityRPS: 49 + distanceThreeInclineAngleRotations: 0.1196 + distanceThreeLaunchVelocityRPS: 100 distanceAutoThingLaunchVelocityRPS: 60 closeDistanceBetweenBeamBreaks: 0.419 farDistanceBetweenBeamBreaks: 13.570 @@ -311,7 +312,7 @@ subsystems: launchVelocityAdjustmentAmountRPS: 0.04 inclineAngleAdjustmentAmountDegrees: 0.02 turretAngleAdjustmentAmountDegrees: 0.04 - topLaunchMotorBackspinMultiplier: 0.8 # Less than one for backspin. + topLaunchMotorBackspinMultiplier: 1 # Less than one for backspin. vision: cameras: forwardLeft: # Pi IP: 10.18.16.11