Decrease code fragmentation, make constants follow naming convention

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<N3,N1 > {
-        return Constants.OdometryConstants.VisionDeviation
+        return Safety.OdometryConstants.VisionDeviation
     }
 }

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()
     }

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()

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)

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)

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,

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()

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()
     }
 

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
+    }
+
+
+}