From cc8c46a6b382a56b779a32017c60b424fa283488 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 14 Oct 2023 13:01:47 -0500 Subject: [PATCH] Worked On odometry localization and drive to coordinates --- .../Autonomous/States/AutoStateSample.java | 5 +- .../States/DriveToCoordinatesState.java | 82 +++++++++++++++++++ .../CenterStage/Common/PrototypeRobot.java | 70 +++++++++++++++- 3 files changed, 152 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java index af0abe7..0a60da1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java @@ -7,6 +7,8 @@ import dev.cyberarm.engine.V2.CyberarmState; public class AutoStateSample extends CyberarmState { private final boolean stateDisabled; + private double newX; + private double newY; PrototypeRobot robot; public AutoStateSample(PrototypeRobot robot, String groupName, String actionName) { this.robot = robot; @@ -20,9 +22,6 @@ public class AutoStateSample extends CyberarmState { @Override public void exec() { - // do stuff - // ------------- - // end: setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java new file mode 100644 index 0000000..00abe95 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java @@ -0,0 +1,82 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import com.arcrobotics.ftclib.controller.PIDController; + +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class DriveToCoordinatesState extends CyberarmState { + + private final boolean stateDisabled; + PrototypeRobot robot; + private PIDController driveXPidController; + private PIDController driveYPidController; + private PIDController driveHPidController; + private double pidX; + private double pidY; + private double pidH; + + public double xTarget; + public double yTarget; + public double hTarget; + + public double currentVelocityX; + public double currentVelocityY; + public double currentVelocityH; + + public double targetVelocityFL; + public double targetVelocityBL; + public double targetVelocityFR; + public double targetVelocityBR; + + + public static double Xp = 0, Xi = 0, Xd = 0; // p = 0.02 i = 0.09 d = 0 + public static double Yp = 0, Yi = 0, Yd = 0;// p = 0.03 i = 0.07 d = 0 + public static double Hp = 0, Hi = 0, Hd = 0;// p = 0.03 i = 0.07 d = 0 + + public DriveToCoordinatesState(PrototypeRobot robot, String groupName, String actionName) { + this.robot = robot; + driveXPidController = new PIDController(Xp, Xi, Xd); + driveYPidController = new PIDController(-Yp, -Yi, -Yd); + driveHPidController = new PIDController(-Hp, -Hi, -Hd); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); + this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); + this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); + } + + @Override + public void start() { + //add variables that need to be reinitillized + } + + @Override + public void exec() { + // PID functions to come up with the velocity to get to the final point. + driveXPidController.setPID(Xp, Xi, Xd); + pidX = driveXPidController.calculate(robot.positionX, xTarget); + driveYPidController.setPID(Yp, Yi, Yd); + pidY = (driveYPidController.calculate(robot.positionY, yTarget)); + driveHPidController.setPID(Hp, Hi, Hd); + pidH = (driveYPidController.calculate(robot.positionH, hTarget)); + + // PID robot Velocity Ratios + currentVelocityX = robot.MaxVelocityForward * pidX; + currentVelocityY = robot.MaxStrafeVelocity * pidY; + currentVelocityH = robot.MaxRotationalVelocity * pidH; + + // Kinematic Formulas with plugged in velocities to solve for wheel velocities. + targetVelocityFL = currentVelocityX - currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); + targetVelocityBL = currentVelocityX + currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); + targetVelocityBR = currentVelocityX - currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); + targetVelocityFR = currentVelocityX + currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); + + robot.frontLeft.setVelocity((targetVelocityFL / robot.R)); + robot.backLeft.setVelocity((targetVelocityBL / robot.R)); + robot.backRight.setVelocity((targetVelocityBR / robot.R)); + robot.frontRight.setVelocity((targetVelocityFR / robot.R)); + +// setHasFinished(true) + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index ea03e41..a51ff16 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -1,9 +1,7 @@ package org.timecrafters.CenterStage.Common; import com.arcrobotics.ftclib.drivebase.HDrive; -import com.arcrobotics.ftclib.hardware.RevIMU; import com.arcrobotics.ftclib.hardware.motors.MotorEx; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -29,10 +27,42 @@ public class PrototypeRobot extends Robot { public float currentSetPosElbow; private HardwareMap hardwareMap; public MotorEx frontLeft, frontRight, backLeft, backRight, lift; + public DcMotor odometerR, odometerL, odometerA; public IMU imu; public Servo depositorShoulder, depositorElbow, depositor; private HDrive xDrive; private String string; + 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; + + + + public final double cm_per_tick = (2 * Math.PI * R) / N; private CyberarmEngine engine; public TimeCraftersConfiguration configuration; @@ -118,4 +148,40 @@ public class PrototypeRobot extends Robot { servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow))); } + 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; + + } } \ No newline at end of file