Merge pull request #1 from frc937/swerve-base

Ported swerve code from swerve box repo

src/main/deploy/swerve/controllerproperties.json

@@ -0,0 +1,8 @@
+{
+  "angleJoystickRadiusDeadband": 0.5,
+  "heading": {
+    "p": 0.4,
+    "i": 0,
+    "d": 0.01
+  }
+}

src/main/deploy/swerve/modules/backleft.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": -12,
+    "left": 9
+  },
+  "absoluteEncoderOffset": 211.0,
+  "drive": {
+    "type": "sparkmax",
+    "id": 5,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 6,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "thrifty",
+    "id": 2,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  }
+}

src/main/deploy/swerve/modules/backright.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": -12,
+    "left": -9
+  },
+  "absoluteEncoderOffset": 31.9,
+  "drive": {
+    "type": "sparkmax",
+    "id": 7,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 8,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "thrifty",
+    "id": 3,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  }
+}

src/main/deploy/swerve/modules/frontleft.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": 12,
+    "left": 9
+  },
+  "absoluteEncoderOffset": 140.6,
+  "drive": {
+    "type": "sparkmax",
+    "id": 3,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 4,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "thrifty",
+    "id": 0,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  }
+}

src/main/deploy/swerve/modules/frontright.json

@@ -0,0 +1,30 @@
+{
+  "location": {
+    "front": 12,
+    "left": -9
+  },
+  "absoluteEncoderOffset": 5.5,
+  "drive": {
+    "type": "sparkmax",
+    "id": 1,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 2,
+    "canbus": null
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "encoder": {
+    "type": "thrifty",
+    "id": 1,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  }
+}

src/main/deploy/swerve/modules/physicalproperties.json

@@ -0,0 +1,16 @@
+{
+  "optimalVoltage": 12,
+  "wheelGripCoefficientOfFriction": 1.19,
+  "currentLimit": {
+    "drive": 40,
+    "angle": 20
+  },
+  "conversionFactor": {
+    "angle": 0.006866455,
+    "drive": 0.000011545
+  },
+  "rampRate": {
+    "drive": 0.25,
+    "angle": 0.25
+  }
+}

src/main/deploy/swerve/modules/pidfproperties.json

@@ -0,0 +1,16 @@
+{
+  "drive": {
+    "p": 0.0020645,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  },
+  "angle": {
+    "p": 0.0020645,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  }
+}

src/main/deploy/swerve/swervedrive.json

@@ -0,0 +1,14 @@
+{
+  "imu": {
+    "type": "navx",
+    "id": 0,
+    "canbus": null
+  },
+  "invertedIMU": true,
+  "modules": [
+    "frontleft.json",
+    "frontright.json",
+    "backleft.json",
+    "backright.json"
+  ]
+}

src/main/java/frc/robot/Constants.java

@@ -0,0 +1,25 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+package frc.robot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static class OperatorConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

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

@@ -10,17 +10,94 @@
  */
 package frc.robot;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.DriveFieldOriented;
+import frc.robot.commands.DriveRobotOriented;
+import frc.robot.commands.EnterXMode;
+import frc.robot.subsystems.Drive;
 
 public class RobotContainer {
+
+  /*
+   * **************
+   * * SUBSYSTEMS *
+   * **************
+   */
+  private final Drive drive = new Drive();
+
+  /*
+   * ************
+   * * COMMANDS *
+   * ************
+   */
+
+  private final DriveRobotOriented driveRobotOriented = new DriveRobotOriented(drive);
+  private final DriveFieldOriented driveFieldOriented = new DriveFieldOriented(drive);
+  private final EnterXMode enterXMode = new EnterXMode(drive);
+
+  /*
+   * *****************
+   * * OTHER OBJECTS *
+   * *****************
+   */
+
+  public static CommandXboxController m_driverController =
+      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+
   public RobotContainer() {
     configureBindings();
+
+    drive.setDefaultCommand(driveRobotOriented);
   }
 
-  private void configureBindings() {}
+  private void configureBindings() {
+    m_driverController.leftStick().toggleOnTrue(driveFieldOriented);
+
+    m_driverController.x().onTrue(enterXMode);
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
+
+  public static double scaleAxis(double axis) {
+    double deadbanded = MathUtil.applyDeadband(axis, 0.1);
+    return Math.pow(deadbanded, 3);
+  }
+
+  public static double getControllerLeftXAxis() {
+    return m_driverController.getLeftX();
+  }
+
+  public static double getScaledControllerLeftXAxis() {
+    return scaleAxis(getControllerLeftXAxis());
+  }
+
+  public static double getControllerLeftYAxis() {
+    return m_driverController.getLeftY();
+  }
+
+  public static double getScaledControllerLeftYAxis() {
+    return scaleAxis(getControllerLeftYAxis());
+  }
+
+  public static double getControllerRightXAxis() {
+    return m_driverController.getRightX();
+  }
+
+  public static double getScaledControllerRightXAxis() {
+    return scaleAxis(getControllerRightXAxis());
+  }
+
+  public static double getControllerRightYAxis() {
+    return m_driverController.getRightY();
+  }
+
+  public static double getScaledControllerRightYAxis() {
+    return scaleAxis(getControllerRightYAxis());
+  }
 }

src/main/java/frc/robot/commands/DriveFieldOriented.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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+public class DriveFieldOriented extends Command {
+  private final Drive drive;
+
+  /** Creates a new DriveFieldOriented. */
+  public DriveFieldOriented(Drive drive) {
+    this.drive = drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    drive.driveFieldOriented(
+        RobotContainer.getScaledControllerLeftYAxis(),
+        RobotContainer.getScaledControllerLeftXAxis(),
+        RobotContainer.getControllerRightXAxis());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drive.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}