From 39727c78708d6358c5cca0f4a331a874568be635 Mon Sep 17 00:00:00 2001 From: John Joseph Date: Tue, 5 Dec 2023 16:27:01 -0500 Subject: [PATCH] Parallel deadline in auto, no end condition for elevator moveToPos, update constants, --- simgui-ds.json | 3 +- simgui.json | 16 ++--- src/main/kotlin/frc/team449/RobotLoop.kt | 10 +-- .../control/TrapezoidalExponentialProfile.kt | 9 +-- .../frc/team449/control/auto/ChoreoRoutine.kt | 2 +- .../robot2023/auto/routines/OnePiecePick.kt | 3 +- .../robot2023/constants/MotorConstants.kt | 13 ++++ .../robot2023/constants/RobotConstants.kt | 14 +++- .../constants/subsystem/ElevatorConstants.kt | 16 ++--- .../constants/subsystem/IntakeConstants.kt | 4 +- .../subsystem/ManipulatorConstants.kt | 2 +- .../subsystems/ControllerBindings.kt | 23 ++++-- .../team449/robot2023/subsystems/Intake.kt | 11 ++- .../robot2023/subsystems/elevator/Elevator.kt | 71 ++++++++----------- .../subsystems/elevator/ElevatorSim.kt | 11 +-- 15 files changed, 118 insertions(+), 90 deletions(-) create mode 100644 src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt diff --git a/simgui-ds.json b/simgui-ds.json index 15cec71..4726e2d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -94,8 +94,7 @@ "guid": "Keyboard0" }, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index a898469..e7b202e 100644 --- a/simgui.json +++ b/simgui.json @@ -49,12 +49,8 @@ } }, "/SmartDashboard/Field": { - "bottom": 544, - "builtin": "2023 Charged Up", "height": 8.013679504394531, - "left": 46, - "right": 1088, - "top": 36, + "image": "C:\\Users\\jpoth\\Downloads\\field(1).png", "width": 16.541748046875, "window": { "visible": true @@ -79,6 +75,9 @@ "open": true, "robot": { "drive": { + "double[]##v_/Monologuing/robot/drive/1.1 Estimated Pose": { + "open": true + }, "double[]##v_/Monologuing/robot/drive/Current Chassis Speeds": { "open": true }, @@ -86,11 +85,10 @@ "open": true } }, - "elevator": { - "open": true - }, "intake": { - "open": true + "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": { + "open": true + } }, "open": true } diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt index 3f98790..4cf9e24 100644 --- a/src/main/kotlin/frc/team449/RobotLoop.kt +++ b/src/main/kotlin/frc/team449/RobotLoop.kt @@ -1,9 +1,7 @@ package frc.team449 -import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.* import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.TimedRobot -import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler @@ -45,7 +43,7 @@ class RobotLoop : TimedRobot(), Logged { SmartDashboard.putData("Field", robot.field) SmartDashboard.putData("Routine Chooser", routineChooser) SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance()) - SmartDashboard.putBoolean("Enable Logging?", true) + SmartDashboard.putBoolean("Enable Logging?", false) controllerBinder.bindButtons() @@ -63,8 +61,10 @@ class RobotLoop : TimedRobot(), Logged { robot.field.getObject("bumpers").pose = robot.drive.pose - if (SmartDashboard.getBoolean("Enable Logging?", true)) { + if (SmartDashboard.getBoolean("Enable Logging?", false)) { Monologue.update() + } else { + Monologue.updateFileLog() } } diff --git a/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt b/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt index 7b37b73..28ac4a0 100644 --- a/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt +++ b/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt @@ -1,6 +1,7 @@ package frc.team449.control import edu.wpi.first.math.util.Units +import frc.team449.robot2023.constants.MotorConstants import frc.team449.robot2023.constants.subsystem.ElevatorConstants import kotlin.math.* @@ -32,10 +33,10 @@ class TrapezoidalExponentialProfile( } // NEO Motor Constants - val freeSpeed = ElevatorConstants.FREE_SPEED - val freeCurrent = ElevatorConstants.FREE_CURRENT - val stallCurrent = ElevatorConstants.STALL_CURRENT - val stallTorque = ElevatorConstants.STALL_TORQUE + val freeSpeed = MotorConstants.FREE_SPEED + val freeCurrent = MotorConstants.FREE_CURRENT + val stallCurrent = MotorConstants.STALL_CURRENT + val stallTorque = MotorConstants.STALL_TORQUE private fun expDecelIntercept( vFree: Double, diff --git a/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt b/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt index 3c39475..8acffc5 100644 --- a/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt +++ b/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt @@ -44,7 +44,7 @@ class ChoreoRoutine( for (i in 0 until trajectories.size) { ezraGallun.addCommands( - ParallelCommandGroup( + ParallelDeadlineGroup( ChoreoFollower( drive, trajectories[i], diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt index 42351d7..ef0ed26 100644 --- a/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt +++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt @@ -17,7 +17,8 @@ class OnePiecePick( ChoreoRoutine( drive = robot.drive, parallelEventMap = hashMapOf( - 0 to robot.elevator.high(), + 0 to robot.intake.extend().andThen( + robot.elevator.high()), 1 to robot.elevator.stow() ), stopEventMap = hashMapOf( diff --git a/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt new file mode 100644 index 0000000..42987e3 --- /dev/null +++ b/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt @@ -0,0 +1,13 @@ +package frc.team449.robot2023.constants + +import edu.wpi.first.math.util.Units + +object MotorConstants { + /** NEO characteristics */ + const val EFFICIENCY = 0.90 + const val NOMINAL_VOLTAGE = 12.0 + const val STALL_TORQUE = 3.36 * EFFICIENCY + const val STALL_CURRENT = 166.0 + const val FREE_CURRENT = 1.3 + val FREE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5676.0) +} \ No newline at end of file diff --git a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt index 83af5c5..71947d7 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt @@ -3,6 +3,7 @@ package frc.team449.robot2023.constants import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.math.util.Units import edu.wpi.first.wpilibj.RobotBase import frc.team449.robot2023.constants.drives.SwerveConstants @@ -18,10 +19,21 @@ object RobotConstants { const val TRANSLATION_DEADBAND = .15 const val ROTATION_DEADBAND = .15 + /** In kilograms, include bumpers and battery and all */ + const val ROBOT_WEIGHT = 60 + /** Drive configuration */ const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s const val MAX_ROT_SPEED = PI // rad/s - val MAX_ACCEL = if (RobotBase.isSimulation()) 7.5 else 14.75 // m/s/s + val MAX_ACCEL = if (RobotBase.isSimulation()) 12.2625 else + 4 * DCMotor( + MotorConstants.NOMINAL_VOLTAGE, + MotorConstants.STALL_TORQUE, + MotorConstants.STALL_CURRENT, + MotorConstants.FREE_CURRENT, + MotorConstants.FREE_SPEED, + 1 + ).getTorque(80.0) * SwerveConstants.DRIVE_GEARING / (Units.inchesToMeters(2.0) * ROBOT_WEIGHT) // m/s/s val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)) const val LOOP_TIME = 0.020 diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt index 5e1f80d..14cd2d1 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt @@ -8,14 +8,6 @@ import kotlin.math.PI object ElevatorConstants { - /** NEO characteristics */ - const val EFFICIENCY = 0.90 - const val NOMINAL_VOLTAGE = 12.0 - const val STALL_TORQUE = 3.36 * EFFICIENCY - const val STALL_CURRENT = 166.0 - const val FREE_CURRENT = 1.3 - val FREE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5676.0) - /** Mechanism2d Visual constants */ const val ANGLE = 75.0 const val MIN_LENGTH = 0.1 @@ -23,18 +15,18 @@ object ElevatorConstants { val COLOR = Color8Bit(255, 0, 255) val DESIRED_COLOR = Color8Bit(0, 255, 0) - const val LEFT_ID = 20 + const val LEFT_ID = 60 const val LEFT_INVERTED = false - const val RIGHT_ID = 21 + const val RIGHT_ID = 61 const val RIGHT_INVERTED = true const val EFFECTIVE_GEARING = 6.48 / 2 const val PULLEY_RADIUS = 0.022352 const val UPR = PULLEY_RADIUS * 2 * PI var kS = 0.0 - var kG = 1.075 + var kG = 1.26725 - const val HIGH_DISTANCE = 1.4 + const val HIGH_DISTANCE = 1.114425 const val LOW_DISTANCE = 0.25 const val STOW_DISTANCE = 0.0 diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt index 08133b2..da711be 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt @@ -2,11 +2,13 @@ package frc.team449.robot2023.constants.subsystem object IntakeConstants { - const val MOTOR_ID = 25 + const val MOTOR_ID = 62 const val INVERTED = false const val FORWARD = 0 const val REVERSE = 1 const val INTAKE_VOLTAGE = 4.0 + + const val EXTENSION_TIME = 0.25 } diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt index 9740e40..1f0dd27 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt @@ -2,7 +2,7 @@ package frc.team449.robot2023.constants.subsystem object ManipulatorConstants { - const val MOTOR_ID = 26 + const val MOTOR_ID = 59 const val INVERTED = false val INTAKE_VOLTAGE = 3.0 diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt index 02607b2..d7c9c05 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt @@ -16,20 +16,31 @@ class ControllerBindings( fun bindButtons() { JoystickButton(mechanismController, XboxController.Button.kX.value).onTrue( - robot.elevator.low() + SequentialCommandGroup( + // How long does it take to extend and retract the intake? Is it the play to have mech manually extend intake right as we enter the + // community or have it automated such that it extends intake and moves elevator at the same time? +// robot.intake.extend(), + robot.elevator.low() + ) ) JoystickButton(mechanismController, XboxController.Button.kY.value).onTrue( - robot.elevator.high() + SequentialCommandGroup( +// robot.intake.extend(), + robot.elevator.high() + ) ) JoystickButton(mechanismController, XboxController.Button.kB.value).onTrue( - robot.elevator.stow() + SequentialCommandGroup( +// robot.intake.extend(), + robot.elevator.stow() + ) ) JoystickButton(mechanismController, XboxController.Button.kA.value).onTrue( SequentialCommandGroup( - robot.elevator.showSummaryStats(), + robot.elevator.summaryStats(), robot.elevator.tuneKG() ) ) @@ -55,7 +66,9 @@ class ControllerBindings( ) Trigger { abs(mechanismController.leftY) > 0.25 }.onTrue( - robot.elevator.manualMovement({ mechanismController.leftY }) + robot.elevator.manualMovement({ -mechanismController.leftY }) + ).onFalse( + robot.elevator.defaultCommand ) Trigger { driveController.leftTriggerAxis > 0.8 }.onTrue( diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt index 5cd2176..a4c0d71 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt @@ -7,7 +7,9 @@ import edu.wpi.first.wpilibj.DoubleSolenoid import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.SubsystemBase +import edu.wpi.first.wpilibj2.command.WaitCommand import frc.team449.robot2023.constants.subsystem.IntakeConstants import frc.team449.system.SparkUtil @@ -32,7 +34,12 @@ class Intake( } fun extend(): Command { - return this.runOnce { piston.set(DoubleSolenoid.Value.kForward) } + return SequentialCommandGroup( + this.runOnce { + piston.set(DoubleSolenoid.Value.kForward) + }, + WaitCommand(IntakeConstants.EXTENSION_TIME) + ) } fun retract(): Command { @@ -62,7 +69,7 @@ class Intake( override fun initSendable(builder: SendableBuilder) { builder.publishConstString("1.0", "Piston") - builder.addStringProperty("Piston Status", { piston.get().toString() }, {}) + builder.addStringProperty("1.1 Piston Status", { piston.get().toString() }, {}) builder.publishConstString("2.0", "Motor Voltages") builder.addDoubleProperty("2.1 Last Voltage", { lastVoltage }, null) diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt index 1cab682..87b8506 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt @@ -1,5 +1,6 @@ package frc.team449.robot2023.subsystems.elevator +import edu.wpi.first.math.MathUtil import edu.wpi.first.math.Nat import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.controller.LinearQuadraticRegulator @@ -19,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.* import frc.team449.control.TrapezoidalExponentialProfile import frc.team449.robot2023.Robot +import frc.team449.robot2023.constants.MotorConstants import frc.team449.robot2023.constants.subsystem.ElevatorConstants import frc.team449.robot2023.subsystems.Intake import frc.team449.system.motor.WrappedMotor @@ -87,26 +89,20 @@ open class Elevator( ) } - private fun isFinished(goal: Double, epsilon: Double = 1e-4): Boolean { - return if (abs(desiredState.first - goal) < epsilon) { - true - } else { - false - } - } - - private fun summaryStats() { - val summary = dtList.stream().mapToDouble { it * 1000 }.summaryStatistics() + fun summaryStats(): Command { + return InstantCommand({ + val summary = dtList.stream().mapToDouble { it * 1000 }.summaryStatistics() - var variance = 0.0 + var variance = 0.0 - for (num in dtList) { - variance += (num * 1000 - summary.average).pow(2) - } + for (num in dtList) { + variance += (num * 1000 - summary.average).pow(2) + } - variance /= dtList.size + variance /= dtList.size - println("Count: ${summary.count}, Average (ms): ${summary.average}, Min (ms): ${summary.min}, Max (ms): ${summary.max}, Std dev (ms) ${sqrt(variance)}") + println("Count: ${summary.count}, Average (ms): ${summary.average}, Min (ms): ${summary.min}, Max (ms): ${summary.max}, Std dev (ms) ${sqrt(variance)}") + }) } private fun moveToPos(distance: Double): Command { @@ -142,35 +138,34 @@ open class Elevator( ElevatorConstants.DT, this ) - .until { isFinished(distance) } - .andThen(InstantCommand({ desiredState = Pair(distance, 0.0) })) ) } - fun manualMovement(movement: DoubleSupplier, rate: Double = 0.0075): Command { + fun manualMovement(movement: DoubleSupplier, rate: Double = 0.5 / 200): Command { return NotifierCommand( { - var setpoint = currentState.first - if (movement.asDouble < 0 && currentState.first > ElevatorConstants.MIN_SAFE_POS + 0.1 || - intake.piston.get() == DoubleSolenoid.Value.kForward || - movement.asDouble > 0 && currentState.first > ElevatorConstants.MIN_SAFE_POS - ) { - setpoint += currentState.first + movement.asDouble * rate + desiredState = if (movement.asDouble < 0 && currentState.first > ElevatorConstants.MIN_SAFE_POS + 0.1 + || intake.piston.get() == DoubleSolenoid.Value.kForward + || movement.asDouble > 0 && currentState.first > ElevatorConstants.MIN_SAFE_POS) { + Pair( + MathUtil.clamp(desiredState.first + movement.asDouble.pow(2) * rate * sign(movement.asDouble), 0.0, ElevatorConstants.HIGH_DISTANCE), + movement.asDouble.pow(2) * sign(movement.asDouble) * rate * 200) + } else { + Pair(desiredState.first, 0.0) } - loop.setNextR(setpoint, 0.0) + loop.setNextR(desiredState.first, desiredState.second) loop.correct(VecBuilder.fill(positionSupplier.get())) loop.predict(ElevatorConstants.DT) motor.setVoltage( loop.getU(0) + ElevatorConstants.kG + sign(desiredState.second) * ElevatorConstants.kS ) - - desiredState = Pair(setpoint, 0.0) }, ElevatorConstants.DT, this ) + .handleInterrupt { desiredState = Pair(desiredState.first, 0.0) } } fun high(): Command { @@ -191,20 +186,14 @@ open class Elevator( return ConditionalCommand( moveToPos(ElevatorConstants.STOW_DISTANCE), InstantCommand() - ) { - currentState.first > ElevatorConstants.MIN_SAFE_POS && ElevatorConstants.STOW_DISTANCE > ElevatorConstants.MIN_SAFE_POS || - intake.piston.get() == DoubleSolenoid.Value.kForward - } + ) { currentState.first > ElevatorConstants.MIN_SAFE_POS && ElevatorConstants.STOW_DISTANCE > ElevatorConstants.MIN_SAFE_POS + || intake.piston.get() == DoubleSolenoid.Value.kForward } } fun tuneKG(): Command { return this.run { motor.setVoltage(ElevatorConstants.kG) } } - fun showSummaryStats(): Command { - return InstantCommand(::summaryStats) - } - fun stop(): Command { return this.runOnce { motor.stopMotor() @@ -239,11 +228,11 @@ open class Elevator( fun createStateSpaceElevator(robot: Robot): Elevator { val plant = LinearSystemId.createElevatorSystem( DCMotor( - ElevatorConstants.NOMINAL_VOLTAGE, - ElevatorConstants.STALL_TORQUE, - ElevatorConstants.STALL_CURRENT, - ElevatorConstants.FREE_CURRENT, - ElevatorConstants.FREE_SPEED, + MotorConstants.NOMINAL_VOLTAGE, + MotorConstants.STALL_TORQUE, + MotorConstants.STALL_CURRENT, + MotorConstants.FREE_CURRENT, + MotorConstants.FREE_SPEED, ElevatorConstants.NUM_MOTORS ), ElevatorConstants.CARRIAGE_MASS, diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt index 84551b1..c7c4f88 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt @@ -7,6 +7,7 @@ import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.math.util.Units import edu.wpi.first.util.sendable.SendableBuilder import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.team449.robot2023.constants.MotorConstants import frc.team449.robot2023.constants.RobotConstants import frc.team449.robot2023.constants.subsystem.ElevatorConstants import frc.team449.robot2023.subsystems.Intake @@ -23,11 +24,11 @@ class ElevatorSim( private val elevatorSim = TiltedElevatorSim( DCMotor( - ElevatorConstants.NOMINAL_VOLTAGE, - ElevatorConstants.STALL_TORQUE, - ElevatorConstants.STALL_CURRENT, - ElevatorConstants.FREE_CURRENT, - ElevatorConstants.FREE_SPEED, + MotorConstants.NOMINAL_VOLTAGE, + MotorConstants.STALL_TORQUE, + MotorConstants.STALL_CURRENT, + MotorConstants.FREE_CURRENT, + MotorConstants.FREE_SPEED, ElevatorConstants.NUM_MOTORS ), ElevatorConstants.EFFECTIVE_GEARING,