From a188fdfdb53a5589b42ec1d733318790d54e9960 Mon Sep 17 00:00:00 2001 From: Picklze1 Date: Fri, 2 Feb 2024 18:42:30 -0700 Subject: [PATCH] PID impimentation in Drive Code --- .../auton/{autonPark.java => AutonPark.java} | 2 +- .../PID.java} | 21 +++-- .../subsystems/robotMethods/RobotMethods.java | 12 +++ .../ftc/teamcode/teleop/TeleOp.java | 94 +++++++++++-------- 4 files changed, 80 insertions(+), 49 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/{autonPark.java => AutonPark.java} (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/{pid/RobotComponents.java => robotMethods/PID.java} (81%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonPark.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonPark.java index 0545e3c5..3834fd00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java +++ b/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}}; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/PID.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/PID.java index 4308574d..3053467a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java +++ b/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()); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java index 740dd340..3364b4f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java +++ b/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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java index 200ade59..129c0c5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java +++ b/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.get("leftFront").setPower(leftFPower); - robot.get("leftRear").setPower(leftBPower); - robot.get("rightFront").setPower(rightFPower); - robot.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.get("leftFront"))); + double leftRearCorrection = rightFrontPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.get("leftRear"))); + double rightFrontCorrection = leftRearPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.get("rightFront"))); + double rightRearCorrection = rightRearPID.calculate(forwardPower * targetRPM, pid.calculateRPM(robot.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.get("leftFront").setPower(frontLeftPower); + robot.get("leftRear").setPower(rearLeftPower); + robot.get("rightFront").setPower(frontRightPower); + robot.get("rightRear").setPower(rearRightPower); + } + + // Scoring method, all together like a bowl of chili. public void scoring() { telemetry.addData("Scoring Status", "Started");