Create Autonomous

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Launch Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "RA22_RobotCode"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

build.gradle

@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2022.3.1"
+    id "edu.wpi.first.GradleRIO" version "2022.4.1"
     id "jacoco"
 }
 
@@ -53,6 +53,8 @@ def includeDesktopSupport = true
 dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
+
+    implementation 'com.google.code.gson:gson:2.9.0'
 
     roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
     roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)

src/main/deploy/autos/auto-test.json

@@ -0,0 +1,28 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "SECONDS",
+            "amount": 0.1,
+            "args": [
+                -0.05, 
+                -0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                1
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": -90,
+            "args": [
+                0.1,
+                -0.1
+            ]
+        }
+    ]
+}

src/main/deploy/autos/autonomous.json

@@ -0,0 +1,28 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "CURRENT",
+            "amount": 35,
+            "args": [
+                0.1,
+                0.1
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                1
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "FEET",
+            "amount": 6.75,
+            "args": [
+                -0.1,
+                -0.1
+            ]
+        }
+    ]
+}

src/main/deploy/autos/current-test.json

@@ -0,0 +1,28 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "CURRENT",
+            "amount": 20,
+            "args": [
+                0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                1
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "FEET",
+            "amount": 5,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        }
+    ]
+}

src/main/deploy/autos/distance-test.json

@@ -0,0 +1,76 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        }
+    ]
+}

src/main/deploy/autos/shooter-test.json

@@ -0,0 +1,17 @@
+{
+    "auto": [
+        {
+            "type": "shoot",
+            "unit": "SPEED",
+            "args": [
+                1000
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                5
+            ]
+        }
+    ]
+}

src/main/java/frc/robot/Autonomous.java

@@ -0,0 +1,5 @@
+package frc.robot;
+
+public abstract class Autonomous {
+    public abstract void run();
+}

src/main/java/frc/robot/DriveModule.java

@@ -83,6 +83,10 @@ public void setSpeed(double speed) {
         main.set(TalonFXControlMode.Velocity, speed * 22000);
     }
 
+    public void setPower(double power) {
+        main.set(TalonFXControlMode.PercentOutput, power);
+    }
+
     /**
      * Get the velocity of the module.
      *
@@ -92,6 +96,15 @@ public double getSpeed() {
         return main.getSelectedSensorVelocity();// * VELOCITY_COEFFICIENT;
     }
 
+    /**
+     * Get the main sensor position (in raw sensor units).
+     * 
+     * @return Position of selected sensor (in raw sensor units).
+     */
+    public double getDriveEnc() {
+        return main.getSelectedSensorPosition();
+    }
+
     /**
      * Gets the average current drawn.
      *

src/main/java/frc/robot/Drivetrain.java

@@ -37,9 +37,14 @@ public class Drivetrain implements Loggable {
      * @param leftSpeed The speed of the left motors
      * @param rightSpeed The speed of the right motors
      */
-    public void drive(double leftSpeed, double rightSpeed) { // Probably implement deadbands later
-        left.setSpeed(leftSpeed);
-        right.setSpeed(rightSpeed);
+    public void drive(double leftValue, double rightValue, boolean power) { // Probably implement deadbands later
+        if(power) {
+            left.setPower(leftValue);
+            right.setPower(rightValue);
+        } else {
+            left.setSpeed(leftValue);
+            right.setSpeed(rightValue);
+        }
     }
 
     /**
@@ -49,7 +54,7 @@ public void drive(double leftSpeed, double rightSpeed) { // Probably implement d
      * @param speedInput The speed to drive
      */
     public void arcadeDrive(double turnInput, double speedInput) {
-        this.drive(speedInput - turnInput, speedInput + turnInput);
+        this.drive(speedInput - turnInput, speedInput + turnInput, false);
     }
 
     /**
@@ -59,7 +64,7 @@ public void arcadeDrive(double turnInput, double speedInput) {
      * @param rightDrive The speed to set the right motors
      */
     public void tankDrive(double leftDrive, double rightDrive) {
-        this.drive(leftDrive, rightDrive);
+        this.drive(leftDrive, rightDrive, false);
     }
 
     /**
@@ -80,6 +85,19 @@ public boolean getShifter() {
         return shifter.get();
     }
 
+    /**
+     * Gets the encoder information for the left DriveModule
+     * 
+     * @return Position of left sensor (in raw sensor units).
+     */
+    public double getEncoder() {
+        return (left.getDriveEnc() + right.getDriveEnc()) / 2;
+    }
+
+    public double getAverageCurrent() {
+        return (left.getAverageCurrent() + right.getAverageCurrent()) / 2;
+    }
+
     /**
      * Shifts gears based on current.
      */