From d9ca39cbef56574f5ac099e1e0215d8b4b2de140 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 30 Nov 2023 20:32:26 -0600 Subject: [PATCH] finished Heading Lock, and started adding the PID controller for the robot to move the arm --- .../CenterStage/Common/XDrivetrainBot.java | 26 ++++--------------- .../States/PrototypeRobotDrivetrainState.java | 4 +-- .../TeleOp/States/XDrivetrainRobotState.java | 21 ++++++++++++--- .../TeleOp/States/headingLockTeleOp.java | 13 +--------- 4 files changed, 25 insertions(+), 39 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 e26c799..a44b6c1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java @@ -17,10 +17,9 @@ 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); + public double collectLock = Math.toRadians(-90); ElapsedTime timer = new ElapsedTime(); public int armPosition = 0; public boolean stateFinished; @@ -28,7 +27,7 @@ public class XDrivetrainBot extends Robot { public int oldArmPosition = 0; public long waitTime; private HardwareMap hardwareMap; - public DcMotor frontLeft, frontRight, backLeft, backRight; + public DcMotor frontLeft, frontRight, backLeft, backRight, armMotor; public DcMotor odometerR, odometerL, odometerA; public IMU imu; private String string; @@ -69,9 +68,6 @@ public class XDrivetrainBot extends Robot { public float collectorPos; public double rx; - - - public final double cm_per_tick = (2 * Math.PI * R) / N; private CyberarmEngine engine; @@ -96,6 +92,7 @@ public class XDrivetrainBot extends Robot { frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); + armMotor = engine.hardwareMap.dcMotor.get("arm"); // configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -105,6 +102,7 @@ public class XDrivetrainBot extends Robot { backRight.setDirection(DcMotorSimple.Direction.FORWARD); backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + armMotor.setDirection(DcMotorSimple.Direction.FORWARD); @@ -113,6 +111,7 @@ public class XDrivetrainBot extends Robot { backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //IMU IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( @@ -193,19 +192,4 @@ public class XDrivetrainBot extends Robot { 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/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index 9f642d5..b009b3b 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 @@ -12,8 +12,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { private PrototypeRobot robot; private int maxExtension = 2000; private int minExtension = 0; - private PIDController HeadingPidController; - private double targetHeading; public double integralSum = 0; double Kp = 0; double Ki = 0; @@ -111,7 +109,7 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { robot.driveTrainTeleOp(); if (engine.gamepad1.b){ - targetHeading = robot.backDropLock; +// targetHeading = robot.backDropLock; } // lift 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 900a1a8..b27891c 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 @@ -12,11 +12,12 @@ import dev.cyberarm.engine.V2.CyberarmState; @Config public class XDrivetrainRobotState extends CyberarmState { private XDrivetrainBot robot; - private PIDController HeadingPidController; + private PIDController ArmPidController; + public static double ArmP, ArmI, ArmD; private double targetHeading; private double currentHeading; public double integralSum = 0; - public static double Kp = 0; + public static double Kp = 1; public static double Ki = 0; public static double Kd = 0; ElapsedTime timer = new ElapsedTime(); @@ -27,6 +28,8 @@ public class XDrivetrainRobotState extends CyberarmState { public XDrivetrainRobotState(XDrivetrainBot robot) { this.robot = robot; + ArmPidController = new PIDController(ArmP, ArmI, ArmD); + } @@ -42,8 +45,12 @@ public class XDrivetrainRobotState extends CyberarmState { return radians; } + public void ArmControl(){ + + } + public double HeadingPIDControl(double reference, double current){ - double error = angleWrap(reference - current); + double error = angleWrap(current - reference); integralSum += error * timer.seconds(); double derivative = (error - lastError) / timer.seconds(); @@ -78,6 +85,14 @@ public class XDrivetrainRobotState extends CyberarmState { headingLock = true; targetHeading = robot.backDropLock; } + if (engine.gamepad1.x){ + headingLock = true; + targetHeading = robot.collectLock; + } + if (engine.gamepad1.a){ + headingLock = true; + targetHeading = currentHeading; + } if (engine.gamepad1.right_stick_x != 0){ headingLock = false; } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java index 3345675..9cae281 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java @@ -15,7 +15,7 @@ public class headingLockTeleOp extends CyberarmState { private PrototypeRobot robot; private PIDController HeadingPidController; public double integralSum = 0; - public static double Kp = 0; + public static double Kp = 1; public static double Ki = 0; public static double Kd = 0; private double lastError = 0; @@ -38,17 +38,6 @@ public class headingLockTeleOp extends CyberarmState { 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() { robot.headingLock = false;