From f5f3b24452c0dfd3fd15345aa231105190d04c96 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Tue, 24 Oct 2023 09:17:36 -0400 Subject: [PATCH] bobchanges --- src/main/kotlin/com/team4099/robot2023/Robot.kt | 1 + .../kotlin/com/team4099/robot2023/RobotContainer.kt | 10 +++++++--- .../robot2023/auto/mode/ConeCubeHoldBumpAuto.kt | 2 +- .../robot2023/config/constants/ElevatorConstants.kt | 6 +++--- .../subsystems/drivetrain/drive/DrivetrainIOReal.kt | 3 --- .../drivetrain/swervemodule/SwerveModuleIOFalcon.kt | 11 +++++++---- .../robot2023/subsystems/elevator/Elevator.kt | 3 +++ .../robot2023/subsystems/elevator/ElevatorIO.kt | 2 +- .../robot2023/subsystems/elevator/ElevatorIONeo.kt | 4 +++- .../subsystems/groundintake/GroundIntakeIONeo.kt | 4 +++- .../subsystems/manipulator/ManipulatorIONeo.kt | 4 +++- .../subsystems/superstructure/Superstructure.kt | 13 ++++++++++++- .../team4099/robot2023/subsystems/vision/Vision.kt | 6 +++--- 13 files changed, 47 insertions(+), 22 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 30c2d6c0..35c833aa 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -148,6 +148,7 @@ object Robot : LoggedRobot() { FMSData.allianceColor = DriverStation.getAlliance() RobotContainer.setDriveBrakeMode() RobotContainer.setSteeringBrakeMode() + RobotContainer.requestSuperstructureIdle() RobotContainer.getAutonomousCommand().schedule() } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 315dd8f0..493cbfa1 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -78,9 +78,9 @@ object RobotContainer { ) superstructure = Superstructure( - Elevator(object: ElevatorIO {}), - GroundIntake(object: GroundIntakeIO {}), - Manipulator(object: ManipulatorIO {}), + Elevator(ElevatorIONeo), + GroundIntake(GroundIntakeIONeo), + Manipulator(ManipulatorIONeo), Led(object : LedIO {}), GameBoy(GameboyIOServer) ) @@ -140,6 +140,10 @@ object RobotContainer { superstructure.currentRequest = Request.SuperstructureRequest.Idle() } + fun requestSuperstructureHome() { + superstructure.currentRequest = Request.SuperstructureRequest.Home() + } + fun zeroArm() { superstructure.groundIntakeZeroArm() } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt index 8eb78339..c809a1bb 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt @@ -51,7 +51,7 @@ class ConeCubeHoldBumpAuto(val drivetrain: Drivetrain, val superstructure: Super // pick up cube Waypoint( Translation2d( - FieldConstants.StagingLocations.translations[0]!!.x, + FieldConstants.StagingLocations.translations[0]!!.x + 1.5.meters, FieldConstants.StagingLocations.translations[0]!!.y ) .translation2d, 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..24ed0028 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -67,8 +67,8 @@ object ElevatorConstants { // circumference / 2pi = radius val SPOOL_RADIUS = 0.005.meters * 32.0 / (2 * PI) - val MAX_VELOCITY = 140.inches.perSecond // 75 - val MAX_ACCELERATION = 415.inches.perSecond.perSecond // 225 + val MAX_VELOCITY = 100.inches.perSecond // 75 + val MAX_ACCELERATION = 300.inches.perSecond.perSecond // 225 val ENABLE_ELEVATOR = 1.0 @@ -86,7 +86,7 @@ object ElevatorConstants { // TODO(do tests to figure out what these values should be) val CUBE_DROP_POSITION_DELTA = 3.2.inches val CONE_DROP_POSITION_DELTA = 0.0.inches - val GROUND_INTAKE_CUBE_HEIGHT = 6.0.inches + val GROUND_INTAKE_CUBE_HEIGHT = 8.5.inches val DOUBLE_SUBSTATION_CUBE_OFFSET = 0.0.inches val DOUBLE_SUBSTATION_CONE_OFFSET = 11.0.inches val SINGLE_SUBSTATION_CUBE_OFFSET = 0.0.inches 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..694cebec 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 @@ -39,9 +39,6 @@ object DrivetrainIOReal : DrivetrainIO { ) ), SwerveModule( - // object: SwerveModuleIO { - // override val label: String = Constants.Drivetrain.BACK_RIGHT_MODULE_NAME - // } SwerveModuleIOFalcon( TalonFX(Constants.Drivetrain.BACK_RIGHT_STEERING_ID, CANIVORE_NAME), TalonFX(Constants.Drivetrain.BACK_RIGHT_DRIVE_ID, CANIVORE_NAME), 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 8e42f90b..0db0417a 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 @@ -135,6 +135,7 @@ class SwerveModuleIOFalcon( driveFalcon.setNeutralMode(NeutralMode.Brake) + /* MotorChecker.add( "Drivetrain", "Drive", @@ -158,11 +159,12 @@ class SwerveModuleIOFalcon( 110.celsius ) ) + */ } override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts - inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts + inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts //no + inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts //no inputs.driveStatorCurrent = driveFalcon.statorCurrent.amps inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.amps @@ -175,13 +177,14 @@ class SwerveModuleIOFalcon( inputs.driveVelocity = driveSensor.velocity inputs.steeringVelocity = steeringSensor.velocity - inputs.driveTemp = driveFalcon.temperature.celsius - inputs.steeringTemp = steeringFalcon.temperature.celsius + inputs.driveTemp = driveFalcon.temperature.celsius //no + inputs.steeringTemp = steeringFalcon.temperature.celsius //no inputs.potentiometerOutputRaw = potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI inputs.potentiometerOutputRadians = potentiometerOutput.radians + Logger.getInstance() .recordOutput( "$label/potentiometerRadiansWithOffset", 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..fe0aa183 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -413,6 +413,9 @@ class Elevator(val io: ElevatorIO) { Logger.getInstance() .recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) + Logger.getInstance() + .recordOutput("Elevator/lastHomingStatorCurrentTripTime", lastHomingStatorCurrentTripTime.inSeconds) + if (Constants.Tuning.DEBUGING_MODE) { Logger.getInstance() .recordOutput( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index f415d852..c80b0e26 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -41,7 +41,7 @@ interface ElevatorIO { var leaderRawPosition = 0.0 var followerRawPosition = 0.0 - var isSimulating = false + var isSimulating = true override fun toLog(table: LogTable) { table?.put("elevatorPositionInches", elevatorPosition.inInches) 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..c8e416c5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt @@ -77,6 +77,7 @@ object ElevatorIONeo : ElevatorIO { leaderSparkMax.burnFlash() followerSparkMax.burnFlash() + /* MotorChecker.add( "Elevator", "Extension", @@ -91,6 +92,7 @@ object ElevatorIONeo : ElevatorIO { 90.celsius ), ) + */ } override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { @@ -139,7 +141,7 @@ object ElevatorIONeo : ElevatorIO { */ override fun setOutputVoltage(voltage: ElectricalPotential) { // divide by 2 cause full power elevator is scary - leaderSparkMax.setVoltage(voltage.inVolts) + leaderSparkMax.set(voltage.inVolts / 12) } /** 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..f62b1768 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt @@ -108,6 +108,7 @@ object GroundIntakeIONeo : GroundIntakeIO { armSparkMax.burnFlash() + /* MotorChecker.add( "Ground Intake", "Roller", @@ -115,10 +116,11 @@ object GroundIntakeIONeo : GroundIntakeIO { mutableListOf(Neo(rollerSparkMax, "Roller Motor")), GroundIntakeConstants.ROLLER_CURRENT_LIMIT, 70.celsius, - GroundIntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps, + GroundIntakeConstants.ROLLER_CURRENT_LIMIT - 5.amps, 90.celsius ), ) + */ MotorChecker.add( "Ground Intake", 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..97dd51fd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt @@ -78,6 +78,7 @@ object ManipulatorIONeo : ManipulatorIO { armSparkMax.idleMode = CANSparkMax.IdleMode.kBrake armSparkMax.burnFlash() + /* MotorChecker.add( "Manipulator", "ExtensionMotor", @@ -85,10 +86,11 @@ object ManipulatorIONeo : ManipulatorIO { mutableListOf(Neo(armSparkMax, "Extension Motor")), ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT, 70.celsius, - ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT - 30.amps, + ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT - 10.amps, 90.celsius ) ) + */ MotorChecker.add( "Manipulator", 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..70d5ff80 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -233,6 +233,13 @@ class Superstructure( } } + if (elevator.currentRequest is Request.ElevatorRequest.Home){ + elevator.currentRequest = + Request.ElevatorRequest.TargetingPosition( + 3.inches + ) + } + // Outputs val rollerVoltage = when (theoreticalGamePiece) { @@ -282,7 +289,7 @@ class Superstructure( if (manipulator.hasCube){ elevator.currentRequest = Request.ElevatorRequest.TargetingPosition( - 4.inches + 6.inches ) } else { elevator.currentRequest = @@ -299,6 +306,10 @@ class Superstructure( Request.ManipulatorRequest.TargetingPosition( manipulator.armPositionTarget, rollerVoltage ) + elevator.currentRequest = + Request.ElevatorRequest.TargetingPosition( + elevator.elevatorPositionTarget + ) } // Transition diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index b27c15cf..20f7c22b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -111,9 +111,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { // Logger.getInstance().recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d) robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() - println( - "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}" - ) +// println( +// "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}" +// ) } 2.0 -> { val error0 = values[1]