From f06b8252bf2f0cd8037e195e3cddfcfc67280e68 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 2 Dec 2023 12:00:22 -0600 Subject: [PATCH] pid working on arm --- .../CenterStage/Common/XDrivetrainBot.java | 9 +++- .../States/PrototypeRobotDrivetrainState.java | 5 -- .../TeleOp/States/XDrivetrainRobotState.java | 53 +++++++++++++++++-- 3 files changed, 57 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java index a44b6c1..9d941c6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java @@ -27,9 +27,10 @@ public class XDrivetrainBot extends Robot { public int oldArmPosition = 0; public long waitTime; private HardwareMap hardwareMap; - public DcMotor frontLeft, frontRight, backLeft, backRight, armMotor; + public DcMotor frontLeft, frontRight, backLeft, backRight, armMotor, chinUpMotor; public DcMotor odometerR, odometerL, odometerA; public IMU imu; + public Servo liftServo; private String string; private double drivePower = 1; @@ -93,6 +94,10 @@ public class XDrivetrainBot extends Robot { backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); armMotor = engine.hardwareMap.dcMotor.get("arm"); + chinUpMotor = engine.hardwareMap.dcMotor.get("chinUpMotor"); + + //SERVOS + liftServo = engine.hardwareMap.servo.get("lift"); // configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -103,6 +108,7 @@ public class XDrivetrainBot extends Robot { backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); armMotor.setDirection(DcMotorSimple.Direction.FORWARD); + chinUpMotor.setDirection(DcMotorSimple.Direction.REVERSE); @@ -112,6 +118,7 @@ public class XDrivetrainBot extends Robot { backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + chinUpMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //IMU IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index b009b3b..326b411 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -107,11 +107,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { // drivetrain robot.driveTrainTeleOp(); - - if (engine.gamepad1.b){ -// targetHeading = robot.backDropLock; - - } // lift SliderTeleOp(); // collector depositor diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java index b27891c..ffa943a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java @@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.TeleOp.States; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -13,23 +14,29 @@ import dev.cyberarm.engine.V2.CyberarmState; public class XDrivetrainRobotState extends CyberarmState { private XDrivetrainBot robot; private PIDController ArmPidController; - public static double ArmP, ArmI, ArmD; + private float ticksInDegree = 144 / 180; + + public static double ArmP = 0, ArmI = 0, ArmD = 0, ArmF = 0; private double targetHeading; + + public double power; private double currentHeading; public double integralSum = 0; + public double ArmintegralSum = 0; + public static double liftServoPos = 0.5; public static double Kp = 1; public static double Ki = 0; public static double Kd = 0; + public static double target; ElapsedTime timer = new ElapsedTime(); private double lastError = 0; + private double ArmlastError = 0; private boolean headingLock = false; private long lastCheckedTime; public XDrivetrainRobotState(XDrivetrainBot robot) { this.robot = robot; - ArmPidController = new PIDController(ArmP, ArmI, ArmD); - } @@ -45,10 +52,30 @@ public class XDrivetrainRobotState extends CyberarmState { return radians; } - public void ArmControl(){ + public double ArmControlPower(double reference, double current){ + double error = (current - reference); + ArmintegralSum += error * timer.seconds(); + double derivative = (error - ArmlastError) / timer.seconds(); + + timer.reset(); + + double output = (error * ArmP) + (derivative * ArmD) + (integralSum * ArmI); + return output; } + public void chinUpControl(){ + if(engine.gamepad1.left_bumper){ + robot.chinUpMotor.setPower(-1); + } else if(engine.gamepad1.right_bumper){ + robot.chinUpMotor.setPower(1); + } else { + robot.chinUpMotor.setPower(0); + } + + } + + public double HeadingPIDControl(double reference, double current){ double error = angleWrap(current - reference); integralSum += error * timer.seconds(); @@ -66,6 +93,10 @@ public class XDrivetrainRobotState extends CyberarmState { @Override public void exec() { + + if (engine.gamepad2.right_stick_button){ + robot.armMotor.setMode(DcMotor.RunMode.RESET_ENCODERS); + } currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); if (engine.gamepad1.right_stick_button) { @@ -78,6 +109,11 @@ public class XDrivetrainRobotState extends CyberarmState { robot.rx = engine.gamepad1.right_stick_x; } + + robot.armMotor.setPower(ArmControlPower(target, robot.armMotor.getCurrentPosition())); + + robot.liftServo.setPosition(liftServoPos); + // drivetrain robot.driveTrainTeleOp(); @@ -97,6 +133,9 @@ public class XDrivetrainRobotState extends CyberarmState { headingLock = false; } + chinUpControl(); + + } @Override @@ -109,6 +148,12 @@ public class XDrivetrainRobotState extends CyberarmState { engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); engine.telemetry.addData("heading lock?", headingLock); engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading)); + engine.telemetry.addData("robot arm current pos ", robot.armMotor.getCurrentPosition()); + engine.telemetry.addData("arm pid power ", power); + engine.telemetry.addData("p", ArmP); + engine.telemetry.addData("i", ArmI); + engine.telemetry.addData("d", ArmD); + engine.telemetry.addData("f", ArmF); } }