From 595ea231de0139c52973267755633d62e8fca16a Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Wed, 11 Oct 2023 19:47:06 -0400 Subject: [PATCH 1/5] update configuration --- build.gradle | 2 +- .../config/constants/ElevatorConstants.kt | 1 + .../swervemodule/SwerveModuleIOFalcon.kt | 71 +++--- vendordeps/Phoenix.json | 232 ++++-------------- 4 files changed, 85 insertions(+), 221 deletions(-) diff --git a/build.gradle b/build.gradle index 62efe4db..094fabf0 100644 --- a/build.gradle +++ b/build.gradle @@ -83,7 +83,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'org.jetbrains.kotlin:kotlin-test-junit5' - implementation 'com.github.team4099:FalconUtils:1.1.19' + implementation 'com.github.team4099:FalconUtils:1.1.24' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index a491074a..bad65aca 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -72,6 +72,7 @@ object ElevatorConstants { val ENABLE_ELEVATOR = 1.0 + val ELEVATOR_MAX_EXTENSION = 54.8.inches val ELEVATOR_MAX_RETRACTION = 0.0.inches val ELEVATOR_SOFT_LIMIT_EXTENSION = 54.5.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 0556c33e..89a98d50 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -1,10 +1,8 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.DemandType -import com.ctre.phoenix.motorcontrol.NeutralMode -import com.ctre.phoenix.motorcontrol.can.TalonFX -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 @@ -74,65 +72,56 @@ class SwerveModuleIOFalcon( } init { - driveFalcon.configFactoryDefault() - steeringFalcon.configFactoryDefault() + driveFalcon.configurator.apply(TalonFXConfiguration()) + steeringFalcon.configurator.apply(TalonFXConfiguration()) driveFalcon.clearStickyFaults() steeringFalcon.clearStickyFaults() - steeringConfiguration.slot0.kP = + steeringConfiguration.Slot0.kP = steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.slot0.kI = + steeringConfiguration.Slot0.kI = steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.slot0.kD = + steeringConfiguration.Slot0.kD = steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.slot0.kF = + steeringConfiguration.Slot0.kV + = steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.motionCruiseVelocity = + steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.motionAcceleration = + steeringConfiguration.MotionMagic.MotionMagicAcceleration = steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.peakOutputForward = 1.0 - steeringConfiguration.peakOutputReverse = -1.0 - steeringConfiguration.supplyCurrLimit.currentLimit = + steeringConfiguration.MotorOutput.PeakForwardDutyCycle = 1.0 + steeringConfiguration.MotorOutput.PeakForwardDutyCycle = -1.0 + steeringConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.supplyCurrLimit.enable = true + steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - steeringFalcon.setNeutralMode(NeutralMode.Brake) // change back to coast maybe? + steeringConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake // change back to coast maybe? steeringFalcon.inverted = true - steeringFalcon.configAllSettings(steeringConfiguration) - steeringFalcon.configAllowableClosedloopError( - 0, steeringSensor.positionToRawUnits(DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR) - ) + steeringFalcon.configurator.apply(steeringConfiguration) - driveConfiguration.slot0.kP = + driveConfiguration.Slot0.kP = driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.slot0.kI = + driveConfiguration.Slot0.kI = driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.slot0.kD = + driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.slot0.kF = 0.05425 + driveConfiguration.Slot0.kV = 0.05425 // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.supplyCurrLimit.currentLimit = + driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.supplyCurrLimit.triggerThresholdCurrent = + driveConfiguration.CurrentLimits.SupplyCurrentThreshold = DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.supplyCurrLimit.triggerThresholdTime = + driveConfiguration.CurrentLimits.SupplyTimeThreshold = DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.supplyCurrLimit.enable = true - driveConfiguration.statorCurrLimit.currentLimit = + driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + driveConfiguration.CurrentLimits.StatorCurrentLimit = DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.statorCurrLimit.triggerThresholdCurrent = - DrivetrainConstants.DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.statorCurrLimit.triggerThresholdTime = - DrivetrainConstants.DRIVE_STATOR_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.statorCurrLimit.enable = false // TODO tune - driveConfiguration.voltageCompSaturation = 12.0 - - driveFalcon.configAllSettings(driveConfiguration) - driveFalcon.enableVoltageCompensation(true) + driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - driveFalcon.setNeutralMode(NeutralMode.Brake) + driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + driveFalcon.configurator.apply(driveConfiguration) MotorChecker.add( "Drivetrain", diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index b98097e2..a5620fa9 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,61 +1,25 @@ { - "fileName": "PhoenixProAnd5.json", - "name": "CTRE-Phoenix (Pro And v5)", - "version": "23.0.8", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "23.2.2", "frcYear": 2023, - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2023-latest.json", "javaDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "23.0.8" + "version": "23.2.2" } ], "jniDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -66,9 +30,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -79,9 +43,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -92,9 +56,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -105,9 +69,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,9 +82,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -131,9 +95,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -144,9 +108,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -157,9 +121,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -170,9 +134,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -185,40 +149,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCI", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -230,9 +164,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -245,10 +179,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPISim", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -260,39 +194,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -305,9 +209,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,9 +224,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -335,9 +239,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -350,9 +254,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -365,9 +269,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -380,9 +284,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -395,9 +299,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -410,9 +314,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -423,36 +327,6 @@ "osxuniversal" ], "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "wpiapi-cpp", - "version": "23.0.8", - "libName": "CTRE_PhoenixPro_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "23.0.8", - "libName": "CTRE_PhoenixPro_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" } ] } \ No newline at end of file From fe21f75d468c09d8bf070fee4d2ce67aebf08a8d Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Wed, 11 Oct 2023 19:48:04 -0400 Subject: [PATCH 2/5] add file --- .../subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 89a98d50..ffa14567 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -127,6 +127,7 @@ class SwerveModuleIOFalcon( "Drivetrain", "Drive", MotorCollection( + mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, 90.celsius, From 60498b34cf1d7c8abc06637cf9d8f4f5676a23bc Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sat, 14 Oct 2023 01:27:21 -0400 Subject: [PATCH 3/5] finish phoenix6 conversion --- build.gradle | 2 +- .../kotlin/com/team4099/robot2023/Robot.kt | 4 - .../com/team4099/robot2023/RobotContainer.kt | 22 +--- .../robot2023/auto/AutonomousSelector.kt | 2 - .../robot2023/commands/AutoIntakeCommand.kt | 1 - .../commands/drivetrain/DrivePathCommand.kt | 8 +- .../commands/drivetrain/GoToAngle.kt | 4 +- .../commands/drivetrain/TeleopDriveCommand.kt | 2 - .../robot2023/config/constants/Constants.kt | 4 - .../config/constants/ElevatorConstants.kt | 4 +- .../config/constants/FieldConstants.kt | 27 ++--- .../config/constants/GroundIntakeConstants.kt | 11 +- .../config/constants/LedConstants.kt | 22 ---- .../config/constants/ManipulatorConstants.kt | 7 +- .../drivetrain/drive/DrivetrainIOReal.kt | 2 +- .../drivetrain/gyro/GyroIOPigeon2.kt | 39 +++---- .../drivetrain/swervemodule/SwerveModule.kt | 67 ++++++----- .../swervemodule/SwerveModuleIOFalcon.kt | 107 +++++++++++------- .../robot2023/subsystems/elevator/Elevator.kt | 2 +- .../subsystems/elevator/ElevatorIONeo.kt | 3 +- .../subsystems/elevator/ElevatorIOSim.kt | 3 +- .../robot2023/subsystems/falconspin/Motor.kt | 100 +++++++++------- .../groundintake/GroundIntakeIONeo.kt | 10 +- .../groundintake/GroundIntakeIOSim.kt | 5 +- .../robot2023/subsystems/led/LedIOCandle.kt | 47 -------- .../manipulator/ManipulatorIONeo.kt | 5 +- .../manipulator/ManipulatorIOSim.kt | 5 +- .../superstructure/Superstructure.kt | 16 +-- 28 files changed, 217 insertions(+), 314 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt diff --git a/build.gradle b/build.gradle index 094fabf0..69ad5ff0 100644 --- a/build.gradle +++ b/build.gradle @@ -83,7 +83,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'org.jetbrains.kotlin:kotlin-test-junit5' - implementation 'com.github.team4099:FalconUtils:1.1.24' + implementation 'com.github.team4099:FalconUtils:1.1.26' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 30c2d6c0..efc8ee5c 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -11,11 +11,9 @@ import com.team4099.robot2023.util.Alert.AlertType import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID -import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.livewindow.LiveWindow import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command @@ -135,8 +133,6 @@ object Robot : LoggedRobot() { CommandScheduler.getInstance().onCommandInterrupt { command: Command -> Logger.getInstance().recordOutput("/ActiveCommands/${command.name}", false) } - - } override fun disabledExit() { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 5690b3de..dc6841c2 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,32 +1,23 @@ package com.team4099.robot2023 -import com.ctre.phoenix.motorcontrol.can.TalonFX import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.AutoScoreCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand -import com.team4099.robot2023.commands.drivetrain.SwerveModuleTuningCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONeo import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.gameboy.GameBoy import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode import com.team4099.robot2023.subsystems.groundintake.GroundIntake -import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim import com.team4099.robot2023.subsystems.led.Led @@ -35,7 +26,6 @@ import com.team4099.robot2023.subsystems.led.LedIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.manipulator.Manipulator -import com.team4099.robot2023.subsystems.manipulator.ManipulatorIO import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim import com.team4099.robot2023.subsystems.superstructure.Request @@ -43,7 +33,6 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan -import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger import org.team4099.lib.smoothDeadband @@ -63,7 +52,7 @@ object RobotContainer { init { if (RobotBase.isReal()) { // Real Hardware Implementations - //drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} + // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision( @@ -122,7 +111,6 @@ object RobotContainer { drivetrain ) - /* module steeing tuning @@ -228,11 +216,13 @@ object RobotContainer { ) ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand()) - ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure) - .finallyDo { Logger.getInstance().recordOutput("Auto/isAutoDriving", false) } + ControlBoard.autoScore.whileTrue( + AutoScoreCommand(drivetrain, superstructure).finallyDo { + Logger.getInstance().recordOutput("Auto/isAutoDriving", false) + } ) - //ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) + // ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) // ControlBoard.dpadDown.whileTrue(PickupFromSubstationCommand(drivetrain, superstructure)) // ControlBoard.doubleSubstationIntake.whileTrue(AutoScoreCommand(drivetrain, diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index fae01386..2820247d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -90,8 +90,6 @@ object AutonomousSelector { autonomousModeChooser.addOption("Score Preload Cone", AutonomousMode.PRELOAD_SCORE_AUTO) - - autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0) autoEngageWidgit = diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt index aa9171d2..765b7f78 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -88,5 +88,4 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr superstructure.groundIntakeConeCommand() ) } - } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 3d22fae4..d1ab9fef 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -32,10 +32,10 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.cos import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond import org.team4099.lib.units.derived.inMetersPerSecondPerMeter -import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSecond +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond @@ -96,7 +96,7 @@ class DrivePathCommand( "Pathfollow/thetakD", DrivetrainConstants.PID.AUTO_THETA_PID_KD, Pair( - { it.inDegreesPerSecondPerDegreesPerSecond }, + { it.inDegreesPerSecondPerDegreePerSecond }, { it.degrees.perSecond.perDegreePerSecond } ) ) @@ -116,7 +116,7 @@ class DrivePathCommand( LoggedTunableValue( "Pathfollow/poskI", DrivetrainConstants.PID.AUTO_POS_KI, - Pair({ it.inMetersPerSecondPerMeterSecond }, { it.meters.perSecond.perMeterSeconds }) + Pair({ it.inMetersPerSecondPerMeterSeconds }, { it.meters.perSecond.perMeterSeconds }) ) val poskD = LoggedTunableValue( diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 16424f2f..85b89d45 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -15,8 +15,8 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond import org.team4099.lib.units.derived.perDegree import org.team4099.lib.units.derived.perDegreePerSecond import org.team4099.lib.units.derived.perDegreeSeconds @@ -44,7 +44,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { LoggedTunableValue( "Pathfollow/thetakD", Pair( - { it.inDegreesPerSecondPerDegreesPerSecond }, + { it.inDegreesPerSecondPerDegreePerSecond }, { it.degrees.perSecond.perDegreePerSecond } ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index ed1fc96c..5f0ef9ba 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -4,8 +4,6 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.CommandBase import org.littletonrobotics.junction.Logger -import java.util.function.DoubleSupplier -import java.util.function.Supplier class TeleopDriveCommand( val driver: DriverProfile, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index a1823da7..6820ea13 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -1,6 +1,5 @@ package com.team4099.robot2023.config.constants -import edu.wpi.first.wpilibj.AnalogInput import org.team4099.lib.units.base.seconds import org.team4099.lib.units.milli @@ -25,9 +24,6 @@ object Constants { val LOOP_PERIOD_TIME = 20.milli.seconds val POWER_DISTRIBUTION_HUB_ID = 1 - - - enum class GamePiece { CUBE, CONE, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index bad65aca..17a5c160 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -14,7 +14,6 @@ import org.team4099.lib.units.base.pounds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.cos import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.gearRatio import org.team4099.lib.units.derived.sin import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond @@ -23,7 +22,7 @@ import kotlin.math.PI object ElevatorConstants { const val SENSOR_CPR = 42 - val GEAR_RATIO = ((58.0 / 14.0) * (84.0 / 58.0) * (28.0 / 84.0)).gearRatio + val GEAR_RATIO = ((58.0 / 14.0) * (84.0 / 58.0) * (28.0 / 84.0)) val CARRIAGE_MASS = 10.pounds const val FOLLOW_MOTOR_INVERTED = true @@ -72,7 +71,6 @@ object ElevatorConstants { val ENABLE_ELEVATOR = 1.0 - val ELEVATOR_MAX_EXTENSION = 54.8.inches val ELEVATOR_MAX_RETRACTION = 0.0.inches val ELEVATOR_SOFT_LIMIT_EXTENSION = 54.5.inches diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 74ceb56c..6e9adc61 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -84,15 +84,7 @@ object FieldConstants { Rotation3d() ) ), - AprilTag( - 8, - Pose3d( - (0).inches, - (0).inches, - (0).inches, - Rotation3d() - ) - ) + AprilTag(8, Pose3d((0).inches, (0).inches, (0).inches, Rotation3d())) ) // val homeAprilTags: List = @@ -156,7 +148,7 @@ object FieldConstants { // Rotation3d(0.0.degrees, 0.0.degrees, 180.degrees))) // ) - //church tags + // church tags val aprilTags: List = listOf( AprilTag( @@ -168,52 +160,47 @@ object FieldConstants { Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), - AprilTag( 1, Pose3d( 0.inches, - 62.25.inches + (3*4.25).inches, + 62.25.inches + (3 * 4.25).inches, 25.25.inches + 5.5.inches, Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), - AprilTag( 2, Pose3d( 0.inches, - 94.35.inches + (5*4.25).inches, + 94.35.inches + (5 * 4.25).inches, 25.inches + 5.5.inches, Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), - AprilTag( 3, Pose3d( 0.inches, 160.inches, - 135.60.inches + (7*4.25).inches, + 135.60.inches + (7 * 4.25).inches, Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), - AprilTag( 4, Pose3d( 0.inches, - 168.1.inches + (9*4.25).inches, + 168.1.inches + (9 * 4.25).inches, 24.875.inches + 5.5.inches, Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) ), - AprilTag( 5, Pose3d( 0.inches, - 199.35.inches + (11*4.25).inches, + 199.35.inches + (11 * 4.25).inches, 25.125.inches + 5.5.inches, Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt index 66238fa4..97167815 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt @@ -8,9 +8,6 @@ import org.team4099.lib.units.base.pounds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.driven -import org.team4099.lib.units.derived.driving -import org.team4099.lib.units.derived.gearRatio import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts import org.team4099.lib.units.kilo @@ -44,17 +41,15 @@ object GroundIntakeConstants { val ABSOLUTE_ENCODER_OFFSET = (-196.87).degrees // From encoder to intake - val ROLLER_GEAR_RATIO = (36.0.driven / 18.0.driving).gearRatio + val ROLLER_GEAR_RATIO = (36.0 / 18.0) // units are kg * m^2' val ROLLER_MOMENT_INERTIA = 0.00313.kilo.grams * 1.0.meters.squared // gear reduction from absolute encoder to output - val ARM_ENCODER_GEAR_RATIO = (32.0.driven / 16.0.driving).gearRatio + val ARM_ENCODER_GEAR_RATIO = (32.0 / 16.0) // gear reduction from motor to output - val ARM_OUTPUT_GEAR_RATIO = - ((60.0.driven / 12.0.driving) * (80.0.driven / 18.0.driving) * (32.0.driven / 16.0.driving)) - .gearRatio + val ARM_OUTPUT_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) val ARM_LENGTH = 12.695.inches // TODO figure out via cad (this should be distance to com) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LedConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LedConstants.kt index cbc10b7a..271739c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LedConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LedConstants.kt @@ -1,7 +1,5 @@ package com.team4099.robot2023.config.constants -import com.ctre.phoenix.led.Animation -import com.ctre.phoenix.led.StrobeAnimation import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj.util.Color8Bit import org.team4099.lib.drivers.BlinkinLedDriver as Blinkin @@ -35,26 +33,6 @@ class LedConstants { MOVEMENT(Blinkin.BlinkinLedMode.FIXED_STROBE_WHITE) } - enum class CandleMode( - val animation: Animation?, - val r: Int, - val g: Int, - val b: Int, - val address: Pair? - ) { - IDLE(null, 255, 0, 0, null), - OUTTAKE(null, 0, 0, 255, null), - INTAKE(null, 64, 255, 51, null), - AUTO(null, 255, 215, 0, null), - TELEOP(null, 255, 255, 255, null), - CUBE(null, 162, 25, 255, null), - CONE(null, 0, 0, 0, null), - SINGLE_SUBSTATION(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(0, 25)), - DOUBLE_SUBSTATION(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(100, 25)), - SCORE(StrobeAnimation(255, 255, 255), 0, 0, 0, Pair(50, 25)), - MOVEMENT(StrobeAnimation(255, 255, 255), 0, 0, 0, null) - } - enum class SimMode(val color: Color8Bit) { IDLE(Color8Bit(Color.kOrange)) } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt index 1f99b96c..9c6810b6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt @@ -8,9 +8,6 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.pounds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.driven -import org.team4099.lib.units.derived.driving -import org.team4099.lib.units.derived.gearRatio import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.kilo @@ -66,8 +63,8 @@ object ManipulatorConstants { const val SENSOR_CPR = 42.0 - val ARM_GEAR_RATIO = ((84.0.driven / 29.0.driving) * (60.0.driven / 16.0.driving)).gearRatio - val ROLLER_GEAR_RATIO = 20.0.gearRatio + val ARM_GEAR_RATIO = ((84.0 / 29.0) * (60.0 / 16.0)) + val ROLLER_GEAR_RATIO = 20.0 // TODO: Change current thresholds val CONE_CURRENT_THRESHOLD = 25.amps diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt index 7a477e3a..e86fea20 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.subsystems.drivetrain.drive -import com.ctre.phoenix.motorcontrol.can.TalonFX +import com.ctre.phoenix6.hardware.TalonFX import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.Constants.Universal.CANIVORE_NAME import com.team4099.robot2023.config.constants.DrivetrainConstants diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt index cc400426..7d54b660 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt @@ -1,8 +1,7 @@ package com.team4099.robot2023.subsystems.drivetrain.gyro -import com.ctre.phoenix.ErrorCode -import com.ctre.phoenix.sensors.Pigeon2 -import com.ctre.phoenix.sensors.Pigeon2Configuration +import com.ctre.phoenix6.configs.Pigeon2Configuration +import com.ctre.phoenix6.hardware.Pigeon2 import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.GyroConstants @@ -18,9 +17,8 @@ import kotlin.math.IEEErem object GyroIOPigeon2 : GyroIO { private var pigeon2 = Pigeon2(Constants.Gyro.PIGEON_2_ID, Constants.Universal.CANIVORE_NAME) - private val xyzDps = DoubleArray(3) - private val isConnected = pigeon2.lastError.equals(ErrorCode.OK) + private val isConnected: Boolean = pigeon2.upTime.value > 0.0 var gyroYawOffset: Angle = 0.0.degrees var gyroPitchOffset: Angle = 0.0.degrees @@ -32,7 +30,7 @@ object GyroIOPigeon2 : GyroIO { val gyroYaw: Angle get() { if (isConnected) { - var rawYaw = pigeon2.yaw + gyroYawOffset.inDegrees + var rawYaw = pigeon2.yaw.value + gyroYawOffset.inDegrees rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyroYawRate.inDegreesPerSecond return rawYaw.IEEErem(360.0).degrees } else { @@ -43,7 +41,7 @@ object GyroIOPigeon2 : GyroIO { val gyroPitch: Angle get() { if (isConnected) { - val rawPitch = pigeon2.pitch + gyroPitchOffset.inDegrees + val rawPitch = pigeon2.pitch.value + gyroPitchOffset.inDegrees return rawPitch.IEEErem(360.0).degrees } else { return (-1.337).degrees @@ -53,7 +51,7 @@ object GyroIOPigeon2 : GyroIO { val gyroRoll: Angle get() { if (isConnected) { - val rawRoll = pigeon2.roll + gyroRollOffset.inDegrees + val rawRoll = pigeon2.roll.value + gyroRollOffset.inDegrees return rawRoll.IEEErem(360.0).degrees } else { return -1.337.degrees @@ -63,7 +61,7 @@ object GyroIOPigeon2 : GyroIO { val gyroYawRate: AngularVelocity get() { if (isConnected) { - return xyzDps[2].degrees.perSecond + return pigeon2.angularVelocityZ.value.degrees.perSecond } else { return -1.337.degrees.perSecond } @@ -72,7 +70,7 @@ object GyroIOPigeon2 : GyroIO { val gyroPitchRate: AngularVelocity get() { if (isConnected) { - return xyzDps[1].degrees.perSecond + return pigeon2.angularVelocityX.value.degrees.perSecond } else { return -1.337.degrees.perSecond } @@ -81,7 +79,7 @@ object GyroIOPigeon2 : GyroIO { val gyroRollRate: AngularVelocity get() { if (isConnected) { - return xyzDps[0].degrees.perSecond + return pigeon2.angularVelocityY.value.degrees.perSecond } else { return -1.337.degrees.perSecond } @@ -89,18 +87,17 @@ object GyroIOPigeon2 : GyroIO { init { val pigeon2Configuration = Pigeon2Configuration() - pigeon2Configuration.MountPosePitch = GyroConstants.mountPitch.inRadians - pigeon2Configuration.MountPoseYaw = GyroConstants.mountYaw.inRadians - pigeon2Configuration.MountPoseRoll = GyroConstants.mountRoll.inRadians + pigeon2Configuration.MountPose.MountPosePitch = GyroConstants.mountPitch.inRadians + pigeon2Configuration.MountPose.MountPoseYaw = GyroConstants.mountYaw.inRadians + pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians // TODO look into more pigeon configuration stuff - pigeon2.configAllSettings(pigeon2Configuration) + pigeon2.configurator.apply(pigeon2Configuration) } override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - pigeon2.getRawGyro(xyzDps) // calling this here so it updated xyzDps which is called upon later - inputs.rawGyroYaw = pigeon2.yaw.degrees + inputs.rawGyroYaw = pigeon2.yaw.value.degrees inputs.gyroConnected = isConnected @@ -112,18 +109,18 @@ object GyroIOPigeon2 : GyroIO { inputs.gyroPitchRate = gyroPitchRate inputs.gyroRollRate = gyroRollRate - Logger.getInstance().recordOutput("Gyro/rawYawDegrees", pigeon2.yaw) + Logger.getInstance().recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value) } override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = toAngle - pigeon2.yaw.IEEErem(360.0).degrees + gyroYawOffset = toAngle - pigeon2.yaw.value.IEEErem(360.0).degrees } override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = toAngle - pigeon2.pitch.IEEErem(360.0).degrees + gyroPitchOffset = toAngle - pigeon2.pitch.value.IEEErem(360.0).degrees } override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = toAngle - pigeon2.roll.IEEErem(360.0).degrees + gyroRollOffset = toAngle - pigeon2.roll.value.IEEErem(360.0).degrees } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index 71d718ee..bef85758 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -132,24 +132,24 @@ class SwerveModule(val io: SwerveModuleIO) { } Logger.getInstance().processInputs(io.label, inputs) - Logger.getInstance() - .recordOutput( - "${io.label}/driveSpeedSetpointMetersPerSecond", - if (!shouldInvert) speedSetPoint.inMetersPerSecond - else -speedSetPoint.inMetersPerSecond - ) - Logger.getInstance() - .recordOutput( - "${io.label}/driveAccelSetpointMetersPerSecondSq", - accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.getInstance() - .recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) - Logger.getInstance() - .recordOutput( - "${io.label}/steeringValueDegreesWithMod", - inputs.steeringPosition.inDegrees.IEEErem(360.0) - ) + Logger.getInstance() + .recordOutput( + "${io.label}/driveSpeedSetpointMetersPerSecond", + if (!shouldInvert) speedSetPoint.inMetersPerSecond + else -speedSetPoint.inMetersPerSecond + ) + Logger.getInstance() + .recordOutput( + "${io.label}/driveAccelSetpointMetersPerSecondSq", + accelerationSetPoint.inMetersPerSecondPerSecond + ) + Logger.getInstance() + .recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) + Logger.getInstance() + .recordOutput( + "${io.label}/steeringValueDegreesWithMod", + inputs.steeringPosition.inDegrees.IEEErem(360.0) + ) Logger.getInstance().recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond) Logger.getInstance().recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians) @@ -237,16 +237,23 @@ class SwerveModule(val io: SwerveModuleIO) { SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) io.setOpenLoop( optimizedState.angle.angle, - optimizedState - .speedMetersPerSecond - .meters - .perSecond * Math.cos(abs((optimizedState.angle.angle - inputs.steeringPosition).inRadians))// consider desaturating wheel speeds here if it doesn't work + optimizedState.speedMetersPerSecond.meters.perSecond * + Math.cos( + abs( + (optimizedState.angle.angle - inputs.steeringPosition) + .inRadians + ) + ) // consider desaturating wheel speeds here if it doesn't work // from drivetrain ) Logger.getInstance() .recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) } else { - io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond * Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians))) + io.setOpenLoop( + desiredState.angle.angle, + desiredState.speedMetersPerSecond.meters.perSecond * + Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)) + ) Logger.getInstance() .recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } @@ -268,7 +275,7 @@ class SwerveModule(val io: SwerveModuleIO) { ) { if (optimize) { val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) val optimizedAccelState = SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) @@ -276,21 +283,13 @@ class SwerveModule(val io: SwerveModuleIO) { speedSetPoint = optimizedVelState.speedMetersPerSecond.meters.perSecond accelerationSetPoint = optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - io.setClosedLoop( - steeringSetPoint, - speedSetPoint, - accelerationSetPoint - ) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) } else { steeringSetPoint = desiredVelState.angle.angle speedSetPoint = desiredVelState.speedMetersPerSecond.meters.perSecond accelerationSetPoint = desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - io.setClosedLoop( - steeringSetPoint, - speedSetPoint, - accelerationSetPoint - ) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index ffa14567..8d337781 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -1,6 +1,12 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule +import com.ctre.phoenix6.configs.MotionMagicConfigs +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionDutyCycle +import com.ctre.phoenix6.controls.VelocityDutyCycle import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants @@ -84,20 +90,18 @@ class SwerveModuleIOFalcon( steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) steeringConfiguration.Slot0.kD = steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV - = + steeringConfiguration.Slot0.kV = steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) steeringConfiguration.MotionMagic.MotionMagicAcceleration = steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.MotorOutput.PeakForwardDutyCycle = 1.0 - steeringConfiguration.MotorOutput.PeakForwardDutyCycle = -1.0 steeringConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - steeringConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake // change back to coast maybe? + steeringConfiguration.MotorOutput.NeutralMode = + NeutralModeValue.Brake // change back to coast maybe? steeringFalcon.inverted = true steeringFalcon.configurator.apply(steeringConfiguration) @@ -109,7 +113,7 @@ class SwerveModuleIOFalcon( driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) driveConfiguration.Slot0.kV = 0.05425 // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = + driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes driveConfiguration.CurrentLimits.SupplyCurrentThreshold = DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes @@ -127,7 +131,6 @@ class SwerveModuleIOFalcon( "Drivetrain", "Drive", MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, 90.celsius, @@ -150,13 +153,14 @@ class SwerveModuleIOFalcon( } override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts - inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts + inputs.driveAppliedVoltage = (driveFalcon.get() * RobotController.getBatteryVoltage()).volts + inputs.steeringAppliedVoltage = + (steeringFalcon.get() * RobotController.getBatteryVoltage()).volts - inputs.driveStatorCurrent = driveFalcon.statorCurrent.amps - inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.amps - inputs.steeringStatorCurrent = steeringFalcon.statorCurrent.amps - inputs.steeringSupplyCurrent = steeringFalcon.statorCurrent.amps + inputs.driveStatorCurrent = driveFalcon.statorCurrent.value.amps + inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.value.amps + inputs.steeringStatorCurrent = steeringFalcon.statorCurrent.value.amps + inputs.steeringSupplyCurrent = steeringFalcon.statorCurrent.value.amps inputs.drivePosition = driveSensor.position inputs.steeringPosition = steeringSensor.position @@ -164,8 +168,9 @@ class SwerveModuleIOFalcon( inputs.driveVelocity = driveSensor.velocity inputs.steeringVelocity = steeringSensor.velocity - inputs.driveTemp = driveFalcon.temperature.celsius - inputs.steeringTemp = steeringFalcon.temperature.celsius + // processor temp is also something we may want to log ? + inputs.driveTemp = driveFalcon.deviceTemp.value.celsius + inputs.steeringTemp = steeringFalcon.deviceTemp.value.celsius inputs.potentiometerOutputRaw = potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI @@ -177,13 +182,11 @@ class SwerveModuleIOFalcon( (inputs.potentiometerOutputRadians - zeroOffset).inRadians ) - Logger.getInstance() - .recordOutput("$label/sensorVelocityRawUnits", driveFalcon.selectedSensorVelocity) - Logger.getInstance().recordOutput("$label/motorOutput", driveFalcon.motorOutputPercent) + Logger.getInstance().recordOutput("$label/motorOutput", driveFalcon.get()) } override fun setSteeringSetpoint(angle: Angle) { - steeringFalcon.set(ControlMode.Position, steeringSensor.positionToRawUnits(angle)) + steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle))) } override fun setClosedLoop( @@ -192,12 +195,12 @@ class SwerveModuleIOFalcon( acceleration: LinearAcceleration ) { val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign - driveFalcon.set( - ControlMode.Velocity, - driveSensor.velocityToRawUnits(speed), - DemandType.ArbitraryFeedForward, - feedforward.inVolts / 12.0 + driveFalcon.setControl( + VelocityDutyCycle( + driveSensor.velocityToRawUnits(speed), false, feedforward.inVolts / 12.0, 0, false + ) ) + setSteeringSetpoint(steering) } @@ -208,7 +211,7 @@ class SwerveModuleIOFalcon( * @param speed: Desired speed */ override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - driveFalcon.set(ControlMode.PercentOutput, speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) + driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX)) setSteeringSetpoint(steering) } @@ -217,19 +220,20 @@ class SwerveModuleIOFalcon( } override fun zeroSteering() { - steeringFalcon.selectedSensorPosition = + steeringFalcon.setRotorPosition( steeringSensor.positionToRawUnits( if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) (potentiometerOutput.radians) - zeroOffset else (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) ) + ) Logger.getInstance() .recordOutput("$label/zeroPositionRadians", steeringSensor.position.inRadians) - println("Loading Zero for Module $label (${steeringFalcon.selectedSensorPosition})") + println("Loading Zero for Module $label (${steeringFalcon.position.value})") } override fun zeroDrive() { - driveFalcon.selectedSensorPosition = 0.0 + driveFalcon.setRotorPosition(0.0) } override fun configureDrivePID( @@ -237,9 +241,14 @@ class SwerveModuleIOFalcon( kI: IntegralGain, Volt>, kD: DerivativeGain, Volt> ) { - driveFalcon.config_kP(0, driveSensor.proportionalVelocityGainToRawUnits(kP)) - driveFalcon.config_kI(0, driveSensor.integralVelocityGainToRawUnits(kI)) - driveFalcon.config_kD(0, driveSensor.derivativeVelocityGainToRawUnits(kD)) + val PIDConfig = Slot0Configs() + + PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP) + PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI) + PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD) + PIDConfig.kV = 0.05425 + + driveFalcon.configurator.apply(PIDConfig) } override fun configureSteeringPID( @@ -247,32 +256,50 @@ class SwerveModuleIOFalcon( kI: IntegralGain, kD: DerivativeGain ) { - steeringFalcon.config_kP(0, steeringSensor.proportionalPositionGainToRawUnits(kP)) - steeringFalcon.config_kI(0, steeringSensor.integralPositionGainToRawUnits(kI)) - steeringFalcon.config_kD(0, steeringSensor.derivativePositionGainToRawUnits(kD)) + val PIDConfig = Slot0Configs() + + PIDConfig.kP = steeringSensor.proportionalPositionGainToRawUnits(kP) + PIDConfig.kI = steeringSensor.integralPositionGainToRawUnits(kI) + PIDConfig.kD = steeringSensor.derivativePositionGainToRawUnits(kD) + PIDConfig.kV = 0.05425 + + driveFalcon.configurator.apply(PIDConfig) } override fun configureSteeringMotionMagic( maxVel: AngularVelocity, maxAccel: AngularAcceleration ) { - steeringConfiguration.motionCruiseVelocity = steeringSensor.velocityToRawUnits(maxVel) - steeringConfiguration.motionAcceleration = steeringSensor.accelerationToRawUnits(maxAccel) + + val motionMagicConfig = MotionMagicConfigs() + + motionMagicConfig.MotionMagicCruiseVelocity = steeringSensor.velocityToRawUnits(maxVel) + motionMagicConfig.MotionMagicAcceleration = steeringSensor.accelerationToRawUnits(maxAccel) + + steeringFalcon.configurator.apply(motionMagicConfig) } override fun setDriveBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + if (brake) { - driveFalcon.setNeutralMode(NeutralMode.Brake) + motorOutputConfig.NeutralMode = NeutralModeValue.Brake } else { - driveFalcon.setNeutralMode(NeutralMode.Coast) + motorOutputConfig.NeutralMode = NeutralModeValue.Coast } + driveFalcon.configurator.apply(motorOutputConfig) } override fun setSteeringBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + if (brake) { - steeringFalcon.setNeutralMode(NeutralMode.Brake) + motorOutputConfig.NeutralMode = NeutralModeValue.Brake } else { - steeringFalcon.setNeutralMode(NeutralMode.Coast) + motorOutputConfig.NeutralMode = NeutralModeValue.Coast } + steeringFalcon.configurator.apply(motorOutputConfig) + // motor output configs might overwrite invert? + steeringFalcon.inverted = true } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index b08fc941..402277ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -78,7 +78,7 @@ class Elevator(val io: ElevatorIO) { val enableElevator = LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) - val minPosition = + val minPosition = LoggedTunableValue( "Elevator/minPosition", ElevatorConstants.ELEVATOR_IDLE_HEIGHT, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt index 57cd20a8..47285c3f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt @@ -21,7 +21,6 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivenOverDriving import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxLinearMechanismSensor @@ -35,7 +34,7 @@ object ElevatorIONeo : ElevatorIO { private val leaderSensor = sparkMaxLinearMechanismSensor( leaderSparkMax, - ElevatorConstants.GEAR_RATIO.asDrivenOverDriving, + ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_RADIUS * 2, ElevatorConstants.VOLTAGE_COMPENSATION ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index 48cb1b48..80ba0a51 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -22,7 +22,6 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivingOverDriven import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond @@ -32,7 +31,7 @@ object ElevatorIOSim : ElevatorIO { val elevatorSim: ElevatorSim = ElevatorSim( DCMotor.getNEO(2), - ElevatorConstants.GEAR_RATIO.asDrivingOverDriven, + ElevatorConstants.GEAR_RATIO, ElevatorConstants.CARRIAGE_MASS, ElevatorConstants.SPOOL_RADIUS, ElevatorConstants.ELEVATOR_MAX_RETRACTION, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt index 2d1c5cbe..b3f7a0bd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt @@ -1,12 +1,12 @@ package com.team4099.robot2023.subsystems.falconspin -import com.ctre.phoenix.ErrorCode -import com.ctre.phoenix.motorcontrol.StickyFaults -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration -import com.ctre.phoenix.motorcontrol.can.TalonFX +import com.ctre.phoenix6.StatusCode +import com.ctre.phoenix6.configs.CurrentLimitsConfigs +import com.ctre.phoenix6.hardware.TalonFX import com.revrobotics.CANSparkMax import com.revrobotics.REVLibError import edu.wpi.first.math.Num +import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.simulation.LinearSystemSim import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.team4099.lib.units.base.Current @@ -178,19 +178,19 @@ class Falcon500( ) : Motor() { override val appliedVoltage: ElectricalPotential - get() = falcon500.motorOutputVoltage.volts + get() = (falcon500.get() * RobotController.getBatteryVoltage()).volts override val busVoltage: ElectricalPotential - get() = falcon500.busVoltage.volts + get() = falcon500.supplyVoltage.value.volts override val temperature: Temperature - get() = falcon500.temperature.celsius + get() = falcon500.deviceTemp.value.celsius override val statorCurrent: Current - get() = falcon500.statorCurrent.amps + get() = falcon500.statorCurrent.value.amps override val supplyCurrent: Current - get() = falcon500.supplyCurrent.amps + get() = falcon500.supplyCurrent.value.amps override val id: Int get() = falcon500.deviceID @@ -199,48 +199,61 @@ class Falcon500( override val stickyFaults: List get() { - val faults = StickyFaults() - falcon500.getStickyFaults(faults) val retVal = mutableListOf() - if (faults.UnderVoltage) { + if (falcon500.stickyFault_Hardware.value) { + retVal.add("HardwareFault") + } + if (falcon500.stickyFault_ProcTemp.value) { + retVal.add("ProcessorTemperature") + } + if (falcon500.stickyFault_DeviceTemp.value) { + retVal.add("DeviceTemperature") + } + if (falcon500.stickyFault_Undervoltage.value) { retVal.add("UnderVoltage") } - if (faults.ForwardLimitSwitch) { + if (falcon500.stickyFault_BootDuringEnable.value) { + retVal.add("DeviceBootDuringEnable") + } + if (falcon500.stickyFault_UnlicensedFeatureInUse.value) { + retVal.add("Unlicensed") + } + if (falcon500.stickyFault_OverSupplyV.value) { + retVal.add("OverSupplyVoltage") + } + if (falcon500.stickyFault_UnstableSupplyV.value) { + retVal.add("UnstableSupplyVoltage") + } + if (falcon500.stickyFault_UnstableSupplyV.value) { + retVal.add("UnstableSupplyVoltage") + } + if (falcon500.stickyFault_ForwardHardLimit.value) { retVal.add("ForwardLimitSwitch") } - if (faults.ReverseLimitSwitch) { + if (falcon500.stickyFault_ReverseHardLimit.value) { retVal.add("ReverseLimitSwitch") } - if (faults.ForwardSoftLimit) { + if (falcon500.stickyFault_ForwardSoftLimit.value) { retVal.add("ForwardSoftLimit") } - if (faults.ReverseSoftLimit) { + if (falcon500.stickyFault_ReverseSoftLimit.value) { retVal.add("ReverseSoftLimit") } - if (faults.ResetDuringEn) { - retVal.add("ResetDuringEn") - } - if (faults.SensorOverflow) { - retVal.add("SensorOverflow") + if (falcon500.stickyFault_MissingRemoteSensor.value) { + retVal.add("MissingRemoteSensor") } - if (faults.SensorOutOfPhase) { - retVal.add("SensorOutOfPhase") + if (falcon500.stickyFault_FusedSensorOutOfSync.value) { + retVal.add("MissingRemoteSensor") } - if (faults.HardwareESDReset) { - retVal.add("HardwareESDReset") + if (falcon500.stickyFault_StatorCurrLimit.value) { + retVal.add("StatorCurrentLimit") } - if (faults.RemoteLossOfSignal) { - retVal.add("RemoteLossOfSignal") + if (falcon500.stickyFault_SupplyCurrLimit.value) { + retVal.add("SupplyCurrentLimit") } - if (faults.APIError) { - retVal.add("APIError") - } - if (faults.SupplyOverV) { - retVal.add("SupplyOverV") - } - if (faults.SupplyUnstable) { - retVal.add("SupplyUnstable") + if (falcon500.stickyFault_UsingFusedCANcoderWhileUnlicensed.value) { + retVal.add("UsingFusedCANcoderWhileUnlicensed") } return retVal @@ -252,14 +265,13 @@ class Falcon500( thresholdTime: Time? ): Boolean { if (thresholdLimit != null && thresholdTime != null) { - val supplyCurrentConfigSuccess = - falcon500.configSupplyCurrentLimit( - SupplyCurrentLimitConfiguration( - true, limit.inAmperes, thresholdLimit.inAmperes, thresholdTime.inSeconds - ) - ) - - if (supplyCurrentConfigSuccess == ErrorCode.OK) { + val supplyCurrentConfig = CurrentLimitsConfigs() + supplyCurrentConfig.SupplyCurrentLimit = limit.inAmperes + supplyCurrentConfig.SupplyCurrentThreshold = thresholdLimit.inAmperes + supplyCurrentConfig.SupplyTimeThreshold = thresholdTime.inSeconds + val supplyCurrentConfigSuccess = falcon500.configurator.apply(supplyCurrentConfig) + + if (supplyCurrentConfigSuccess == StatusCode.OK) { if (limit == firstStageCurrentLimit) { currentLimitStage = CURRENT_STAGE_LIMIT.FIRST } else if (limit == baseCurrentLimit) { @@ -267,7 +279,7 @@ class Falcon500( } } - return supplyCurrentConfigSuccess == ErrorCode.OK + return supplyCurrentConfigSuccess == StatusCode.OK } return false } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt index ca57fbcf..f80560ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt @@ -21,7 +21,6 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivenOverDriving import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inVolts @@ -42,14 +41,14 @@ object GroundIntakeIONeo : GroundIntakeIO { private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - GroundIntakeConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + GroundIntakeConstants.ROLLER_GEAR_RATIO, GroundIntakeConstants.VOLTAGE_COMPENSATION ) private val armSensor = sparkMaxAngularMechanismSensor( armSparkMax, - GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO.asDrivenOverDriving, + GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO, GroundIntakeConstants.VOLTAGE_COMPENSATION ) @@ -65,7 +64,7 @@ object GroundIntakeIONeo : GroundIntakeIO { var output = ( (-throughBoreEncoder.absolutePosition.rotations) * - GroundIntakeConstants.ARM_ENCODER_GEAR_RATIO.asDrivenOverDriving + GroundIntakeConstants.ARM_ENCODER_GEAR_RATIO ) if (output in (-55).degrees..0.0.degrees) { @@ -171,8 +170,7 @@ object GroundIntakeIONeo : GroundIntakeIO { Logger.getInstance() .recordOutput( "GroundIntake/armSensorVelocity", - armSparkMax.encoder.velocity * - GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO.asDrivenOverDriving + armSparkMax.encoder.velocity * GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIOSim.kt index 77b6a637..aecaf84c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIOSim.kt @@ -28,7 +28,6 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivingOverDriven import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inKilogramsMeterSquared @@ -45,14 +44,14 @@ object GroundIntakeIOSim : GroundIntakeIO { val rollerSim = FlywheelSim( DCMotor.getNEO(1), - GroundIntakeConstants.ROLLER_GEAR_RATIO.asDrivingOverDriven, + GroundIntakeConstants.ROLLER_GEAR_RATIO, GroundIntakeConstants.ROLLER_MOMENT_INERTIA.inKilogramsMeterSquared ) val armSim = SingleJointedArmSim( DCMotor.getNEO(1), - GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO.asDrivingOverDriven, + GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO, GroundIntakeConstants.ARM_MOMENT_INERTIA.inKilogramsMeterSquared, GroundIntakeConstants.ARM_LENGTH.inMeters, -15.degrees.inRadians, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt deleted file mode 100644 index 61713a70..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ /dev/null @@ -1,47 +0,0 @@ -package com.team4099.robot2023.subsystems.led - -import com.ctre.phoenix.led.CANdle -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.LedConstants.CandleMode -import com.team4099.robot2023.config.constants.LedConstants.LEDMode - -object LedIOCandle : LedIO { - - private val ledController = CANdle(Constants.Led.LED_CANDLE_ID, Constants.Universal.CANIVORE_NAME) - private var lastState: LEDMode = LEDMode.IDLE - - override fun updateInputs(inputs: LedIO.LedIOInputs) { - inputs.ledState = lastState.name - } - - override fun setState(newState: LEDMode) { - lastState = newState - when (newState) { - LEDMode.IDLE -> setCANdleState(CandleMode.IDLE) - LEDMode.OUTTAKE -> setCANdleState(CandleMode.OUTTAKE) - LEDMode.INTAKE -> setCANdleState(CandleMode.INTAKE) - LEDMode.AUTO -> setCANdleState(CandleMode.AUTO) - LEDMode.TELEOP -> setCANdleState(CandleMode.TELEOP) - LEDMode.CUBE -> setCANdleState(CandleMode.CUBE) - LEDMode.CONE -> setCANdleState(CandleMode.CONE) - LEDMode.SINGLE_SUBSTATION -> setCANdleState(CandleMode.SINGLE_SUBSTATION) - LEDMode.DOUBLE_SUBSTATION -> setCANdleState(CandleMode.DOUBLE_SUBSTATION) - LEDMode.SCORE -> setCANdleState(CandleMode.SCORE) - LEDMode.MOVEMENT -> setCANdleState(CandleMode.MOVEMENT) - } - } - - private fun setCANdleState(state: CandleMode) { - if (state.animation == null) { - if (state.address != null) { - ledController.setLEDs( - state.r, state.g, state.b, 255, state.address.first, state.address.second - ) - } else { - ledController.setLEDs(state.r, state.g, state.b) - } - } else { - ledController.animate(state.animation) - } - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt index 9ea6957f..fd31cd8c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt @@ -19,7 +19,6 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivenOverDriving import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxAngularMechanismSensor @@ -32,7 +31,7 @@ object ManipulatorIONeo : ManipulatorIO { private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - ManipulatorConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ManipulatorConstants.ROLLER_GEAR_RATIO, ManipulatorConstants.ROLLER_VOLTAGE_COMPENSATION ) private val armSparkMax = @@ -40,7 +39,7 @@ object ManipulatorIONeo : ManipulatorIO { private val armSensor = sparkMaxLinearMechanismSensor( armSparkMax, - ManipulatorConstants.ARM_GEAR_RATIO.asDrivenOverDriving, + ManipulatorConstants.ARM_GEAR_RATIO, ManipulatorConstants.ARM_SPOOL_RADIUS * 2, ManipulatorConstants.ARM_VOLTAGE_COMPENSATION ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIOSim.kt index 36feb471..b5e864b0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIOSim.kt @@ -28,7 +28,6 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Volt -import org.team4099.lib.units.derived.asDrivenOverDriving import org.team4099.lib.units.derived.inKilogramsMeterSquared import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.radians @@ -40,14 +39,14 @@ object ManipulatorIOSim : ManipulatorIO { val rollerSim: FlywheelSim = FlywheelSim( DCMotor.getNEO(1), - ManipulatorConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ManipulatorConstants.ROLLER_GEAR_RATIO, ManipulatorConstants.MOMENT_INERTIA.inKilogramsMeterSquared ) val armSim: ElevatorSim = ElevatorSim( DCMotor.getNEO(1), - ManipulatorConstants.ARM_GEAR_RATIO.asDrivenOverDriving, + ManipulatorConstants.ARM_GEAR_RATIO, ManipulatorConstants.ARM_MASS, ManipulatorConstants.ARM_SPOOL_RADIUS, ManipulatorConstants.ARM_MAX_RETRACTION, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index e99e93c0..37c12571 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -279,20 +279,11 @@ class Superstructure( ) if (manipulator.isAtTargetedPosition) { - if (manipulator.hasCube){ - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 4.inches - ) + if (manipulator.hasCube) { + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(4.inches) } else { - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 3.inches - ) + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(3.inches) } - - - } } else { manipulator.currentRequest = @@ -777,7 +768,6 @@ class Superstructure( GroundIntake.TunableGroundIntakeStates.stowedDownAngle.get(), rollerVoltage ) scoringConeWithoutLoweringGroundIntake = false - } } else { groundIntake.currentRequest = From dfb9730c0b13c6d54544fe3ffdeb1bac30befb6e Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sun, 15 Oct 2023 17:30:38 -0400 Subject: [PATCH 4/5] fix gyro angular velocity axis --- .../robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt index 7d54b660..b5323f6a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt @@ -70,7 +70,7 @@ object GyroIOPigeon2 : GyroIO { val gyroPitchRate: AngularVelocity get() { if (isConnected) { - return pigeon2.angularVelocityX.value.degrees.perSecond + return pigeon2.angularVelocityY.value.degrees.perSecond } else { return -1.337.degrees.perSecond } @@ -79,7 +79,7 @@ object GyroIOPigeon2 : GyroIO { val gyroRollRate: AngularVelocity get() { if (isConnected) { - return pigeon2.angularVelocityY.value.degrees.perSecond + return pigeon2.angularVelocityX.value.degrees.perSecond } else { return -1.337.degrees.perSecond } From 9cbb6edf89192074fa901412cb4aa240ba69aec6 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sun, 12 Nov 2023 13:38:46 -0500 Subject: [PATCH 5/5] add FOC support --- .../robot2023/config/constants/DrivetrainConstants.kt | 1 + .../drivetrain/swervemodule/SwerveModuleIOFalcon.kt | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 841dc084..b944808b 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -28,6 +28,7 @@ import org.team4099.lib.units.perSecond import kotlin.math.sqrt object DrivetrainConstants { + const val FOC_ENABLED = false const val MINIMIZE_SKEW = false const val WHEEL_COUNT = 4 diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 8d337781..8a541c06 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -186,7 +186,7 @@ class SwerveModuleIOFalcon( } override fun setSteeringSetpoint(angle: Angle) { - steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle))) + steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle), DrivetrainConstants.FOC_ENABLED, 0.0, 0, false)) } override fun setClosedLoop( @@ -197,7 +197,7 @@ class SwerveModuleIOFalcon( val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign driveFalcon.setControl( VelocityDutyCycle( - driveSensor.velocityToRawUnits(speed), false, feedforward.inVolts / 12.0, 0, false + driveSensor.velocityToRawUnits(speed), DrivetrainConstants.FOC_ENABLED, feedforward.inVolts / 12.0, 0, false ) ) @@ -211,7 +211,7 @@ class SwerveModuleIOFalcon( * @param speed: Desired speed */ override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX)) + driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, DrivetrainConstants.FOC_ENABLED, false)) setSteeringSetpoint(steering) }