Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Phoenix6 #49

Open
wants to merge 5 commits into
base: offseason-tuning
Choose a base branch
from
Open

Phoenix6 #49

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
595ea23
update configuration
MatthewChoulas Oct 11, 2023
fe21f75
add file
MatthewChoulas Oct 11, 2023
60498b3
finish phoenix6 conversion
MatthewChoulas Oct 14, 2023
dfb9730
fix gyro angular velocity axis
MatthewChoulas Oct 15, 2023
9cbb6ed
add FOC support
MatthewChoulas Nov 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.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"
Expand Down
4 changes: 0 additions & 4 deletions src/main/kotlin/com/team4099/robot2023/Robot.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -135,8 +133,6 @@ object Robot : LoggedRobot() {
CommandScheduler.getInstance().onCommandInterrupt { command: Command ->
Logger.getInstance().recordOutput("/ActiveCommands/${command.name}", false)
}


}

override fun disabledExit() {
Expand Down
22 changes: 6 additions & 16 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -35,15 +26,13 @@ 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
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
Expand All @@ -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(
Expand Down Expand Up @@ -122,7 +111,6 @@ object RobotContainer {
drivetrain
)


/*
module steeing tuning

Expand Down Expand Up @@ -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,
Expand Down
2 changes: 0 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
1 change: 0 additions & 1 deletion src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,5 +88,4 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr
superstructure.groundIntakeConeCommand()
)
}

}
8 changes: 4 additions & 4 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -96,7 +96,7 @@ class DrivePathCommand(
"Pathfollow/thetakD",
DrivetrainConstants.PID.AUTO_THETA_PID_KD,
Pair(
{ it.inDegreesPerSecondPerDegreesPerSecond },
{ it.inDegreesPerSecondPerDegreePerSecond },
{ it.degrees.perSecond.perDegreePerSecond }
)
)
Expand All @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -44,7 +44,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
LoggedTunableValue(
"Pathfollow/thetakD",
Pair(
{ it.inDegreesPerSecondPerDegreesPerSecond },
{ it.inDegreesPerSecondPerDegreePerSecond },
{ it.degrees.perSecond.perDegreePerSecond }
)
)
Expand Down
2 changes: 0 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
4 changes: 0 additions & 4 deletions src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -25,9 +24,6 @@ object Constants {
val LOOP_PERIOD_TIME = 20.milli.seconds
val POWER_DISTRIBUTION_HUB_ID = 1




enum class GamePiece {
CUBE,
CONE,
Expand Down
3 changes: 1 addition & 2 deletions src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
27 changes: 7 additions & 20 deletions src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<AprilTag> =
Expand Down Expand Up @@ -156,7 +148,7 @@ object FieldConstants {
// Rotation3d(0.0.degrees, 0.0.degrees, 180.degrees)))
// )

//church tags
// church tags
val aprilTags: List<AprilTag> =
listOf(
AprilTag(
Expand All @@ -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)
)
Expand Down
11 changes: 3 additions & 8 deletions src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down
22 changes: 0 additions & 22 deletions src/main/kotlin/com/team4099/robot2023/config/constants/LedConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -35,26 +33,6 @@ class LedConstants {
MOVEMENT(Blinkin.BlinkinLedMode.FIXED_STROBE_WHITE)
}

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I deleted this cause according to phoenix6 migration guide candle support hasn't been added yet

enum class CandleMode(
val animation: Animation?,
val r: Int,
val g: Int,
val b: Int,
val address: Pair<Int, Int>?
) {
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))
}
Expand Down
7 changes: 2 additions & 5 deletions src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down
Loading
Loading