From 0e48311d32e10cde3888d339aa34817388f2b585 Mon Sep 17 00:00:00 2001 From: John Joseph Date: Tue, 12 Dec 2023 10:46:11 -0500 Subject: [PATCH] Inverse gearing for state space and profile, assign animation for disable, teleop, auto, extend intake, running intake --- simgui-ds.json | 1 + simgui.json | 8 +++++-- src/main/kotlin/frc/team449/RobotLoop.kt | 17 ++++++++++++-- .../control/TrapezoidalExponentialProfile.kt | 4 ++-- .../robot2023/commands/light/SolidColor.kt | 3 +-- .../constants/subsystem/ElevatorConstants.kt | 2 +- .../subsystems/ControllerBindings.kt | 23 +++++++++++-------- .../robot2023/subsystems/elevator/Elevator.kt | 6 +++-- .../subsystems/elevator/ElevatorSim.kt | 2 +- 9 files changed, 45 insertions(+), 21 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..c2bdbe4 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,7 @@ } ], "robotJoysticks": [ + {}, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index 9f7d37d..603462e 100644 --- a/simgui.json +++ b/simgui.json @@ -105,10 +105,14 @@ "open": true } }, + "elevator": { + "open": true + }, "intake": { "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": { "open": true - } + }, + "open": true }, "open": true } @@ -152,7 +156,7 @@ 0.0, 0.8500000238418579 ], - "height": 926, + "height": 306, "series": [ { "color": [ diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt index 71a9db5..558723f 100644 --- a/src/main/kotlin/frc/team449/RobotLoop.kt +++ b/src/main/kotlin/frc/team449/RobotLoop.kt @@ -5,9 +5,12 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler +import edu.wpi.first.wpilibj2.command.InstantCommand import frc.team449.robot2023.Robot import frc.team449.robot2023.auto.routines.RoutineChooser import frc.team449.robot2023.commands.light.BlairChasing +import frc.team449.robot2023.commands.light.BreatheHue +import frc.team449.robot2023.commands.light.Rainbow import frc.team449.robot2023.constants.vision.VisionConstants import frc.team449.robot2023.subsystems.ControllerBindings import monologue.Logged @@ -50,11 +53,11 @@ class RobotLoop : TimedRobot(), Logged { SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance()) SmartDashboard.putBoolean("Enable Logging?", false) + robot.light.defaultCommand = BlairChasing(robot.light) + controllerBinder.bindButtons() Monologue.setupLogging(this, "/Monologuing") - - robot.light.defaultCommand = BlairChasing(robot.light) } override fun robotPeriodic() { @@ -77,6 +80,12 @@ class RobotLoop : TimedRobot(), Logged { /** Every time auto starts, we update the chosen auto command */ this.autoCommand = routineMap[routineChooser.selected] CommandScheduler.getInstance().schedule(this.autoCommand) + + if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { + BreatheHue(robot.light, 0).schedule() + } else { + BreatheHue(robot.light, 95).schedule() + } } override fun autonomousPeriodic() {} @@ -88,6 +97,8 @@ class RobotLoop : TimedRobot(), Logged { CommandScheduler.getInstance().cancel(autoCommand) } + (robot.light.currentCommand?: InstantCommand()).cancel() + robot.drive.defaultCommand = robot.driveCommand } @@ -97,6 +108,8 @@ class RobotLoop : TimedRobot(), Logged { override fun disabledInit() { robot.drive.stop() + Rainbow(robot.light).schedule() + // if (VisionConstants.ESTIMATORS.isEmpty()) { // VisionConstants.ESTIMATORS.add( // VisionEstimator( diff --git a/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt b/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt index 4b66238..1870fb2 100644 --- a/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt +++ b/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt @@ -11,7 +11,7 @@ class TrapezoidalExponentialProfile( pulleyRadius: Double = ElevatorConstants.PULLEY_RADIUS, currentLimit: Int = ElevatorConstants.CURRENT_LIMIT, numMotors: Int = ElevatorConstants.NUM_MOTORS, - effectiveGearing: Double = ElevatorConstants.EFFECTIVE_GEARING, + effectiveGearing: Double = 1 / ElevatorConstants.EFFECTIVE_GEARING, systemMass: Double = ElevatorConstants.CARRIAGE_MASS, angle: Double = ElevatorConstants.ANGLE, private val tolerance: Double = 0.05, @@ -19,7 +19,7 @@ class TrapezoidalExponentialProfile( startingDistance: Double = 0.0, private var finalDistance: Double, private var aStop: Double = 9.81, - private val efficiency: Double = ElevatorConstants.EFFICIENCY + efficiency: Double = ElevatorConstants.EFFICIENCY ) { private val trueStartingDistance: Double = startingDistance private val trueFinalDistance: Double = finalDistance diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt index 85f19c2..0449d6a 100644 --- a/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt +++ b/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt @@ -3,8 +3,7 @@ package frc.team449.robot2023.commands.light import edu.wpi.first.wpilibj2.command.Command import frc.team449.robot2023.subsystems.light.Light -/** Description: Have a random color set for every led every 20ms - * a.k.a. you go blind */ +/** Description: Have a solid color */ class SolidColor( private val led: Light, private val r: Int, 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 eac86c1..dabf0f2 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt @@ -23,7 +23,7 @@ object ElevatorConstants { var UPR = PULLEY_RADIUS * 2 * PI var kS = 0.0 - var kG = 1.26725 + var kG = 1.085 const val HIGH_DISTANCE = 1.114425 const val LOW_DISTANCE = 0.25 diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt index 1cd7bc3..11b3a39 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton import edu.wpi.first.wpilibj2.command.button.Trigger import frc.team449.robot2023.Robot import frc.team449.robot2023.commands.characterization.Characterization -import frc.team449.robot2023.commands.light.SolidColor +import frc.team449.robot2023.commands.light.BreatheHue import kotlin.math.abs class ControllerBindings( @@ -29,7 +29,7 @@ class ControllerBindings( JoystickButton(mechanismController, XboxController.Button.kY.value).onTrue( SequentialCommandGroup( - robot.intake.extend(), +// robot.intake.extend(), robot.elevator.high() ) ) @@ -70,15 +70,17 @@ class ControllerBindings( ) ) - Trigger { driveController.rightTriggerAxis > 0.8 }.onTrue( + Trigger { mechanismController.rightTriggerAxis > 0.8 }.onTrue( ParallelCommandGroup( robot.intake.intake(), - robot.manipulator.intake() + robot.manipulator.intake(), + BreatheHue(robot.light, 30) ) ).onFalse( ParallelCommandGroup( robot.intake.stop(), - robot.manipulator.stop() + robot.manipulator.stop(), + robot.light.defaultCommand ) ) @@ -88,20 +90,23 @@ class ControllerBindings( robot.elevator.defaultCommand ) - Trigger { driveController.leftTriggerAxis > 0.8 }.onTrue( + Trigger { mechanismController.leftTriggerAxis > 0.8 }.onTrue( ParallelCommandGroup( robot.intake.outtake(), - robot.manipulator.outtake() + robot.manipulator.outtake(), + BreatheHue(robot.light, 160) + ) ).onFalse( ParallelCommandGroup( robot.intake.stop(), - robot.manipulator.stop() + robot.manipulator.stop(), + robot.light.defaultCommand ) ) Trigger { robot.intake.piston.get() == DoubleSolenoid.Value.kForward }.onTrue( - SolidColor(robot.light, 0, 255, 0) + BreatheHue(robot.light, 60) ).onFalse( robot.light.defaultCommand ) 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 bc9b5ee..7ef386a 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt @@ -149,7 +149,9 @@ open class Elevator( loop.predict(ElevatorConstants.DT) motor.setVoltage( - loop.getU(0) + ElevatorConstants.kG + sign(desiredState.second) * ElevatorConstants.kS + loop.getU(0) + + ElevatorConstants.kG + + sign(desiredState.second) * ElevatorConstants.kS ) dtList.add(dt) @@ -268,7 +270,7 @@ open class Elevator( ), ElevatorConstants.CARRIAGE_MASS, ElevatorConstants.PULLEY_RADIUS, - ElevatorConstants.EFFECTIVE_GEARING + 1 / ElevatorConstants.EFFECTIVE_GEARING ) val observer = KalmanFilter( 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 eccb41a..ea891ed 100644 --- a/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt +++ b/src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt @@ -32,7 +32,7 @@ class ElevatorSim( MotorConstants.FREE_SPEED, ElevatorConstants.NUM_MOTORS ), - ElevatorConstants.EFFECTIVE_GEARING, + 1 / ElevatorConstants.EFFECTIVE_GEARING, ElevatorConstants.CARRIAGE_MASS, ElevatorConstants.PULLEY_RADIUS, 0.0,