diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c93d59b..52e1d66 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,8 @@ public final class Constants { public static final double stickDeadband = 0.1; - - public static final class IntakeArm{ //TODO: This must be tuned to specific robot + + public static final class IntakeArm { // TODO: This must be tuned to specific robot // mm public static final int KMotorID = 0; public static final double mmVelocity = 5.0; @@ -48,7 +48,7 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo public static final boolean ForwardSoftLimitEnable = true; public static final double ForwardSoftLimitThreshold = 300; public static final boolean ReverseSoftLimitEnable = true; - public static final double RevesrseSoftLimitThreshold = 0; + public static final double RevesrseSoftLimitThreshold = 0; // not mm public static final double minimumError = 0; public static final double startingValue = 0; @@ -58,8 +58,9 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo public static final double trapSetPoint = 0; // switch public static final int switchID = 0; - + } + public static final class Swerve { public static final double minimumErrorAligning = 0; // TODO: This must be tuned to specific robot public static final PIDController aligningPID = new PIDController(0, 0, 0); @@ -67,21 +68,24 @@ public static final class Swerve { public static final int pigeonID = 10; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- - public static final COTSFalconSwerveConstants chosenModule = - COTSFalconSwerveConstants.SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4i_L1); + public static final COTSFalconSwerveConstants chosenModule = COTSFalconSwerveConstants + .SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4i_L1); /* Drivetrain Constants */ public static final double trackWidth = 0.62; public static final double wheelBase = 0.62; public static final double wheelCircumference = chosenModule.wheelCircumference; - /* Swerve Kinematics - * No need to ever change this unless you are not doing a traditional rectangular/square 4 module swerve */ - public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(wheelBase / 2.0, trackWidth / 2.0), - new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), - new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), - new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); + /* + * Swerve Kinematics + * No need to ever change this unless you are not doing a traditional + * rectangular/square 4 module swerve + */ + public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( + new Translation2d(wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); /* Module Gear Ratios */ public static final double driveGearRatio = chosenModule.driveGearRatio; @@ -105,36 +109,39 @@ public static final class Swerve { public static final double driveCurrentThresholdTime = 0.1; public static final boolean driveEnableCurrentLimit = true; - /* These values are used by the drive falcon to ramp in open loop and closed loop driving. - * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc */ + /* + * These values are used by the drive falcon to ramp in open loop and closed + * loop driving. + * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc + */ public static final double openLoopRamp = 0.25; public static final double closedLoopRamp = 0.0; public static final double voltageComp = 12; /* Angle Motor PID Values */ - public static final double angleKP = 0.08;//chosenModule.angleKP; + public static final double angleKP = 0.08;// chosenModule.angleKP; public static final double angleKI = chosenModule.angleKI; public static final double angleKD = chosenModule.angleKD; public static final double angleKFF = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.2; //TODO: This must be tuned to specific robot + public static final double driveKP = 0.2; // TODO: This must be tuned to specific robot public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; /* Drive Motor Characterization Values From SYSID */ - public static final double driveKS = 0.15; //TODO: This must be tuned to specific robot + public static final double driveKS = 0.15; // TODO: This must be tuned to specific robot public static final double driveKV = 2.7; public static final double driveKA = 0; /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 4.5; //TODO: This must be tuned to specific robot - public static final double XYSlowRatio = 0.25; // TODO: make it more accrute - public static final double rotationSlowRatio = 0.25; // TODO: make it more accrute + public static final double maxSpeed = 4.5; // TODO: This must be tuned to specific robot + public static final double XYSlowRatio = 0.25; // TODO: make it more accrute + public static final double rotationSlowRatio = 0.25; // TODO: make it more accrute /** Radians per Second */ - public static final double maxAngularVelocity = 10.0; //TODO: This must be tuned to specific robot + public static final double maxAngularVelocity = 10.0; // TODO: This must be tuned to specific robot /* Neutral Modes */ public static final IdleMode angleNeutralMode = IdleMode.kCoast; @@ -146,9 +153,9 @@ public static final class Mod0 { public static final int driveMotorID = 6; public static final int angleMotorID = 2; public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.113770 ); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.113770); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } /* Front Right Module - Module 1 */ @@ -157,32 +164,33 @@ public static final class Mod1 { public static final int angleMotorID = 1; public static final int canCoderID = 1; public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.474121); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } - + /* Back Left Module - Module 2 */ public static final class Mod2 { public static final int driveMotorID = 7; public static final int angleMotorID = 3; public static final int canCoderID = 3; public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.403320); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } /* Back Right Module - Module 3 */ - public static final class Mod3 { + public static final class Mod3 { public static final int driveMotorID = 8; public static final int angleMotorID = 4; public static final int canCoderID = 4; - public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.387695 ); - public static final SwerveModuleConstants constants = - new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.387695); + public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, + canCoderID, angleOffset); } } - public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot + public static final class AutoConstants { // TODO: The below constants are used in the example auto, and must be + // tuned to specific robot public static final double kMaxSpeedMetersPerSecond = 3.5; public static final double kMaxAccelerationMetersPerSecondSquared = 9; public static final double kMaxAngularSpeedRadiansPerSecond = 9; @@ -191,17 +199,17 @@ public static final class AutoConstants { //TODO: The below constants are used i public static final PIDConstants rotation_PID = new PIDConstants(3, 0); public static final PIDConstants XY_PID = new PIDConstants(3, 0); - public static final double driveBaseRadius = - Math.sqrt(Math.pow((Constants.Swerve.wheelBase / 2), 2) - + Math.pow((Constants.Swerve.trackWidth / 2), 2)); + public static final double driveBaseRadius = Math.sqrt(Math.pow((Constants.Swerve.wheelBase / 2), 2) + + Math.pow((Constants.Swerve.trackWidth / 2), 2)); public static final ReplanningConfig replanningConfig = new ReplanningConfig(true, true); - - public static final PathConstraints pathConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared, kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); - + + public static final PathConstraints pathConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared, kMaxAngularSpeedRadiansPerSecond, + kMaxAngularSpeedRadiansPerSecondSquared); + /* Constraint for the motion profilied robot angle controller */ - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } @@ -211,12 +219,12 @@ public static class Vision { // Cam mounted facing forward, half a meter forward of center, half a meter up // from center. public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + new Rotation3d(0, 0, 0)); public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + new Rotation3d(0, 0, 0)); // The layout of the AprilTags on the field - public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); // The standard deviations of our vision estimated poses, which affect // correction rate @@ -227,61 +235,61 @@ public static class Vision { public static final Matrix kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0))); - } - - public static class ShooterArmConstants { - - // technical Constants - public static final int ShooterArmID = 0; - public static final int SwitchID = 1; - public static final int encoderCountsPerRevolution =1; - public static final double gearRatio = 50.0; - public static final double TICKS_PER_DEGREE = encoderCountsPerRevolution * gearRatio / 360.0; - - // condition Costants - public static final double minimumError = 2.0; - - // motionMagic Constants - public static final double mmCruise = 5; - public static final double mmAcceleration = 10; - public static final double mmJerk = 50; - - public static final double kp = 24; - public static final double kd = 0.1; - public static final double ks = 24; - public static final double ka = 24; - public static final double kv = 24; - - // MaxVol Constant - public static final double peekReverseVoltage = -11.5; - public static final double peekForwardVoltage = 11.5; - - // Constant limit values - public static final double forwardLimit = 300; - public static final double backwordLimit = 300; - - // ArmPoseReset Constant - public static final double resetPose = 0.0; - public static final double resetSpeed = -1.0; - - public static final double startPose =0.0; - - public static final InterpolationMap SHOOTER_ANGLE_INTERPOLATION_MAP = new InterpolationMap() - .put(1, 14100) - .put(1.2, 13800) - .put(1.4, 12800) - .put(1.6, 12800) - .put(1.8, 12900) - .put(2, 13200) - .put(2.1, 13300); -} + public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0))); + } + + public static class ShooterArmConstants { + + // technical Constants + public static final int ShooterArmID = 0; + public static final int SwitchID = 1; + public static final int encoderCountsPerRevolution = 1; + public static final double gearRatio = 50.0; + public static final double TICKS_PER_DEGREE = encoderCountsPerRevolution * gearRatio / 360.0; + + // condition Costants + public static final double minimumError = 2.0; + + // motionMagic Constants + public static final double mmCruise = 5; + public static final double mmAcceleration = 10; + public static final double mmJerk = 50; + + public static final double kp = 24; + public static final double kd = 0.1; + public static final double ks = 24; + public static final double ka = 24; + public static final double kv = 24; + + // MaxVol Constant + public static final double peekReverseVoltage = -11.5; + public static final double peekForwardVoltage = 11.5; + + // Constant limit values + public static final double forwardLimit = 300; + public static final double backwordLimit = 300; + + // ArmPoseReset Constant + public static final double resetPose = 0.0; + public static final double resetSpeed = -1.0; + + public static final double startPose = 0.0; + + public static final InterpolationMap SHOOTER_ANGLE_INTERPOLATION_MAP = new InterpolationMap() + .put(1, 14100) + .put(1.2, 13800) + .put(1.4, 12800) + .put(1.6, 12800) + .put(1.8, 12900) + .put(2, 13200) + .put(2.1, 13300); + } public static class ShooterConstants { - //m_shooterMotor ID + // m_shooterMotor ID public static final int kMotorShooterID = 20; - //Motion Magic Values + // Motion Magic Values public static final int MotionMagicCruiseVelocity = 5; public static final int MotionMagicAcceleration = 10; public static final int MotionMagicJerk = 50; @@ -292,45 +300,56 @@ public static class ShooterConstants { public static final int SensorToMechanismRatio = 50; public static final int MaxError = 13; - - //PID values + + // PID values public static final int kP = 24; public static final double kD = 0.1; public static final double kS = 0.12; public static final double kV = 0.25; - //Interpolation Map + // Interpolation Map public static final InterpolationMap ShooterInterpolation = new InterpolationMap() - .put(1,9) - .put(1.2, 9.2) - .put(1.4, 9.4) - .put(1.6, 9.6) - .put(1.8, 9.8) - .put(2, 10) - .put(2.1, 10.2); + .put(1, 9) + .put(1.2, 9.2) + .put(1.4, 9.4) + .put(1.6, 9.6) + .put(1.8, 9.8) + .put(2, 10) + .put(2.1, 10.2); } - public static class IntakeConstants{ - public static final int SwitchID=1; - public static final int IntakeMotorID=10; + + public static class IntakeConstants { + public static final int SwitchID = 1; + public static final int IntakeMotorID = 10; public static final double getNoteSpeed = 0.0; public static final double GroundIntakePose = 0.0; } - public static class FeederConstants{ - public static final int FeederShootSpeed = 20; - public static final int FeederMotorId = 3; - public static final int SwitchID = 0; - public static final double FeederMotorSpeed = 0.8; - public static final double getNoteSpeed = 0; + public static class FeederConstants { + + public static final int FeederShootSpeed = 20; + public static final int FeederMotorId = 3; + public static final int SwitchID = 0; + public static final double FeederMotorSpeed = 0.8; + public static final double getNoteSpeed = 0; + } - } - - public static class TrapArmConstants{ + public static class TrapArmConstants { public static final int ArmMotorID = 10; - public static final int InnerSwitchID= 0; - public static final int OuterSwitchID= 10; + public static final int InnerSwitchID = 0; + public static final int OuterSwitchID = 10; + + } + + public static class trapConstants { + public static final int TRAP_MOTOR_ID = 0; + + } + public static class climbingConstants { + public static final int M_CLIMBINGRIGHT_MOTOR_ID = 1; + public static final int M_CLIMBINGLEFT_MOTOR_ID = 2; } } diff --git a/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedLeftMotorClimbing.java b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedLeftMotorClimbing.java new file mode 100644 index 0000000..a39d092 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedLeftMotorClimbing.java @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.ClimbingCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.ClimbingSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class SetSpeedLeftMotorClimbing extends InstantCommand { + private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance(); + double speed; + public SetSpeedLeftMotorClimbing(double speed) { + this.addRequirements(climbingSubsystem); + this.speed = speed; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.climbingSubsystem.setSpeedClimbing(speed); + + } +} diff --git a/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedMotorsClimbing.java b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedMotorsClimbing.java new file mode 100644 index 0000000..4dc8bf0 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedMotorsClimbing.java @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.ClimbingCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.ClimbingSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class SetSpeedMotorsClimbing extends InstantCommand { + private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance(); + double speed; + public SetSpeedMotorsClimbing(double speed) { + this.addRequirements(climbingSubsystem); + this.speed = speed; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.climbingSubsystem.setSpeedClimbing(speed); + + } +} diff --git a/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedRightMotorClimbing.java b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedRightMotorClimbing.java new file mode 100644 index 0000000..cf961fb --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedRightMotorClimbing.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.ClimbingCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.ClimbingSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class SetSpeedRightMotorClimbing extends InstantCommand { + private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance(); + double speed; + public SetSpeedRightMotorClimbing(double speed) { + this.addRequirements(climbingSubsystem); + this.speed = speed; + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.climbingSubsystem.setSpeedClimbing(speed); + + } +} diff --git a/src/main/java/frc/robot/commands/TrapSetSpeed.java b/src/main/java/frc/robot/commands/TrapSetSpeed.java new file mode 100644 index 0000000..e0b8d27 --- /dev/null +++ b/src/main/java/frc/robot/commands/TrapSetSpeed.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.SubsystemTrap; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class TrapSetSpeed extends InstantCommand { + private final SubsystemTrap subsystemTrap = SubsystemTrap.getInstance(); + double speed; + public TrapSetSpeed(double speed) { + this.addRequirements(subsystemTrap); + this.speed = speed; + + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.subsystemTrap.setSpeed(speed); + + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimbingSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbingSubsystem.java new file mode 100644 index 0000000..9c7d4e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimbingSubsystem.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.climbingConstants.*; + +public class ClimbingSubsystem extends SubsystemBase { + /** Creates a new ClimbingSubsystem. */ + private CANSparkMax m_climbingRight; + private CANSparkMax m_climbingLeft; + + private static ClimbingSubsystem instance; + + public static ClimbingSubsystem getInstance() { + if (instance == null) { + instance = new ClimbingSubsystem(); + } + return instance; + } + + private ClimbingSubsystem() { + m_climbingLeft = new CANSparkMax(M_CLIMBINGLEFT_MOTOR_ID, MotorType.kBrushless); + m_climbingRight = new CANSparkMax(M_CLIMBINGRIGHT_MOTOR_ID, MotorType.kBrushless); + + } + + public void setSpeedClimbing(double speed) { + m_climbingLeft.set(speed); + m_climbingRight.set(speed); + } + + public void setRightSpeed(double speed) { + m_climbingRight.set(speed); + } + + public void setLeftSpeed(double speed) { + m_climbingLeft.set(speed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/SubsystemTrap.java b/src/main/java/frc/robot/subsystems/SubsystemTrap.java new file mode 100644 index 0000000..ea9903d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SubsystemTrap.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import static frc.robot.Constants.trapConstants.*; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SubsystemTrap extends SubsystemBase { + private WPI_TalonSRX m_trapMotor; + + private static SubsystemTrap instance; + + public static SubsystemTrap getInstance() { + if (instance == null) { + instance = new SubsystemTrap(); + } + return instance; + } + + /** Creates a new SubsystemTrap. */ + private SubsystemTrap() { + m_trapMotor = new WPI_TalonSRX(TRAP_MOTOR_ID); + + } + + public void setSpeed(double speed) { + m_trapMotor.set(speed); + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}