Odometry road runner integration

.gitignore

@@ -86,4 +86,4 @@ lint/outputs/
 lint/tmp/
 # lint/reports/
 
-VuForiaKey.java
+.SpaceVim.d

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java

@@ -1,14 +1,16 @@
 package org.firstinspires.ftc.teamcode.API.Config;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
 import org.firstinspires.ftc.teamcode.API.Sensor;
 
+@Config
 public class Constants {
     /*
      * These are motor constants that should be listed online for your motors.
      */
-    public static final double TICKS_PER_REV = 4096;
+    public static final double TICKS_PER_REV = 8192;
     public static final double MAX_RPM = 340;
 
     /*
@@ -32,17 +34,17 @@ public class Constants {
      */
     public static double WHEEL_RADIUS = 2; // in
     public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
-    public static double TRACK_WIDTH = 15.5; // in
+    public static double TRACK_WIDTH = 14.4; // in
 
     /*
      * These are the feedforward parameters used to model the drive motor behavior. If you are using
      * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
      * motor encoders or have elected not to use them for velocity control, these values should be
      * empirically tuned.
      */
-    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
+    public static double kV = 0.0170;
     public static double kA = 0;
-    public static double kStatic = 0;
+    public static double kStatic = 0.07;
 
     /*
      * These values are used to generate the trajectories for you robot. To ensure proper operation,
@@ -53,18 +55,34 @@ public class Constants {
      * forces acceleration-limited profiling). All distance units are inches.
      */
     public static double MAX_VEL = 30.0;
-    public static double MAX_ACCEL = 20.0;
+    public static double MAX_ACCEL = 17.5;
     public static double MAX_ANG_VEL = Math.toRadians(180.0);
-    public static double MAX_ANG_ACCEL = Math.toRadians(180.0);
+    public static double MAX_ANG_ACCEL = Math.toRadians(90.0);
 
+
+    /**
+     * Returns encoder ticks in converted inches
+     * @param ticks number of encoder ticks as stated by the encoder
+     * @return encoder ticks converted to inches
+     */
     public static double encoderTicksToInches(double ticks) {
         return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
     }
 
+    /**
+     * Returns rpm in converted velocity
+     * @param rpm revolutions per minute of the motor
+     * @return velocity of the robot using the rpm information and some math
+     */
     public static double rpmToVelocity(double rpm) {
         return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
     }
 
+    /**
+     * Returns ticks per second in converted velocity
+     * @param ticksPerSecond ticks per second information as listed on the encoder
+     * @return
+     */
     public static double getMotorVelocityF(double ticksPerSecond) {
         // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
         return 32767 / ticksPerSecond;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java

@@ -15,6 +15,9 @@ public class Naming {
 
     public static final String SERVO_WOBBLE_GRABBER               = "wobbleGrabber";
     public static final String SERVO_LAUNCHER                     = "launcher";
+
+    public static final String SERVO_HOOK_1                       = "hook1";
+    public static final String SERVO_HOOK_2                       = "hook2";
 
     public static final String COLOR_SENSOR_PARK                  = "parkSensor";
     public static final String COLOR_SENSOR_ARM                   = "armSensor";

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java

@@ -35,6 +35,10 @@ public class SmartMotor {
     // Motor configuration
     @Getter @Setter double powerModifier = 1;
 
+    /**
+     * Creates a SmartMotor object based on motor object
+     * @param motor motor object that is being "converted" into a SmartMotor
+     */
     public SmartMotor(DcMotorEx motor) {
         this.motor = motor;
         power = 0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java

@@ -30,7 +30,10 @@ public class InitRobot {
     private static SmartMotor fl;
     private static SmartMotor fr;
 
-    // TODO: JavaDoc
+    /**
+     * Initializes the robot as per information stored in the passed LinearOpMode
+     * @param l LinearOpMode object consisting of all the robot's information
+     */
     public static void init(@NotNull LinearOpMode l) {
         /*
         * #######                   ######
@@ -73,12 +76,16 @@ public static void init(@NotNull LinearOpMode l) {
         // Get servos
         SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER));
         SmartServo launcher = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_LAUNCHER));
+        SmartServo hook1 = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_HOOK_1));
+        SmartServo hook2 = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_HOOK_2));
 
         // Add servos into the list
         HashMap<String, SmartServo> servos = new HashMap<>();
         //servos.put(Naming.SERVO_WOBBLE_ARM, wobbleArm);
         servos.put(Naming.SERVO_WOBBLE_GRABBER, wobbleGrabber);
         servos.put(Naming.SERVO_LAUNCHER, launcher);
+        servos.put(Naming.SERVO_HOOK_1, hook1);
+        servos.put(Naming.SERVO_HOOK_2, hook2);
 
         // Add CRServos into the list
         HashMap<String, CRServo> crServos = new HashMap<>();
@@ -109,10 +116,10 @@ public static void init(@NotNull LinearOpMode l) {
             fr.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         }
 
-        bl.setPowerModifier(Constants.MOTOR_BL_POWERMOD);
-        br.setPowerModifier(Constants.MOTOR_BR_POWERMOD);
-        fl.setPowerModifier(Constants.MOTOR_FL_POWERMOD);
-        fr.setPowerModifier(Constants.MOTOR_FR_POWERMOD);
+        //bl.setPowerModifier(Constants.MOTOR_BL_POWERMOD);
+        //br.setPowerModifier(Constants.MOTOR_BR_POWERMOD);
+        //fl.setPowerModifier(Constants.MOTOR_FL_POWERMOD);
+        //fr.setPowerModifier(Constants.MOTOR_FR_POWERMOD);
 
         // Get color sensors
         SmartColorSensor parkSensor = new SmartColorSensor((NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java

@@ -49,6 +49,11 @@ public class Movement {
     private final static double WOBBLE_RELEASE   = 0;
     private final static double LAUNCHER_OPEN    = 0.6;
     private final static double LAUNCHER_CLOSE   = 1;
+    private final static double HOOK_OPEN        = 1;
+    private final static double HOOK_CLOSE       = 0.4;
+    private final static double HOOK_IN          = 0.6;
+    private final static double HOOK_OUT         = 0;
+
 
     public static HashMap<String, SmartMotor> motors;
     public static HashMap<String, SmartServo> servos;
@@ -175,7 +180,7 @@ public void moveIntake(double intakePower) {
 
     /**
      * Opens the grabber based on a boolean assignment
-     * @param command tue to open
+     * @param command true to open
      */
     public void grabWobble(boolean command) {
         if (command) {
@@ -185,12 +190,39 @@ public void grabWobble(boolean command) {
         }
     }
 
-
+    /**
+     * Pushes the disk for launch based on a boolean assignment
+     * @param command true to push the disk
+     */
     public void launch(boolean command) {
         if (command) {
             Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_OPEN);
         } else {
             Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_CLOSE);
         }
     }
+
+    /**
+     * Pushes the disk into the intake based on a boolean assignment
+     * @param command true to push the disk
+     */
+    public void hook(boolean command) {
+        if (command) {
+            Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_CLOSE);
+        } else {
+            Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_OPEN);
+        }
+    }
+
+    /**
+     * Opens the hook to push the disk into the intake based on a boolean assignment
+     * @param command true to open the hook
+     */
+    public void extendHook(boolean command) {
+        if (command) {
+            Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_1)).setPosition(HOOK_OUT);
+        } else {
+            Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_1)).setPosition(HOOK_IN);
+        }
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Robot.java

@@ -41,7 +41,13 @@ public class Robot {
     public static void whiteLine(String sensor, double power) {
         driveToColor(sensor, power, Sensor.Colors.WHITE);
     }
-
+
+    /**
+     * Stops the robot once it detects the desired color
+     * @param sensor name of the color sensor as noted in Naming
+     * @param power amount of power to be delivered
+     * @param targetColor the specific color as noted in Sensor that the robot must detect
+     */
     public static void driveToColor(String sensor, double power, Sensor.Colors targetColor) {
         while (true) {
             Sensor.Colors color = Sensor.getRGB(sensor);
@@ -53,7 +59,12 @@ public static void driveToColor(String sensor, double power, Sensor.Colors targe
             }
         }
     }
-
+
+    /**
+     * Shoots the desired number of disks based on desired voltage autonomously
+     * @param count number of disks to be shot
+     * @param volts amount of power assigned to shoot the disk
+     */
     public static void shootAuto(int count, double volts) {
         // Power up flywheel
         movement.moveFlywheel(volts/Robot.sensor.getBatteryVoltage(linearOpMode.hardwareMap.voltageSensor));
@@ -64,11 +75,20 @@ public static void shootAuto(int count, double volts) {
             movement.launch(false);
         }
     }
-
+
+    /**
+     * Shoots the desired number of disks based on assigned voltage autonomously
+     * @param count number of disks to be shot
+     */
     public static void shootAuto(int count) {
         shootAuto(count, 10.00);
     }
-
+
+    /**
+     * Moves the robot arm autonomously to interact with the wobble goal
+     * @param sensor name of the color sensor to assist arm movement
+     * @param arm name of the arm motor that is moving the arm
+     */
     public static void moveArm(String sensor, String arm) {
         SmartMotor armMotor = Movement.getMotor(arm);
         Sensor.Colors target;
@@ -99,6 +119,12 @@ public static void moveArm(String sensor, String arm) {
         }
     }
 
+    /**
+     * Moves the robot arm based on boolean assignment to interact with the wobble goal
+     * @param command true to open the arm
+     * @param sensor name of the color sensor to assist arm movement
+     * @param arm name of the arm motor that is moving the arm
+     */
     public static void moveArm(boolean command, String sensor, String arm) {
         SmartMotor armMotor = Movement.getMotor(arm);
         Sensor.Colors target;
@@ -124,10 +150,12 @@ public static void moveArm(boolean command, String sensor, String arm) {
         armMotor.setPower(0);
     }
 
-
+    /**
+     * Pulls the arm out and drop the wobble goal
+     */
     public static void wobbleDrop() {
         Robot.moveArm(true, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM);
-        Robot.movement.grabWobble(true);
+        Robot.movement.grabWobble(true); // TODO modify this function because it's confusing
         linearOpMode.sleep(500);
         Robot.moveArm(false, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java

@@ -62,8 +62,8 @@
  */
 @Config
 public class SampleMecanumDrive extends MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.6, 0.7, 1.2);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.65, 0.5, 1.2);
 
     public static double LATERAL_MULTIPLIER = 1;
 
@@ -134,7 +134,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         }
 
         // TODO: adjust the names of the following hardware devices to match your configuration
-        imu = Sensor.getGyro(Naming.GYRO_0);
+        imu = Sensor.getGyro(Naming.GYRO_1);
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
         parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
         imu.initialize(parameters);
@@ -176,6 +176,11 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
        setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
     }
 
+    /**
+     * Builds a trajectory for the robot to move according to in autonomous
+     * @param startPose starting position of the robot on the field
+     * @return the trajectory the robot must follow
+     */
     public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
         return new TrajectoryBuilder(startPose, velConstraint, accelConstraint);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java

@@ -273,6 +273,11 @@ public void calibrateGyro(String id) {
         ReadWriteFile.writeFile(file, calibrationData.serialize());
     }
 
+    /**
+     * Converts RGB to HSV values for color sensor detection
+     * @param rgb color stored in RGB that is to be converted
+     * @return the color converted to HSV
+     */
     public static double[] RGBtoHSV(double[] rgb) {
         double[] hsv = new double[] {0, 0, 0};
         double max = Math.max(Math.max(rgb[0], rgb[1]), rgb[2]);
@@ -306,7 +311,11 @@ public static double[] RGBtoHSV(double[] rgb) {
         return hsv;
     }
 
-    // Computes the current battery voltage
+    /**
+     * Computes the current battery voltage
+     * @param voltageSensors all the powers of the sensors in the robot
+     * @return the current battery voltage of the robot
+     */
     public double getBatteryVoltage(HardwareMap.DeviceMapping<VoltageSensor> voltageSensors) {
         double result = Double.POSITIVE_INFINITY;
         for (VoltageSensor sensor : voltageSensors) {
@@ -350,7 +359,11 @@ public static void initTfod() {
             tfod.setZoom(2, 16.0/9.0);
         }
     }
-
+
+    /**
+     * Returns the number of disks the robot sees using the camera
+     * @return the number of disks detected (none, zero, or four)
+     */
     public static Disks detectDisks() {
         for (int i = 0; i<1000000; i++) {
             if (Robot.linearOpMode.isStopRequested()) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java

@@ -28,12 +28,12 @@
  */
 @Config
 public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
-    public static double TICKS_PER_REV = 4096;
-    public static double WHEEL_RADIUS = 0.7; // in
+    public static double TICKS_PER_REV = 8192;
+    public static double WHEEL_RADIUS = 0.8; // in. bumped to more than actuality because we want to make sure that it doesn't overshoot
     public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
 
-    public static double LATERAL_DISTANCE = 19.81; // in; distance between the left and right wheels
-    public static double FORWARD_OFFSET = 4.8; // in; offset of the lateral wheel
+    public static double LATERAL_DISTANCE = 15.9; // in; distance between the left and right wheels
+    public static double FORWARD_OFFSET = 6.0; // in; offset of the lateral wheel
 
     private static Encoder leftEncoder, rightEncoder, lateralEncoder;
 
@@ -51,7 +51,7 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
         // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
         leftEncoder.setDirection(Encoder.Direction.FORWARD);
         rightEncoder.setDirection(Encoder.Direction.REVERSE);
-        lateralEncoder.setDirection(Encoder.Direction.FORWARD);
+        lateralEncoder.setDirection(Encoder.Direction.REVERSE);
     }
 
     public static double encoderTicksToInches(double ticks) {