Create Autonomous

.vscode/launch.json

@@ -4,12 +4,18 @@
   // 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",

build.gradle

@@ -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,26 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "SECONDS",
+            "amount": 0.1,
+            "args": [
+                -0.05, 
+                -0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "amount": 1
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": -90,
+            "args": [
+                0.1,
+                -0.1
+            ]
+        }
+    ]
+}

src/main/deploy/autos/autonomous.json

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

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

@@ -0,0 +1,26 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "CURRENT",
+            "amount": 20,
+            "args": [
+                0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "amount": 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,13 @@
+{
+    "auto": [
+        {
+            "type": "shoot",
+            "unit": "SPEED",
+            "amount": 1000
+        },
+        {
+            "type": "wait",
+            "amount": 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

@@ -87,6 +87,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.
      *
@@ -96,6 +100,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

@@ -42,9 +42,15 @@ public void update() {
      * @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.set(leftSpeed);
-        right.set(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);
+        }
     }
 
     /**
@@ -54,7 +60,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);
     }
 
     /**
@@ -64,7 +70,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);
     }
 
     /**
@@ -85,10 +91,26 @@ 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.
      */
     public void checkGears() {
+        left.updateCurrent();
+        right.updateCurrent();
+
         if (getShifter()) {
             if (left.getAverageCurrent() > SHIFT_CURRENT_LOW
                     || right.getAverageCurrent() > SHIFT_CURRENT_LOW) {
@@ -120,12 +142,14 @@ public void checkGears() {
 
     @Override
     public void setupLogging(Logger logger) {
-        // logger.addLoggable(left);
-        // logger.addLoggable(right);
+        // TODO Auto-generated method stub
+
     }
 
     @Override
     public void log(Logger logger) {
         // TODO Auto-generated method stub
+
     }
+
 }