From 047d0fa3c3e154f0b833024827dc625bd5fa52cd Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Wed, 25 Dec 2024 15:09:39 -0800 Subject: [PATCH] Dual Motor Slide Subsystem --- .../subsystem/DualMotorSliderSubsystem.java | 131 ++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java new file mode 100644 index 0000000..f95733a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class DualMotorSliderSubsystem { + + /* + liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object + Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet. + It expects an object of type "DcMotorEx". + */ + private DcMotorEx liftSlideLeft; + + /* + liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object + Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet. + It expects an object of type "DcMotorEx". + */ + private DcMotorEx liftSlideRight; + + /* + targetPosition - Variable that holds target position of slides. + */ + private int targetPosition = 0; + + /* + getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and, + instead, use a "setter" to set the target position value or + a "getter" to get the target position value. + */ + public void setTargetPosition(int value) { + targetPosition = value; + } + private int getTargetPosition() { return targetPosition; } + + /* + PID - Proportional/Integral/Derivative Values + + For a dual motor linear slide, we only tune the P - Proportion. + The Proportion variable (kp) answers the question "how fast do we want to get to our destination?" + + It's the only value we set because the variable ki and kd deal with how to handle when we're off the path. + Since we're going straight, we don't need to worry about. + */ + public final static double kp = 0.0015, ki = 0, kd = 0; + + /* + lastError/integralSum/timer - These 3 variables are placeholders in determining how much + power to send to both motors. + */ + private double lastError = 0; + private double integralSum = 0; + private ElapsedTime timer = new ElapsedTime(); + + public DualMotorSliderSubsystem(HardwareMap hardwareMap) { + /* + liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable + + We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the + driver hub configuration name that matches the motor on the slide. + */ + liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR); + liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR); + } + + public void init() { + /* + Initialize the motors with the following settings (assuming slide is at the very bottom position): + + - Reset the encoders to be zero + - When the motor stops moving (zero power), brake. This means we can't move the motors, not even gravity. + - Sets the motor to run without the encoder. This doesn't mean we won't use the encoder values (because we will). + It just means to not FULLY depend on them, we will just prefer to use motor power instead. + */ + liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + /* + To calculate power, we send the targetPosition value (how high we want the slider motors to + be based on motor 'ticks') to calculatePower function. + + For every loop that occurs, it will constantly calculate power. So long as we're far away from + our "target" position, we will get as much power as possible. The closer we get, the lower the + power we will receive. + */ + public void update() { + double power = calculatePower(); + liftSlideLeft.setPower(power); + liftSlideRight.setPower(power); + } + + /* + Calculating power - To calculate the power, we determine the proportion, derivative and + integral of our closed loop system. + + For more information, please visit: + - Introduction to Closed Loop System: + - https://www.ctrlaltftc.com/introduction-to-closed-loop-control + - The PID controller: + - https://www.ctrlaltftc.com/the-pid-controller + */ + private double calculatePower() { + double error = getTargetPosition() - liftSlideLeft.getCurrentPosition(); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + lastError = error; + timer.reset(); + return (error * kp) + (derivative * kd) + (integralSum * ki); + } + + public void toLowBucketPosition() { + setTargetPosition(1500); + } + + public void toHighBucketPosition() { + setTargetPosition(3000); + } + +}