From faa9d9ed926aba508b35cb100b9e39d8ec3da0f0 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Tue, 28 Nov 2023 20:29:40 -0600 Subject: [PATCH] tested code for PID heading lock in tele op.. got the robot to move --- .../CenterStage/Common/XDrivetrainBot.java | 211 ++++++++++++++++++ .../Engines/XDriveTrainRobotEngine.java | 22 ++ .../States/PrototypeRobotDrivetrainState.java | 186 --------------- .../TeleOp/States/XDrivetrainRobotState.java | 99 ++++++++ 4 files changed, 332 insertions(+), 186 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/XDriveTrainRobotEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java new file mode 100644 index 0000000..e26c799 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java @@ -0,0 +1,211 @@ +package org.timecrafters.CenterStage.Common; + +import com.arcrobotics.ftclib.drivebase.HDrive; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + + +public class XDrivetrainBot extends Robot { + public double PIDrx; + public double targetHeading; + public boolean headingLock = false; + public double backDropLock = Math.toRadians(90); + ElapsedTime timer = new ElapsedTime(); + public int armPosition = 0; + public boolean stateFinished; + public long startOfSequencerTime; + public int oldArmPosition = 0; + public long waitTime; + private HardwareMap hardwareMap; + public DcMotor frontLeft, frontRight, backLeft, backRight; + public DcMotor odometerR, odometerL, odometerA; + public IMU imu; + private String string; + private double drivePower = 1; + + public double xMultiplier = 1; + public double yMultiplier = 1; + public double positionX; + public double positionY; + public double positionH; + + // robot geometry constants for odometry ----------------------------------------------------------------------------------------------- + public int currentRightPosition = 0; + public int currentLeftPosition = 0; + public int currentAuxPosition = 0; + public int oldRightPosition = 0; + public int oldLeftPosition = 0; + public int oldAuxPosition = 0; + public double globalPositionX; + public double globalPositionY; + public double globalPositionH; + public double localPositionX; + public double localPositionY; + public double localPositionH; + + public final static double L = 23.425; // distance between left and right encoder in cm + final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm + public final static double R = 4.5; // wheel radius in cm + final static double N = 8192; // encoder ticks per revolution (REV encoder) + + public final double MaxVelocityForward = 40; + public final double MaxStrafeVelocity = 34; + public final double MaxRotationalVelocity = 20; + private boolean lbsVar1; + private boolean lbsVar2; + private boolean rbsVar2; + public float depositorPos; + public float collectorPos; + public double rx; + + + + + public final double cm_per_tick = (2 * Math.PI * R) / N; + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + public XDrivetrainBot(String string) { + this.engine = engine; + setup(); + this.string = string; + } + + @Override + public void setup() { + System.out.println("Bacon: " + this.string); + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + //MOTORS + frontRight = engine.hardwareMap.dcMotor.get("frontRight"); + frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); + backRight = engine.hardwareMap.dcMotor.get("backRight"); + backLeft = engine.hardwareMap.dcMotor.get("backLeft"); + + +// configuration = new TimeCraftersConfiguration("Blue Crab"); + + + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + + + + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //IMU + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + + imu.initialize(parameters); + + } + + public void driveTrainTeleOp() { + boolean lbs1 = engine.gamepad1.left_stick_button; + if (lbs1 && !lbsVar1) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbsVar1 = lbs1; + + double y = -engine.gamepad1.left_stick_y; + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + // angle math to make things field oriented + double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); + + // joystick math to find the approximate power across each wheel for a movement + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + + // setting each power determined previously from the math above + // as well as multiplying it by a drive power that can be changed. + backLeft.setPower(backLeftPower * drivePower); + backRight.setPower(backRightPower * drivePower); + frontLeft.setPower(frontLeftPower * drivePower); + frontRight.setPower(frontRightPower * drivePower); + } + public void OdometryLocalizer(){ + + if (Math.toDegrees(positionH) > 360){ + positionH -= 360; + } + + globalPositionX = localPositionX; + globalPositionY = localPositionY; + globalPositionH = localPositionH; + + + // update positions + oldRightPosition = currentRightPosition; + oldLeftPosition = currentLeftPosition; + oldAuxPosition = currentAuxPosition; + + currentRightPosition = -odometerR.getCurrentPosition(); + currentLeftPosition = -odometerL.getCurrentPosition(); + currentAuxPosition = odometerA.getCurrentPosition(); + + int dnl1 = currentLeftPosition - oldLeftPosition; + int dnr2 = currentRightPosition - oldRightPosition; + int dna3 = currentAuxPosition - oldAuxPosition; + + // the robot has turned and moved a tiny bit between two measurements + double dtheta = cm_per_tick * (dnr2 - dnl1) / L; + double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; + double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); + + // the small movement of the bot gets added to the field coordinates + double theta = positionH + (dtheta / 2.0); + positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; + positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; + positionH += dtheta; + + } + +// public double PIDControl(double reference, double current){ +// double error = reference - current; +// integralSum += error * timer.seconds(); +// double derivative = (error - lastError) / timer.seconds(); +// +// timer.reset(); +// +// double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); +// return output; +// } + + + + + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/XDriveTrainRobotEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/XDriveTrainRobotEngine.java new file mode 100644 index 0000000..18146da --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/XDriveTrainRobotEngine.java @@ -0,0 +1,22 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.Common.XDrivetrainBot; +import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; +import org.timecrafters.CenterStage.TeleOp.States.XDrivetrainRobotState; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "PID Heading Lock Robot", group = "PROTOTYPE") +public class XDriveTrainRobotEngine extends CyberarmEngine { + private XDrivetrainBot robot; + @Override + public void setup() { + this.robot = new XDrivetrainBot("Hello World"); + this.robot.setup(); + + addState(new XDrivetrainRobotState(robot)); + } +} 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 e480496..9f642d5 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,192 +107,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { robot.collector.setPosition(robot.collectorPos); - // arm sequencer -// robot.ArmSequences(); -// switch (robot.armPosition) { -// case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos -// switch (robot.oldArmPosition) { -// case 0: -// // transfer -// robot.oldArmPosition = 0; -// break; -// case 1: -// // driving -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.oldArmPosition = 0; -// } -// break; -// case 2: -// // collect -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { -// robot.oldArmPosition = 0; -// } -// } -// break; -// case 3: -// // deposit -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position -// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met -// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { -// robot.oldArmPosition = 0; -// } -// } -// } -// } -// break; -// } -// break; -// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos -// switch (robot.oldArmPosition) { -// case 0: -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { -// robot.oldArmPosition = 1; -// } -// } -// break; -// case 1: -// // drive pos -// robot.oldArmPosition = 1; -// -// break; -// case 2: -// // collect -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { -// robot.oldArmPosition = 1; -// } -// } -// break; -// case 3: -// // deposit -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position -// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met -// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.oldArmPosition = 1; -// } -// } -// break; -// } -// break; -// -// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos -// switch (robot.oldArmPosition) { -// case 0: -// // transfer -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { -// robot.oldArmPosition = 2; -// } -// } -// break; -// case 1: -// // driving -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { -// robot.oldArmPosition = 2; -// } -// } -// break; -// case 2: -// // collect -// break; -// case 3: -// // deposit -// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met -// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) { -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) { -// robot.oldArmPosition = 2; -// } -// } -// } -// } -// } -// } -// break; -// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos -// switch (robot.oldArmPosition) { -// case 0: -// // transfer -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ -// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ -// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ -// robot.oldArmPosition = 3; -// } -// } -// } -// } -// } -// break; -// case 1: -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ -// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ -// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ -// robot.oldArmPosition = 3; -// } -// } -// } -// } -// } -// break; -// case 2: -// // collect -// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met -// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { -// robot.oldArmPosition = 3; -// } -// } -// break; -// case 3: -// // deposit -// break; -// } -// break; - -// } - // drivetrain robot.driveTrainTeleOp(); 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 new file mode 100644 index 0000000..900a1a8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java @@ -0,0 +1,99 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.Common.XDrivetrainBot; + +import dev.cyberarm.engine.V2.CyberarmState; +@Config +public class XDrivetrainRobotState extends CyberarmState { + private XDrivetrainBot robot; + private PIDController HeadingPidController; + private double targetHeading; + private double currentHeading; + public double integralSum = 0; + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + ElapsedTime timer = new ElapsedTime(); + private double lastError = 0; + private boolean headingLock = false; + + private long lastCheckedTime; + + public XDrivetrainRobotState(XDrivetrainBot robot) { + this.robot = robot; + + } + + // } + // --------------------------------------------------------------------------------------------------------- Slider control function + public double angleWrap(double radians) { + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI){ + radians += 2 * Math.PI; + } + return radians; + } + + public double HeadingPIDControl(double reference, double current){ + double error = angleWrap(reference - current); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + return output; + } + + @Override + public void init() { + } + + @Override + public void exec() { + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + if (engine.gamepad1.right_stick_button) { + robot.imu.resetYaw(); + } + + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + + } + // drivetrain + robot.driveTrainTeleOp(); + + if (engine.gamepad1.b){ + headingLock = true; + targetHeading = robot.backDropLock; + } + if (engine.gamepad1.right_stick_x != 0){ + headingLock = false; + } + + } + + @Override + public void telemetry () { + engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("arm Pos", robot.armPosition); + engine.telemetry.addData("old arm pos", robot.oldArmPosition); + engine.telemetry.addData("depositor pos", robot.depositorPos); + engine.telemetry.addData("collector pos", robot.collectorPos); + engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); + engine.telemetry.addData("heading lock?", headingLock); + engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading)); + } + } +