Merge pull request #87 from rh-robotics/george-jensen-pid-implimentation

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonPark.java

@@ -13,7 +13,7 @@
 import org.firstinspires.ftc.teamcode.subsystems.robotMethods.*;
 
 @Autonomous(name = "Auton Park")
-public class autonPark extends OpMode {
+public class AutonPark extends OpMode {
     byte startingLocationIndex = 0;
     double[][] startingLocation = new double[][] {new double[] {0, 0, 0}, new double[] {0, 0, 0},
             new double[] {0, 0, 0}, new double[] {0, 0, 0}};

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/PID.java

@@ -1,10 +1,11 @@
-package org.firstinspires.ftc.teamcode.subsystems.pid;
+package org.firstinspires.ftc.teamcode.subsystems.robotMethods;
 
 import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
 /** Initialize a motor as a RobotComponent with its values for PID functionality and extra features */
-public class RobotComponents {
+public class PID {
     private final DcMotorEx motor;
     private final double ticks_per_degree;
     private final double F;
@@ -21,7 +22,7 @@ public class RobotComponents {
      * @param d                  The Derivative Value tuned after P, this value helps reduce oscillation in reaching a target and should result in a smoothed out curve when going to a target. Extremely small and sensitive, typically around 0.001. Re-tune after adjusting I.
      * @param f                  The Feed-forward value, tuned before anything else by increasing until the motor can hold itself against gravity at any position. Only use if you want a motor to hold its position against gravity and never use on locking systems like worm gears (in this case, set to 0)! Value is variable based on necessary motor power, but should be low (under 0.1) to avoid motor overheating and power draw.
      */
-    RobotComponents(DcMotorEx motor, double ticks_per_rotation, double p, double i, double d, double f) {
+    PID(DcMotorEx motor, double ticks_per_rotation, double p, double i, double d, double f) {
         this.motor = motor;
         this.F = f;
         ticks_per_degree = ticks_per_rotation / 360.0;
@@ -63,7 +64,6 @@ public void incrementTarget(double increment) {
      * Sends power to the RobotComponent's motor to smoothly and automatically reach currentTarget (or whatever its called, might be different) using the PID loop. Must be called every code loop!
      */
     public void moveUsingPID() {
-
         controller.reset();
         motor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
         int armPos = motor.getCurrentPosition();
@@ -72,15 +72,22 @@ public void moveUsingPID() {
         double power = pid + ff;
 
         motor.setPower(power);
+    }
+
+    public double calculateRPM(DcMotor motor) {
+        int motorPos = motor.getCurrentPosition();
+        double motorCountsPerRev = 537.7;
+        double gearRatio = 1.0;
 
+        return (motorPos/ motorCountsPerRev) * 60 * gearRatio;
     }
 
     /**
      * Returns true if the motor's current encoder position is within the specified range of currentTarget and false otherwise (e.g. true for a pos of 167 with a target of 170 and a range of 5, false for the same conditions and a pos of 200)
      *
-     * @param range The permissible range of encoder positions relative to the current target (e.g. target of 170 with a range of 5 will accept positions from 165 to 175)
+     * @param tolerance The permissible range of encoder positions relative to the current target (e.g. target of 170 with a range of 5 will accept positions from 165 to 175)
      */
-    public boolean motorCloseEnough(int range) {
-        return (target - range <= motor.getCurrentPosition()) && (target + range >= motor.getCurrentPosition());
+    public boolean motorCloseEnough(int tolerance) {
+        return (target - tolerance <= motor.getCurrentPosition()) && (target + tolerance >= motor.getCurrentPosition());
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystems.robotMethods;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
@@ -9,6 +10,7 @@
 
 import java.util.ArrayList;
 import java.util.HashMap;
+import org.firstinspires.ftc.teamcode.subsystems.robotMethods.PID.*;
 
 public class RobotMethods {
     static float wheelEncoderPPR = 537.7f; // PPR
@@ -190,6 +192,16 @@ private static boolean wheelsInPosition(Hardware robot, double tolerance) {
                 Math.abs(motor4.getTargetPosition()-motor4.getCurrentPosition()) < tolerance;
     }
 
+    public static int getWheelDiameter() {
+        return wheelDiameter;
+    }
+
+    // Stick Sensitivity
+     public double scaleInput(double inputValue) {
+        double expo = 2.0; // Adjust For Sensitivity
+        return Math.pow(inputValue, expo);
+    }
+
     private double inchesToMM(double inches) {
         return inches*25.4;
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -10,20 +11,21 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
 import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement;
-
+import org.firstinspires.ftc.teamcode.subsystems.robotMethods.PID;
+import org.firstinspires.ftc.teamcode.subsystems.robotMethods.RobotMethods;
 
 @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "TeleOp", group = "OpMode")
 public class TeleOp extends OpMode {
     private Hardware robot;
+    private PID pid;
+    private RobotMethods rMethods;
+    private PIDController leftFrontPID;
+    private PIDController leftRearPID;
+    private PIDController rightFrontPID;
+    private PIDController rightRearPID;
 
     // Drive Variables
-    double leftFPower;
-    double rightFPower;
-    double leftBPower;
-    double rightBPower;
-    double drive;
-    double turn;
-    double strafe;
+    int targetRPM = 200;
 
     // Intake Variables
     double intakePower = 0.5;
@@ -41,6 +43,9 @@ public class TeleOp extends OpMode {
     byte lockPositionIndex = 0;
     byte slidePositionIndex = 0;
     byte armPositionIndex = 0;
+    double P_Constant = 0.1;
+    double I_Constant = 0.1;
+    double D_Constant = 0.1;
 
     double[] lockPositions = new double[] {0.0, 0.4, 0.8};
     double[] slidePositions = new double[] {0.0, 1.0};
@@ -63,6 +68,11 @@ public void init() {
 
         robot = new Hardware(hardwareMap, telemetry);
 
+        PIDController leftFrontPID = new PIDController(P_Constant, I_Constant, D_Constant);
+        PIDController leftRearPID = new PIDController(P_Constant, I_Constant, D_Constant);
+        PIDController rightFrontPID = new PIDController(P_Constant, I_Constant, D_Constant);
+        PIDController rightRearPID = new PIDController(P_Constant, I_Constant, D_Constant);
+
         // Init Servos.
         robot.introduce(new HardwareElement<>(Servo.class, hardwareMap, "clawLock"));
         robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "clawJoint"));
@@ -74,10 +84,10 @@ public void init() {
         robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeTube"));
 
         // Init DcMotors.
-        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", "setDirection:FORWARD"));
-        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", "setDirection:FORWARD"));
-        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront", "setDirection:FORWARD"));
-        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", "setDirection:FORWARD, setMode:RUN_USING_ENCODER"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", "setDirection:FORWARD, setMode:RUN_USING_ENCODER"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront", "setDirection:FORWARD, setMode:RUN_USING_ENCODER"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear", "setMode:RUN_USING_ENCODER"));
 
         // Init arm and Viper Slide DcMotors.
         robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "armJoint", "setMode:RUN_USING_ENCODER"));
@@ -247,41 +257,43 @@ public void outerIntakeJoints() {
 
     // Strafe Drive using sticks on Gamepad 1.
     public void driving() {
-        // Values for drive.
-        drive = gamepad1.left_stick_y * 0.8;
-        turn = -gamepad1.right_stick_x * 0.6;
-        strafe = gamepad1.left_stick_x * 0.8;
 
-        // Set power to values calculated above.
-        robot.<DcMotor>get("leftFront").setPower(leftFPower);
-        robot.<DcMotor>get("leftRear").setPower(leftBPower);
-        robot.<DcMotor>get("rightFront").setPower(rightFPower);
-        robot.<DcMotor>get("rightRear").setPower(-rightBPower);
+        double forwardPower = rMethods.scaleInput(currentGamepad1.left_stick_y);
+        double turnPower = rMethods.scaleInput(currentGamepad1.right_stick_x);
+        double strafePower = rMethods.scaleInput(currentGamepad1.left_stick_x);
 
-        telemetry.addData("leftFPower: ", leftFPower);
-        telemetry.addData("leftBPower: ", leftBPower);
-        telemetry.addData("rightFPower: ", rightFPower);
-        telemetry.addData("rightBPower: ", rightBPower);
+        // Values for drive.
+
+        double leftFrontCorrection = leftFrontPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.<DcMotor>get("leftFront")));
+        double leftRearCorrection = rightFrontPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.<DcMotor>get("leftRear")));
+        double rightFrontCorrection = leftRearPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.<DcMotor>get("rightFront")));
+        double rightRearCorrection = rightRearPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.<DcMotor>get("rightRear")));
 
         // Calculate drive power.
-        if (drive != 0 || turn != 0) {
-            leftFPower = Range.clip(drive + turn, -1.0, 1.0);
-            rightFPower = Range.clip(drive - turn, -1.0, 1.0);
-            leftBPower = Range.clip(drive + turn, -1.0, 1.0);
-            rightBPower = Range.clip(drive - turn, -1.0, 1.0);
-        } else if (strafe != 0) {
-            leftFPower = -strafe;
-            rightFPower = strafe;
-            leftBPower = strafe;
-            rightBPower = -strafe;
-        } else {
-            leftFPower = 0;
-            rightFPower = 0;
-            leftBPower = 0;
-            rightBPower = 0;
-        }
+        double leftFrontPower = forwardPower + strafePower + turnPower;
+        double leftRearPower = forwardPower - strafePower + turnPower;
+        double rightFrontPower = forwardPower - strafePower - turnPower;
+        double rightRearPower = forwardPower + strafePower - turnPower;
+
+        setDriveMotorPowers(leftFrontPower + leftFrontCorrection, leftRearPower + leftRearCorrection,
+                rightFrontPower + rightFrontCorrection, rightRearPower + rightRearCorrection);
     }
 
+    private void setDriveMotorPowers(double frontLeftPower, double rearLeftPower, double frontRightPower, double rearRightPower) {
+        // Set motor powers within range [-1, 1]
+        frontLeftPower = Range.clip(frontLeftPower, -1.0, 1.0);
+        frontRightPower = Range.clip(frontRightPower, -1.0, 1.0);
+        rearLeftPower = Range.clip(rearLeftPower, -1.0, 1.0);
+        rearRightPower = Range.clip(rearRightPower, -1.0, 1.0);
+
+        // Set motor powers
+        robot.<DcMotor>get("leftFront").setPower(frontLeftPower);
+        robot.<DcMotor>get("leftRear").setPower(rearLeftPower);
+        robot.<DcMotor>get("rightFront").setPower(frontRightPower);
+        robot.<DcMotor>get("rightRear").setPower(rearRightPower);
+    }
+
+
     // Scoring method, all together like a bowl of chili.
     public void scoring() {
         telemetry.addData("Scoring Status", "Started");