diff --git a/src/main/kotlin/frc/engine/odometry/Vision.kt b/src/main/kotlin/frc/engine/odometry/Vision.kt index e5d26e5..fea6bb9 100644 --- a/src/main/kotlin/frc/engine/odometry/Vision.kt +++ b/src/main/kotlin/frc/engine/odometry/Vision.kt @@ -13,8 +13,8 @@ import edu.wpi.first.networktables.NetworkTableInstance import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.engine.utils.Sugar.degreesToRadians -import frc.robot.Constants import frc.robot.subsystems.Odometry +import frc.robot.Safety import org.photonvision.EstimatedRobotPose import org.photonvision.PhotonCamera import org.photonvision.PhotonPoseEstimator @@ -64,6 +64,6 @@ class Vision ( return null } fun getStdDev() : Matrix { - return Constants.OdometryConstants.VisionDeviation + return Safety.OdometryConstants.VisionDeviation } } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/Commands/Autos/Preload+BottomNote.kt b/src/main/kotlin/frc/robot/Commands/Autos/Preload+BottomNote.kt index e01044a..97967a6 100644 --- a/src/main/kotlin/frc/robot/Commands/Autos/Preload+BottomNote.kt +++ b/src/main/kotlin/frc/robot/Commands/Autos/Preload+BottomNote.kt @@ -6,21 +6,20 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import frc.robot.Commands.Basic.DrivetrainControl.followPathCommand import frc.robot.Commands.Basic.IntakeControl import frc.robot.Commands.Basic.ShootNote -import frc.robot.Constants -import frc.robot.subsystems.Intake +import frc.robot.subsystems.Shooter class `Preload+BottomNote` : Command() { private lateinit var autoCommandGroup : SequentialCommandGroup override fun initialize() { autoCommandGroup = SequentialCommandGroup ( - ShootNote(Constants.ShooterConstants.SpeakerSpeed), + ShootNote(Shooter.SPEAKER_SPEED), ParallelRaceGroup( followPathCommand("BottomSpeakerToBottomNote"), IntakeControl.Pickup() ), followPathCommand("BottomNoteToBottomSpeaker"), - ShootNote(Constants.ShooterConstants.SpeakerSpeed) + ShootNote(Shooter.SPEAKER_SPEED) ) autoCommandGroup.schedule() } diff --git a/src/main/kotlin/frc/robot/Commands/Autos/Preload+Mobility.kt b/src/main/kotlin/frc/robot/Commands/Autos/Preload+Mobility.kt index 3bcb18c..89b58cc 100644 --- a/src/main/kotlin/frc/robot/Commands/Autos/Preload+Mobility.kt +++ b/src/main/kotlin/frc/robot/Commands/Autos/Preload+Mobility.kt @@ -4,15 +4,14 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import frc.robot.Commands.Basic.DrivetrainControl.followPathCommand import frc.robot.Commands.Basic.ShootNote -import frc.robot.Constants -import frc.robot.subsystems.Intake +import frc.robot.Safety import frc.robot.subsystems.Shooter class `Preload+Mobility` : Command() { private lateinit var autoCommandGroup : SequentialCommandGroup override fun initialize() { autoCommandGroup = SequentialCommandGroup ( - ShootNote(Constants.ShooterConstants.SpeakerSpeed), + ShootNote(Shooter.SPEAKER_SPEED), followPathCommand("BottomSpeakerToMobility") ) autoCommandGroup.schedule() diff --git a/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload+Mobility.kt b/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload+Mobility.kt index e668296..f4a0eaa 100644 --- a/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload+Mobility.kt +++ b/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload+Mobility.kt @@ -3,7 +3,6 @@ package frc.robot.Commands.Autos import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import frc.robot.Commands.Basic.* -import frc.robot.Constants /** * Runs shootNoteOpenLoop (Ignore the jank) diff --git a/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload.kt b/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload.kt index 1ec7fde..d9de69a 100644 --- a/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload.kt +++ b/src/main/kotlin/frc/robot/Commands/Autos/TimedPreload.kt @@ -3,7 +3,6 @@ package frc.robot.Commands.Autos import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import frc.robot.Commands.Basic.* -import frc.robot.Constants /** * Runs shootNoteOpenLoop (Ignore the jank) diff --git a/src/main/kotlin/frc/robot/Commands/Autos/TimedTwoNoteAuto.kt b/src/main/kotlin/frc/robot/Commands/Autos/TimedTwoNoteAuto.kt index c230d58..2cd6ae8 100644 --- a/src/main/kotlin/frc/robot/Commands/Autos/TimedTwoNoteAuto.kt +++ b/src/main/kotlin/frc/robot/Commands/Autos/TimedTwoNoteAuto.kt @@ -3,7 +3,7 @@ package frc.robot.Commands.Autos import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import frc.robot.Commands.Basic.* -import frc.robot.Constants + /* Working settings: private val backupVoltage : Double = -4.0, diff --git a/src/main/kotlin/frc/robot/Commands/Basic/IntakeControl.kt b/src/main/kotlin/frc/robot/Commands/Basic/IntakeControl.kt index 3845034..c03b24d 100644 --- a/src/main/kotlin/frc/robot/Commands/Basic/IntakeControl.kt +++ b/src/main/kotlin/frc/robot/Commands/Basic/IntakeControl.kt @@ -3,7 +3,6 @@ package frc.robot.Commands.Basic import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.Command import frc.engine.utils.RPM -import frc.robot.Constants import frc.robot.subsystems.Intake import frc.robot.subsystems.Shooter @@ -12,8 +11,8 @@ object IntakeControl { * @property speed The speed to run the intake at * @property time The time to run the intake for */ class Feed( - private val speed: Double = Constants.IntakeConstants.feedingSpeed, - private val time: Double = Constants.IntakeConstants.feedingTime + private val speed: Double = Intake.FEEDING_SPEED, + private val time: Double = Intake.FEEDING_SPEED ) : Command() { private val timer = Timer() override fun initialize() = timer.restart() @@ -23,14 +22,14 @@ object IntakeControl { } class Pickup ( - private val speed: Double = Constants.IntakeConstants.pickupSpeed + private val speed: Double = Intake.PICKUP_SPEED ) : Command() { override fun execute() = Intake.runIntake(speed) override fun end(interrupted: Boolean) = Intake.stop() } class TimedPickup ( - private val speed: Double = Constants.IntakeConstants.pickupSpeed, - private val time: Double = 1.0 + private val speed: Double = Intake.PICKUP_SPEED, + private val time: Double = 1.0 ) : Command() { private val timer = Timer() override fun initialize() = timer.restart() diff --git a/src/main/kotlin/frc/robot/Commands/TeleOp.kt b/src/main/kotlin/frc/robot/Commands/TeleOp.kt index 2bf4dab..69916c7 100644 --- a/src/main/kotlin/frc/robot/Commands/TeleOp.kt +++ b/src/main/kotlin/frc/robot/Commands/TeleOp.kt @@ -12,9 +12,9 @@ import frc.engine.utils.RPM import frc.engine.utils.RotationsPerSecond import frc.engine.utils.Sugar.clamp import frc.engine.utils.Sugar.within -import frc.robot.Constants +import frc.robot.Safety -import frc.robot.Constants.TeleopConstants as C +import frc.robot.Safety.TeleopConstants as C import frc.robot.subsystems.* import kotlin.math.* @@ -48,7 +48,7 @@ object TeleOp : Command() { Rumble.update() } private fun handleDrive(){ - var baseSpeed = if (OI.speedLower) C.SlowSpeed else C.DriveSpeed + var baseSpeed = if (OI.speedLower) C.SLOW_SPEED else C.DRIVE_SPEED if (OI.reverseDrive) baseSpeed *= -1 //val avrgThrottle = (OI.leftThrottle + OI.rightThrottle)/2 @@ -70,20 +70,18 @@ object TeleOp : Command() { leftSpeed = leftSpeed.clamp(-1.0,1.0) rightSpeed = rightSpeed.clamp(-1.0,1.0)*/ - - - if(!OI.reverseDrive) Drivetrain.voltageDrive(leftSpeed * C.MaxVoltage, rightSpeed * C.MaxVoltage) - else Drivetrain.voltageDrive(rightSpeed * C.MaxVoltage, leftSpeed * C.MaxVoltage) + if(!OI.reverseDrive) Drivetrain.voltageDrive(leftSpeed * C.MAX_VOLTAGE, rightSpeed * C.MAX_VOLTAGE) + else Drivetrain.voltageDrive(rightSpeed * C.MAX_VOLTAGE, leftSpeed * C.MAX_VOLTAGE) } private fun handleIntake() = when { - OI.feedToShoot -> Intake.runIntake(Constants.IntakeConstants.feedingSpeed) + OI.feedToShoot -> Intake.runIntake(Intake.FEEDING_SPEED) OI.intakeThrottle < 0.0 -> Intake.runIntake(OI.intakeThrottle.clamp( - -Constants.IntakeConstants.reverseSpeed, - Constants.IntakeConstants.pickupSpeed)) + -Intake.REVERSE_SPEED, + Intake.PICKUP_SPEED)) OI.intakeThrottle > 0.0 -> { val intakeSpeed = OI.intakeThrottle.clamp( - -Constants.IntakeConstants.reverseSpeed, - Constants.IntakeConstants.pickupSpeed) + -Intake.REVERSE_SPEED, + Intake.PICKUP_SPEED) Intake.runIntake(intakeSpeed/2, intakeSpeed) } else -> Intake.stop() @@ -95,18 +93,18 @@ object TeleOp : Command() { } OI.shooterToSpeaker -> { if (Shooter.isAtSpeed && Shooter.targetSpeed.leftSpeeds != 0.RotationsPerSecond) Rumble.set(0.1,0.3, GenericHID.RumbleType.kRightRumble) - Shooter.runClosedLoop(Constants.ShooterConstants.AmpSpeed) + Shooter.runClosedLoop(Shooter.SPEAKER_SPEED) }//Shooter.runClosedLoop(Shooter.leftTestAmpSpeed,Shooter.rightTestAmpSpeed) OI.shooterToAmp -> { if (Shooter.isAtSpeed && Shooter.targetSpeed.leftSpeeds != 0.RotationsPerSecond) Rumble.set(0.1,0.3, GenericHID.RumbleType.kRightRumble) - Shooter.runClosedLoop(Constants.ShooterConstants.SpeakerSpeed) + Shooter.runClosedLoop(Shooter.AMP_SPEED) } else -> Shooter.stop() } private fun handleClimb() = when(OI.climb) { - OI.DirectionalPOV.UP -> Climber.climb(Constants.ClimbConstants.ClimbPos.Extend) - OI.DirectionalPOV.DOWN -> Climber.climb(Constants.ClimbConstants.ClimbPos.Retract) + OI.DirectionalPOV.UP -> Climber.climb(Safety.ClimbConstants.ClimbPos.Extend) + OI.DirectionalPOV.DOWN -> Climber.climb(Safety.ClimbConstants.ClimbPos.Retract) else -> Climber.stop() } diff --git a/src/main/kotlin/frc/robot/Constants.kt b/src/main/kotlin/frc/robot/Constants.kt deleted file mode 100644 index 09b04e3..0000000 --- a/src/main/kotlin/frc/robot/Constants.kt +++ /dev/null @@ -1,131 +0,0 @@ -package frc.robot - -import edu.wpi.first.math.MatBuilder -import edu.wpi.first.math.Nat -import frc.engine.controls.PIDConstants -import frc.engine.controls.SimpleMotorFeedForwardConstants - -import frc.engine.utils.* - -/* Constants for tuning the robot code. - -This should NOT include hardware constants (ie CAN bus IDs, wheel diameter and gear ratios); -things that are related to physical objects on the robot and are meant to be set once should -be located in RobotHardware.kt instead. - -*/ - -object Constants { - object DriveConstants { - // Yoinked from 2898 charged up code. - // Ramsete parameters, see [https://file.tavsys.net/control/controls-engineering-in-frc.pdf] page 81 - // **DO NOT CHANGE THESE PARAMETERS** - const val DRIVETRAIN_RAMSETE_B = 5.0 // Higher values make it more aggressively stick to the trajectory. 0 < B - const val DRIVETRAIN_RAMSETE_Z = 0.7 // Higher values give it more dampening. 0 < Z < 1 - - const val LeftMotorMainID = 21 - const val LeftMotorSubID = 22 - const val RightMotorMainID = 23 - const val RightMotorSubID = 24 - - val MaxVelocity = `M/s`(2.0) - val MaxAcceleration = MetersPerSecondSquared(0.5) - - const val MotorRevsPerWheelRev = (50/14) * (48/16) //~10:1 As taken from CAD- should verify after drivetrain is built in the real world - val WheelDiameter = Inches(6.0).meterValue() - val TrackWidth = Meters(0.0) //TODO Get track width - - val FF_CONSTANTS = SimpleMotorFeedForwardConstants(0.0, 0.0, 0.0) - val PID_CONSTANTS = PIDConstants(1.0,0.0,0.0) - - - - const val CurrentLimit = 30 //amps, per motor controller - /* See - https://the-charge.com/uploads/3/5/3/0/35304458/testing_and_analysis_of_first_robotics_batteries__2018_.pdf - For info on battery characteristics - */ - } - object IntakeConstants { - const val TopMotorID = 28 - const val BottomMotorID = 27 - - const val CurrentLimit = 20 //Current limit 12 amps? - const val limitSwitchChannel = 1 - - const val pickupSpeed = 0.7 //TODO set intake speed. - const val pushforwardSpeed = 0.3 //TODO set pullback speed. Multiplied by -1 - const val reverseSpeed = 0.5 //TODO set outtake Speed. - const val feedingSpeed = 0.7 //TODO set feeding speed. - - const val feedingTime = 1.0 //TODO set feedingTime. In seconds - const val unfeedTime = 0.5 //TODO set feedingTime. In seconds - - - } - object ShooterConstants { - const val LeftFlywheelMotorID = 25 - const val RightFlywheelMotorID = 26 - - val LEFT_FF_CONSTANTS = SimpleMotorFeedForwardConstants(0.19218, 0.12701, 0.02261) - val LEFT_PID_CONSTANTS = PIDConstants(0.0019781,0.0,0.0) - - val RIGHT_FF_CONSTANTS = SimpleMotorFeedForwardConstants(0.26415, 0.13172, 0.028407) - val RIGHT_PID_CONSTANTS = PIDConstants(0.0073073,0.0,0.0) - - - - const val CurrentLimit = 20 //amps, per motor controller - val SpeakerPoly = Polynomial() //TODO Desmos this - - val SpeakerSpeed = 5000.RPM //Todo - val AmpSpeed = 0.RPM //Todo - - } - object OdometryConstants { - @Suppress("removal") - val VisionDeviation = MatBuilder(Nat.N3(),Nat.N1()).fill(3.0,3.0,1000.0) - } - - object TeleopConstants { - const val MaxIntakeSpeed = 0.5 - const val MaxVoltage = 12.0 - const val DriveSpeed = 1.0 //TODO set drive speed - const val SlowSpeed = 0.3 //TODO set speed boost speed - const val QuickTurnDeadzone = 0.1 //TODO set quick turn dead zone - const val QuickTurnSpeed = 0.7 //TODO set quick turn speed. - - //const val MaxSpeed = 5.0 //M/s - //TODO: Maybe change to feet per second? Metric is Good, but many teams communicate drivetrain speed in feet per second so for communicating quickly it could be worth leaving in fps. - // Possible implementation: - // FeetPerSecond(16.5).metersPerSecondValue() - // (also import utils/Units.kt) - } - object ClimbConstants { - - enum class ClimbPos { - Extend, - Retract, - Chill - } - - const val RightMotorID = 29 - const val LeftMotorID = 30 - - - - const val CurrentLimit = 30 //amps, per side. See drivetrain current limit. - - const val MotorRevsToRetract = (7/1) * (7/1) * 10.0 //TODO: DANGER! Rough estimate; based off of CAD but makes assumptions about winch cord stacking. - //How many revolutions of the motor does it take to fully contract the lifter? - - const val ExtendVoltage = 10.0 //TODO: Tune! - const val RetractVoltage = 10.0 //TODO: Tune! - - //Note: May require profiling to prevent slamming down on chain- test! - - //NOTE: Feedforwards control would normally be required in order to fight gravity, but the climber is actually spring loaded to be in the extended position - } - - -} \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/Safety.kt b/src/main/kotlin/frc/robot/Safety.kt new file mode 100644 index 0000000..b0157cb --- /dev/null +++ b/src/main/kotlin/frc/robot/Safety.kt @@ -0,0 +1,64 @@ +package frc.robot + +import frc.engine.utils.* + +/* Important Constants related to Robot Safety (Do not change these values unless you know what you are doing) + +All other constants should be located nearby to where they are used (ie in subsystem file) + +*/ + +object Safety { + object DriveConstants { + val MaxVelocity = `M/s`(2.0) + val MaxAcceleration = MetersPerSecondSquared(0.5) + + const val CURRENT_LIMIT = 30 //amps, per motor controller on the drivetrain + // See https://the-charge.com/uploads/3/5/3/0/35304458/testing_and_analysis_of_first_robotics_batteries__2018_.pdf + // for more info on battery characteristics + } + object IntakeConstants { + const val CURRENT_LIMIT = 20 //Current limit 12 amps? + } + object ShooterConstants { + const val CURRENT_LIMIT = 20 //amps, per motor controller + } + object TeleopConstants { + const val MAX_VOLTAGE = 12.0 + const val DRIVE_SPEED = 1.0 //TODO set drive speed + const val SLOW_SPEED = 0.3 //TODO set speed boost speed + + //const val MaxSpeed = 5.0 //M/s + //TODO: Maybe change to feet per second? Metric is Good, but many teams communicate drivetrain speed in feet per second so for communicating quickly it could be worth leaving in fps. + // Possible implementation: + // FeetPerSecond(16.5).metersPerSecondValue() + // (also import utils/Units.kt) + } + object ClimbConstants { + + enum class ClimbPos { + Extend, + Retract, + Chill + } + + const val RightMotorID = 29 + const val LeftMotorID = 30 + + + + const val CurrentLimit = 30 //amps, per side. See drivetrain current limit. + + const val MotorRevsToRetract = (7/1) * (7/1) * 10.0 //TODO: DANGER! Rough estimate; based off of CAD but makes assumptions about winch cord stacking. + //How many revolutions of the motor does it take to fully contract the lifter? + + const val ExtendVoltage = 10.0 //TODO: Tune! + const val RetractVoltage = 10.0 //TODO: Tune! + + //Note: May require profiling to prevent slamming down on chain- test! + + //NOTE: Feedforwards control would normally be required in order to fight gravity, but the climber is actually spring loaded to be in the extended position + } + + +} \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt index f48a538..8cf3062 100644 --- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt +++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt @@ -3,7 +3,6 @@ package frc.robot.subsystems import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax import com.revrobotics.RelativeEncoder -import edu.wpi.first.math.controller.SimpleMotorFeedforward import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.DifferentialDriveKinematics import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds @@ -18,45 +17,54 @@ import frc.engine.controls.* import frc.engine.utils.`M/s` import frc.engine.utils.initMotorControllers import frc.engine.utils.* -import frc.robot.Constants.DriveConstants +import frc.robot.Safety.DriveConstants //import frc.robot.subsystems.Odometry.chassisSpeeds object Drivetrain : SubsystemBase() { - private val leftMain = CANSparkMax(DriveConstants.LeftMotorMainID, CANSparkLowLevel.MotorType.kBrushless) - private val leftSecondary = CANSparkMax(DriveConstants.LeftMotorSubID, CANSparkLowLevel.MotorType.kBrushless) - private val rightMain = CANSparkMax(DriveConstants.RightMotorMainID, CANSparkLowLevel.MotorType.kBrushless) - private val rightSecondary = CANSparkMax(DriveConstants.RightMotorSubID, CANSparkLowLevel.MotorType.kBrushless) + + private val leftMain = CANSparkMax(21, CANSparkLowLevel.MotorType.kBrushless) + private val leftSecondary = CANSparkMax(22, CANSparkLowLevel.MotorType.kBrushless) + private val rightMain = CANSparkMax(23, CANSparkLowLevel.MotorType.kBrushless) + private val rightSecondary = CANSparkMax(24, CANSparkLowLevel.MotorType.kBrushless) val leftEncoder: RelativeEncoder = leftMain.encoder val rightEncoder: RelativeEncoder = rightMain.encoder private val drive = DifferentialDrive(leftMain, rightMain) - private val leftPid = DriveConstants.PID_CONSTANTS.toPID() - private val rightPid = DriveConstants.PID_CONSTANTS.toPID() - private val leftFeedForward = DriveConstants.FF_CONSTANTS.toFeedForward() - private val rightFeedForward = DriveConstants.FF_CONSTANTS.toFeedForward() + private val FF_CONSTANTS = SimpleMotorFeedForwardConstants(0.0, 0.0, 0.0) + private val PID_CONSTANTS = PIDConstants(1.0,0.0,0.0) + + private val leftPid = PID_CONSTANTS.toPID() + private val rightPid = PID_CONSTANTS.toPID() + private val leftFeedForward = FF_CONSTANTS.toFeedForward() + private val rightFeedForward = FF_CONSTANTS.toFeedForward() val trajectoryMaker = TrajectoryMaker(DriveConstants.MaxVelocity, DriveConstants.MaxAcceleration) var trajectory : Trajectory? = null private var trajectoryStartTime = 0.seconds + // Yoinked from 2898 charged up code. + + val TrackWidth = Meters(0.0) //TODO Get track width private val ramsete: Ramsete = Ramsete( - DriveConstants.TrackWidth.toMeters(), + TrackWidth.toMeters(), Odometry, leftPid, rightPid, leftFeedForward, rightFeedForward, - DriveConstants.DRIVETRAIN_RAMSETE_B, - DriveConstants.DRIVETRAIN_RAMSETE_Z + // Ramsete parameters, see [https://file.tavsys.net/control/controls-engineering-in-frc.pdf] page 81 + // **DO NOT CHANGE B or Z ** + 5.0, // Ramsete B, Higher values make it more aggressively stick to the trajectory. 0 < B + 0.7, // Ramsete Z, Higher values give it more dampening. 0 < Z < 1 ) init { // Reset motor controllers & set current limits - initMotorControllers(DriveConstants.CurrentLimit, leftMain, rightMain, leftSecondary, rightSecondary) + initMotorControllers(DriveConstants.CURRENT_LIMIT, leftMain, rightMain, leftSecondary, rightSecondary) // Set secondary motors to follow the primary ones leftSecondary.follow(leftMain) @@ -156,7 +164,7 @@ object Drivetrain : SubsystemBase() { * @param speeds Desired ChassisSpeeds */ fun closedLoopDrive(speeds: ChassisSpeeds){ //Todo: speeds is passed directly from odometry - val kinematics = DifferentialDriveKinematics(DriveConstants.TrackWidth.value) + val kinematics = DifferentialDriveKinematics(TrackWidth.value) val wheelSpeeds: DifferentialDriveWheelSpeeds = kinematics.toWheelSpeeds(speeds) closedLoopDrive(wheelSpeeds.leftMetersPerSecond,wheelSpeeds.rightMetersPerSecond) } diff --git a/src/main/kotlin/frc/robot/subsystems/Intake.kt b/src/main/kotlin/frc/robot/subsystems/Intake.kt index 1042d93..46cb5c6 100644 --- a/src/main/kotlin/frc/robot/subsystems/Intake.kt +++ b/src/main/kotlin/frc/robot/subsystems/Intake.kt @@ -7,21 +7,20 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.Timer import frc.engine.utils.initMotorControllers -import frc.robot.Constants.IntakeConstants.unfeedTime -import frc.robot.Constants.IntakeConstants as C +import frc.robot.Safety.IntakeConstants as C object Intake : SubsystemBase() { val limitSwitch = DigitalInput(C.limitSwitchChannel) - private val topMotor = TalonSRX(C.TopMotorID) - private val bottomMotor = TalonSRX(C.BottomMotorID) private val unFeedTimer = Timer() + private val topMotor = TalonSRX(28) //CAN IDs + private val bottomMotor = TalonSRX(27) init { // Reset motor controllers & set current limits - initMotorControllers(C.CurrentLimit, topMotor, bottomMotor) + initMotorControllers(C.CURRENT_LIMIT, topMotor, bottomMotor) // Sets the bottom motor to follow the top (as they should never be running seperatly) //bottomMotor.follow(topMotor) @@ -52,5 +51,17 @@ object Intake : SubsystemBase() { bottomMotor.set(ControlMode.PercentOutput, 0.0) } + const val PICKUP_SPEED = 0.7 + const val PUSHFORWOARD_SPEED = 0.3 + const val REVERSE_SPEED = 0.5 + const val FEEDING_SPEED = 0.7 + /* Old intake time constants from auto-intake system + const val feedingTime = 1.0 //TODO set feedingTime. In seconds + const val unfeedTime = 0.5 //TODO set feedingTime. In seconds + + val limitSwitch = DigitalInput(1) //LIMIT SWITCH CHANNEL ON ROBORIO DIO + + private val unFeedTimer = Timer() + */ } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/Odometry.kt b/src/main/kotlin/frc/robot/subsystems/Odometry.kt index d582201..fb0d37e 100644 --- a/src/main/kotlin/frc/robot/subsystems/Odometry.kt +++ b/src/main/kotlin/frc/robot/subsystems/Odometry.kt @@ -18,15 +18,16 @@ import edu.wpi.first.util.sendable.SendableRegistry import edu.wpi.first.wpilibj.smartdashboard.Field2d import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.robot.subsystems.Drivetrain import frc.engine.odometry.Vision import frc.engine.odometry.aprilTagFieldLayout import frc.engine.utils.m -import frc.robot.Constants -import frc.robot.Constants.DriveConstants as D +import frc.robot.Safety +import frc.robot.subsystems.Drivetrain as D object Odometry : SubsystemBase(), PoseProvider { + @Suppress("removal") + val VisionDeviation = MatBuilder(Nat.N3(), Nat.N1()).fill(3.0,3.0,1000.0) var navx = NAVX() private val vision = Vision("Wyatt", aprilTagFieldLayout) @@ -34,8 +35,8 @@ object Odometry : SubsystemBase(), PoseProvider { private val visionProvider = DifferentialDrivePoseEstimator(DifferentialDriveKinematics(D.TrackWidth.meterValue()), navx.rotation2d, 0.0, 0.0, Pose2d()) private val encoderOnly = DifferentialDrivePoseEstimator(DifferentialDriveKinematics(D.TrackWidth.meterValue()), navx.rotation2d, 0.0, 0.0, Pose2d()) - val leftVel get() = MetersPerSecond(Drivetrain.leftEncoder.velocity) - val rightVel get() = MetersPerSecond(Drivetrain.rightEncoder.velocity) + val leftVel get() = MetersPerSecond(D.leftEncoder.velocity) + val rightVel get() = MetersPerSecond(D.rightEncoder.velocity) val vels get() = DifferentialDriveWheelSpeeds(leftVel.metersPerSecondValue(), rightVel.metersPerSecondValue()) val chassisSpeeds get() = ChassisSpeeds(leftVel.metersPerSecondValue(), rightVel.metersPerSecondValue(), navx.rate) val getChassesSpeeds: () -> ChassisSpeeds = { chassisSpeeds } @@ -50,8 +51,8 @@ object Odometry : SubsystemBase(), PoseProvider { override fun reset(x: Meters, y: Meters, theta: Degrees) { val p = Pose2d(x.value, y.value, Rotation2d.fromDegrees(theta.value)) - visionProvider.resetPosition(navx.rotation2d, Drivetrain.leftEncoder.position, Drivetrain.rightEncoder.position, p) - encoderOnly.resetPosition(navx.rotation2d, Drivetrain.leftEncoder.position, Drivetrain.rightEncoder.position, p) + visionProvider.resetPosition(navx.rotation2d, D.leftEncoder.position, D.rightEncoder.position, p) + encoderOnly.resetPosition(navx.rotation2d, D.leftEncoder.position, D.rightEncoder.position, p) } /*override fun periodic() { @@ -61,12 +62,12 @@ object Odometry : SubsystemBase(), PoseProvider { override fun periodic() { val visionMeasurements = vision.getEstimatedPose(pose) if(visionMeasurements != null){ - visionProvider.setVisionMeasurementStdDevs(Constants.OdometryConstants.VisionDeviation) + visionProvider.setVisionMeasurementStdDevs(VisionDeviation) visionProvider.addVisionMeasurement(visionMeasurements.estimatedPose.toPose2d(), visionMeasurements.timestampSeconds) } - pose = visionProvider.update(navx.rotation2d, Drivetrain.leftEncoder.position, Drivetrain.rightEncoder.position) - encoderOnly.update(navx.rotation2d, Drivetrain.leftEncoder.position, Drivetrain.rightEncoder.position) + pose = visionProvider.update(navx.rotation2d, D.leftEncoder.position, D.rightEncoder.position) + encoderOnly.update(navx.rotation2d, D.leftEncoder.position, D.rightEncoder.position) field.robotPose = pose field.getObject("pure odometry").pose = encoderOnly.estimatedPosition diff --git a/src/main/kotlin/frc/robot/subsystems/Shooter.kt b/src/main/kotlin/frc/robot/subsystems/Shooter.kt index b562ac8..8ccacfa 100644 --- a/src/main/kotlin/frc/robot/subsystems/Shooter.kt +++ b/src/main/kotlin/frc/robot/subsystems/Shooter.kt @@ -4,7 +4,6 @@ import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax import com.revrobotics.RelativeEncoder -import edu.wpi.first.math.controller.SimpleMotorFeedforward import edu.wpi.first.units.* import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard @@ -12,6 +11,8 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.engine.controls.Controller +import frc.engine.controls.PIDConstants +import frc.engine.controls.SimpleMotorFeedForwardConstants import frc.engine.controls.toFeedForward import frc.engine.controls.toPID import frc.engine.utils.RPM @@ -19,31 +20,34 @@ import frc.engine.utils.Rotations import frc.engine.utils.RotationsPerSecond import frc.engine.utils.Sugar.within import frc.engine.utils.initMotorControllers -import frc.robot.Constants.ShooterConstants as C +import frc.robot.Safety.ShooterConstants as C object Shooter : SubsystemBase() { - private val leftFlywheel = CANSparkMax(C.LeftFlywheelMotorID, CANSparkLowLevel.MotorType.kBrushless) - private val rightFlywheel = CANSparkMax(C.RightFlywheelMotorID, CANSparkLowLevel.MotorType.kBrushless) + + private val leftFlywheel = CANSparkMax(25, CANSparkLowLevel.MotorType.kBrushless) //CAN ID! + private val rightFlywheel = CANSparkMax(26, CANSparkLowLevel.MotorType.kBrushless) private val leftEncoder: RelativeEncoder = leftFlywheel.encoder private val rightEncoder: RelativeEncoder = rightFlywheel.encoder - private val leftPid = C.LEFT_PID_CONSTANTS.toPID() - private val rightPid = C.RIGHT_PID_CONSTANTS.toPID() - private val leftFeedForward = C.LEFT_FF_CONSTANTS.toFeedForward() - private val rightFeedForward = C.RIGHT_FF_CONSTANTS.toFeedForward() + private val leftPid = PIDConstants(0.0019781,0.0,0.0).toPID() + private val rightPid = PIDConstants(0.0073073,0.0,0.0).toPID() + private val leftFeedForward = SimpleMotorFeedForwardConstants(0.19218, 0.12701, 0.02261).toFeedForward() + private val rightFeedForward = SimpleMotorFeedForwardConstants(0.26415, 0.13172, 0.028407).toFeedForward() var targetSpeed = ShooterSpeeds() + val SPEAKER_SPEED = 5000.RPM //Todo + val AMP_SPEED = 0.RPM //Todo var leftTestAmpSpeed = 0.RPM var rightTestAmpSpeed = 0.RPM init { // Reset motor controllers & set current limits - initMotorControllers(C.CurrentLimit, leftFlywheel, rightFlywheel) + initMotorControllers(C.CURRENT_LIMIT, leftFlywheel, rightFlywheel) // Invert the left flywheel leftFlywheel.inverted = true