constants and controller weirdness

build.gradle

@@ -51,7 +51,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 wpi.java.debugJni = false
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.

simgui.json

@@ -1,8 +1,35 @@
 {
+  "HALProvider": {
+    "PWM Outputs": {
+      "window": {
+        "visible": true
+      }
+    },
+    "RoboRIO": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/SendableChooser[0]": "String Chooser"
     }
+  },
+  "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "Retained Values": {
+      "open": false
+    },
+    "Transitory Values": {
+      "open": false
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
   }
 }

src/main/java/frc/robot/Robot.java

@@ -32,7 +32,7 @@ public void autonomousInit() {
     }
 
     public void teleopInit() {
-        auto.cancel();
+        if (auto != null) auto.cancel();
         container.led.setRGB(255, 255, 255);
     }
 

src/main/java/frc/robot/RobotContainer.java

@@ -2,8 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.VideoMode;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,18 +20,13 @@
 import frc.robot.commands.ShintakeCommands.Outtake;
 import frc.robot.commands.ShintakeCommands.SetShooter;
 import frc.robot.commands.ShintakeCommands.Shoot;
-import frc.robot.commands.VisionCommands.AngleShooter;
-import frc.robot.commands.VisionCommands.TagAlign;
 import frc.robot.constants.ShintakeConstants;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Led;
 import frc.robot.subsystems.Shintake;
 import frc.robotkt.constants.IOConstants;
-import frc.robotkt.constants.VisionConstants;
 import frc.robotkt.subsystems.Arm;
-import frc.robotkt.subsystems.Camera;
 import frc.robotkt.subsystems.Drivetrain;
-import org.photonvision.PhotonCamera;
 
 public class RobotContainer {
     // Controllers
@@ -48,8 +41,8 @@ public class RobotContainer {
     final Arm arm = new Arm();
 
     // Cameras
-    final Camera camera = new Camera(new PhotonCamera(VisionConstants.kCameraName), drivetrain);
-    final PhotonCamera drivercam = new PhotonCamera(VisionConstants.kDriverCameraName);
+    // final Camera camera = new Camera(new PhotonCamera(VisionConstants.kCameraName), drivetrain);
+    // final PhotonCamera drivercam = new PhotonCamera(VisionConstants.kDriverCameraName);
 
     // Auto
     final SendableChooser<Command> autoChooser = AutoBuilder.buildAutoChooser();
@@ -60,7 +53,7 @@ public RobotContainer() {
         NamedCommands.registerCommand("SetShooter", new SetShooter(shintake, driverctl));
         NamedCommands.registerCommand("Shoot", new Shoot(shintake));
         NamedCommands.registerCommand("Intake", new Intake(shintake, led));
-        NamedCommands.registerCommand("Angle", new AngleShooter(drivetrain, arm, camera, led, driverctl));
+        // NamedCommands.registerCommand("Angle", new AngleShooter(drivetrain, arm, camera, led, driverctl));
         NamedCommands.registerCommand("ResetArm", new ResetArm(arm));
 
         // Auto Chooser
@@ -71,16 +64,16 @@ public RobotContainer() {
 
         // Default commands
         drivetrain.setDefaultCommand(new RunCommand(() -> drivetrain.drive(driverctl), drivetrain));
-        camera.setDefaultCommand(new RunCommand(camera::reset, camera));
+        // camera.setDefaultCommand(new RunCommand(camera::reset, camera));
 
         // Shuffleboard/Driver camera
-        drivercam.setDriverMode(true);
-        CameraServer.startAutomaticCapture().setVideoMode(new VideoMode(1, 320, 240, 60));
+        // drivercam.setDriverMode(true);
+        // CameraServer.startAutomaticCapture().setVideoMode(new VideoMode(1, 320, 240, 60));
     }
 
     private void bindings() {
         // Tag stuff
-        driverctl.b().whileTrue(new TagAlign(drivetrain, camera, driverctl));
+        // driverctl.b().whileTrue(new TagAlign(drivetrain, camera, driverctl));
 
         // Emergency
         driverctl.rightTrigger().whileTrue(new RunCommand(drivetrain::lock, drivetrain));
@@ -105,8 +98,8 @@ private void bindings() {
         driverctl.leftBumper().onTrue(new Shoot(shintake));
 
         // Arm
-        operctl.axisGreaterThan(1, 0.4).whileTrue(new ManualAngle(arm, true));
-        operctl.axisLessThan(1, -0.4).whileTrue(new ManualAngle(arm, false));
+        operctl.axisLessThan(1, -0.4).whileTrue(new ManualAngle(arm, true));
+        operctl.axisGreaterThan(1, 0.4).whileTrue(new ManualAngle(arm, false));
         operctl.rightBumper().whileTrue(new ManualExtend(arm, true));
         operctl.leftBumper().whileTrue(new ManualExtend(arm, false));
 

src/main/kotlin/frc/robotkt/constants/DriveConstants.kt

@@ -32,7 +32,7 @@ object DriveConstants {
         Translation2d(-Dimensions.kWheelBase / 2, -Dimensions.kTrackWidth / 2)
     )
 
-    const val kMaxSpeedMps = 1.0
+    const val kMaxSpeedMps = 15.0
     const val kMaxSpeedAnglular = kTau
     const val kGyroFactor = -1.0
 }

src/main/kotlin/frc/robotkt/constants/IOConstants.kt

@@ -17,7 +17,6 @@ object IOConstants {
         const val kRightMotor = 31
         const val kLeftMotor = 32
         const val kExtMotor = 33
-        const val kPotChannel = 0
     }
 
     object Led {
@@ -40,7 +39,7 @@ object IOConstants {
     object Climber {
         const val kLeft = 41
         const val kRight = 42
-        
+
         const val kLeftLock = 3
         const val kRightLock = 4
     }

src/main/kotlin/frc/robotkt/constants/ModuleConstants.kt

@@ -22,13 +22,6 @@ object ModuleConstants {
         const val kBevelPinionTeeth = 15.0
         const val kReduction = (kBevelGearTeeth * kSpurTeeth) / (kPinionTeeth * kBevelPinionTeeth)
         const val kCurrentLimit = 40
-
-        const val kP = 0.15
-        const val kI = 0.0
-        const val kD = 0.01
-        const val kFF = 1 / Wheel.kFreeSpeed
-        const val kPidMin = -1.0
-        const val kPidMax = 1.0
     }
 
     object DriveEncoder {
@@ -38,13 +31,6 @@ object ModuleConstants {
 
     object TurnMotor {
         const val kCurrentLimit = 40
-
-        const val kP = 0.3
-        const val kI = 0.0
-        const val kD = 0.02
-        const val kFF = 0.0
-        const val kPidMin = -1.0
-        const val kPidMax = 1.0
     }
 
     object TurnEncoder {

src/main/kotlin/frc/robotkt/constants/PidConstants.kt

@@ -38,12 +38,12 @@ object PidConstants {
     }
 
     object Arm {
-        const val kPivP = 0.2
+        const val kPivP = 0.001
         const val kPivI = 0.0
         const val kPivD = 0.0
         const val kPivFF = 0.0
 
-        const val kExtP = 0.7
+        const val kExtP = 0.3
         const val kExtI = 0.0
         const val kExtD = 0.0
         const val kExtFF = 0.0

src/main/kotlin/frc/robotkt/swerve/SwerveModule.kt

@@ -10,16 +10,17 @@ import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.kinematics.SwerveModulePosition
 import edu.wpi.first.math.kinematics.SwerveModuleState
 import frc.robotkt.constants.ModuleConstants
+import frc.robotkt.constants.PidConstants
 
-class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) {
-    val drive = CANSparkMax(driveID, MotorType.kBrushless)
-    val turn = CANSparkMax(turnID, MotorType.kBrushless)
+class SwerveModule(driveId: Int, turnId: Int, val angularOffset: Double) {
+    val driveMotor = CANSparkMax(driveId, MotorType.kBrushless)
+    val turnMotor = CANSparkMax(turnId, MotorType.kBrushless)
 
     val driveEncoder: RelativeEncoder
     val turnEncoder: SparkAbsoluteEncoder
 
-    val drivePID: SparkPIDController
-    val turnPID: SparkPIDController
+    val drivePid: SparkPIDController
+    val turnPid: SparkPIDController
 
     var desiredState = SwerveModuleState(0.0, Rotation2d())
         set(desired) {
@@ -28,25 +29,25 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) {
             corrected.angle = desired.angle.plus(Rotation2d(angularOffset))
 
             val optimized = SwerveModuleState.optimize(corrected, Rotation2d(turnEncoder.position))!!
-            drivePID.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity)
-            turnPID.setReference(optimized.angle.radians, ControlType.kPosition)
+            drivePid.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity)
+            turnPid.setReference(optimized.angle.radians, ControlType.kPosition)
 
             field = desired
         }
 
     init {
-        drive.restoreFactoryDefaults()
-        turn.restoreFactoryDefaults()
+        driveMotor.restoreFactoryDefaults()
+        turnMotor.restoreFactoryDefaults()
 
         // Setup encoders and PID controllers
-        driveEncoder = drive.encoder
-        turnEncoder = turn.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)
+        driveEncoder = driveMotor.encoder
+        turnEncoder = turnMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)
 
-        drivePID = drive.getPIDController()
-        turnPID = turn.getPIDController()
+        drivePid = driveMotor.getPIDController()
+        turnPid = turnMotor.getPIDController()
 
-        drivePID.setFeedbackDevice(driveEncoder)
-        turnPID.setFeedbackDevice(turnEncoder)
+        drivePid.setFeedbackDevice(driveEncoder)
+        turnPid.setFeedbackDevice(turnEncoder)
 
         // Apply position and velocity conversion factors
         driveEncoder.positionConversionFactor = ModuleConstants.DriveEncoder.kPositionFactor
@@ -58,32 +59,35 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) {
         turnEncoder.setInverted(ModuleConstants.TurnEncoder.kInverted)
 
         // Turn PID wrap
-        turnPID.setPositionPIDWrappingEnabled(true)
-        turnPID.setPositionPIDWrappingMinInput(ModuleConstants.TurnEncoder.kMinPidInput)
-        turnPID.setPositionPIDWrappingMaxInput(ModuleConstants.TurnEncoder.kMaxPidInput)
+        turnPid.setPositionPIDWrappingEnabled(true)
+        turnPid.setPositionPIDWrappingMinInput(ModuleConstants.TurnEncoder.kMinPidInput)
+        turnPid.setPositionPIDWrappingMaxInput(ModuleConstants.TurnEncoder.kMaxPidInput)
 
         // Apply PID constants
-        drivePID.setP(ModuleConstants.DriveMotor.kP)
-        drivePID.setI(ModuleConstants.DriveMotor.kI)
-        drivePID.setD(ModuleConstants.DriveMotor.kD)
-        drivePID.setFF(ModuleConstants.DriveMotor.kFF)
-        drivePID.setOutputRange(ModuleConstants.DriveMotor.kPidMin, ModuleConstants.DriveMotor.kPidMax)
-
-        turnPID.setP(ModuleConstants.TurnMotor.kP)
-        turnPID.setI(ModuleConstants.TurnMotor.kI)
-        turnPID.setD(ModuleConstants.TurnMotor.kD)
-        turnPID.setFF(ModuleConstants.TurnMotor.kFF)
-        turnPID.setOutputRange(ModuleConstants.TurnMotor.kPidMin, ModuleConstants.TurnMotor.kPidMax)
+        drivePid.p = PidConstants.DriveMotor.kP
+        drivePid.i = PidConstants.DriveMotor.kI
+        drivePid.d = PidConstants.DriveMotor.kD
+        drivePid.ff = PidConstants.DriveMotor.kFF
+        drivePid.setOutputRange(PidConstants.DriveMotor.kPidMin, PidConstants.DriveMotor.kPidMax)
+
+        turnPid.p = PidConstants.TurnMotor.kP
+        turnPid.i = PidConstants.TurnMotor.kI
+        turnPid.d = PidConstants.TurnMotor.kD
+        turnPid.ff = PidConstants.TurnMotor.kFF
+        turnPid.setOutputRange(PidConstants.TurnMotor.kPidMin, PidConstants.TurnMotor.kPidMax)
 
         // idle mode and smart current
-        drive.setIdleMode(ModuleConstants.kIdleMode)
-        turn.setIdleMode(ModuleConstants.kIdleMode)
-        drive.setSmartCurrentLimit(ModuleConstants.DriveMotor.kCurrentLimit)
-        turn.setSmartCurrentLimit(ModuleConstants.TurnMotor.kCurrentLimit)
+        driveMotor.setIdleMode(ModuleConstants.kIdleMode)
+        turnMotor.setIdleMode(ModuleConstants.kIdleMode)
+        driveMotor.setSmartCurrentLimit(ModuleConstants.DriveMotor.kCurrentLimit)
+        turnMotor.setSmartCurrentLimit(ModuleConstants.TurnMotor.kCurrentLimit)
+
+        // One module is f*ed up
+        if (driveId == 40) driveMotor.inverted = true
 
         // Maintain configs
-        drive.burnFlash()
-        turn.burnFlash()
+        driveMotor.burnFlash()
+        turnMotor.burnFlash()
 
         // Finish setup
         desiredState.angle = Rotation2d(turnEncoder.position)
@@ -97,8 +101,8 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) {
         get() = SwerveModulePosition(driveEncoder.position, Rotation2d(turnEncoder.position - angularOffset))
 
     fun stop() {
-        drive.stopMotor()
-        turn.stopMotor()
+        driveMotor.stopMotor()
+        turnMotor.stopMotor()
     }
 
     fun resetEncoders() {