From b449d90050c1aaaa1e8bbb0669dbd346b58de133 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 27 May 2023 20:26:13 -0700 Subject: [PATCH 01/29] test --- .../team766/robot/mechanisms/januaryTag.java | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 src/main/java/com/team766/robot/mechanisms/januaryTag.java diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java new file mode 100644 index 00000000..d72333ca --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -0,0 +1,50 @@ +package com.team766.robot.mechanisms; + +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +//import com.team766.logging.Category; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import java.io.IOException; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +public class januaryTag extends Mechanism{ + private PhotonCamera camera1; + private PhotonPoseEstimator photonPoseEstimator; + + public januaryTag(){ + camera1 = new PhotonCamera("januaryTag.camera1"); + + try { + // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field. + AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + // Create pose estimator + photonPoseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam); + photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } catch (IOException e) { + // The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know + // where the tags are. + log("Failed to load AprilTagFieldLayout"); + photonPoseEstimator = null; + } + } + +} From 569357e7fa6304e82bd2c2a0eff01f1666dbfb37 Mon Sep 17 00:00:00 2001 From: Max Date: Mon, 29 May 2023 19:47:54 -0700 Subject: [PATCH 02/29] added photonlib to code --- vendordeps/photonlib.json | 41 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 vendordeps/photonlib.json diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..dad31057 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file From 54198b5e8ffb33fe8fd3f9e0d6909cbe85669408 Mon Sep 17 00:00:00 2001 From: Max Date: Mon, 29 May 2023 21:09:22 -0700 Subject: [PATCH 03/29] added a lot, changed some, just check the description (-) old photonvison code that was in their examples (/) followed their documentation through the photonvision website (+) added method to easily find the best target (+) added method to get yaw (+) added method to get pitch (+) added method to get area that the target fills the screen (+) added method to get the target ID of the April tag (+) added dead zones (dz's) and a way to set them (+) added a way to calculate position badly (+) added a go() --- .../team766/robot/mechanisms/januaryTag.java | 117 +++++++++++++++--- .../robot/mechanisms/januaryTagException.java | 0 2 files changed, 102 insertions(+), 15 deletions(-) create mode 100644 src/main/java/com/team766/robot/mechanisms/januaryTagException.java diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index d72333ca..c798f6ac 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -14,37 +14,124 @@ import com.team766.library.RateLimiter; import com.team766.library.ValueProvider; //import com.team766.logging.Category; - +import de.erichseifert.gral.util.GeometryUtils; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import java.io.IOException; +import java.util.ArrayList; +import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; public class januaryTag extends Mechanism{ private PhotonCamera camera1; - private PhotonPoseEstimator photonPoseEstimator; - + private double deadzoneX; + private double deadzoneY; + private double x_target; + private double y_target; + private MotorController leftMotor; + private MotorController rightMotor; public januaryTag(){ camera1 = new PhotonCamera("januaryTag.camera1"); + leftMotor = RobotProvider.instance.getMotor("leftMotor"); + rightMotor = RobotProvider.instance.getMotor("rightMotor"); + } - try { - // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field. - AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); - // Create pose estimator - photonPoseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam); - photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - } catch (IOException e) { - // The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know - // where the tags are. - log("Failed to load AprilTagFieldLayout"); - photonPoseEstimator = null; + public PhotonTrackedTarget getBestTrackedTarget(){ + var result = camera1.getLatestResult(); //getting the result from the camera + boolean hasTargets = result.hasTargets(); // checking to see if there are any targets in the camera's view. IF THERE ISN'T AND YOU USE result.getTargets() YOU WILL GET AN ERROR + + if(hasTargets){ + List targets = result.getTargets(); // getting targets + PhotonPipelineResult photonPipelineResult = new PhotonPipelineResult(2, targets); // TODO: Replace latencyMillis with real latency time + + PhotonTrackedTarget bestTrackedTarget = result.getBestTarget(); // getting the best target that is currently being picked up by the camera so that it can know where it is + return bestTrackedTarget; + }else{ + log("No targets? see what i did there"); + throw new januaryTagException("There were no targets that could be picked up by the camera, so I'm gonna have to throw this error here."); } - } + } + + public double getYaw(){ + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + return theTarget.getYaw(); + } + + public double getPitch(){ + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + return theTarget.getPitch(); + } + + public double getArea(){ + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + return theTarget.getArea(); + } + + public int getTargetID(){ + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + return theTarget.getFiducialId(); + } + + public void setDeadzoneX(double dz){ + deadzoneX = dz; + } + + public void setDeadzoneY(double dz){ + deadzoneY = dz; + } + + public void setDeadzones(double x, double y){ + deadzoneX = x; + deadzoneY = y; + } + + public void findTargets(double x_scoring, double y_scoring){ + double robotX_pos = getYaw(); + double robotY_pos = getPitch(); + + double delta_x = x_scoring - robotX_pos; + x_target = x_scoring; + y_target = y_scoring - (0.2) * Math.abs(delta_x); + } + + public void go(double x_scoring, double y_scoring){ + while(2>1){ + if(getYaw() + deadzoneX > x_target && getYaw() - deadzoneX < x_target && getPitch() + deadzoneY > y_target && getPitch() - deadzoneY < y_target){ + log("Outtaking"); + break; + }else{ + double robotX_pos = getYaw(); + double robotY_pos = getPitch(); + + double delta_x = x_scoring - robotX_pos; + double forward = x_scoring; + double turn = y_scoring - (0.2) * Math.abs(delta_x); + + + double leftMotorPower = (0.2) * turn + (0.2) * forward; + double rightMotorPower = (-0.2) * turn + (0.2) * forward; + + leftMotor.set(leftMotorPower); + rightMotor.set(rightMotorPower); + } + } + } + + + + + + public void debugLogs(){ + + } } diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java new file mode 100644 index 00000000..e69de29b From 107b086e5fecabb238df0c1eebd838d648cce9a6 Mon Sep 17 00:00:00 2001 From: Max Date: Mon, 29 May 2023 21:12:35 -0700 Subject: [PATCH 04/29] this somehow didn't get included in the last commit --- .../com/team766/robot/mechanisms/januaryTagException.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java index e69de29b..0d1b6216 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java @@ -0,0 +1,7 @@ +package com.team766.robot.mechanisms; + +public class januaryTagException extends RuntimeException { + public januaryTagException (String errorMessage) { + super(errorMessage); + } +} From 82ced851301997c49de0d7f61a69e3a66d84b534 Mon Sep 17 00:00:00 2001 From: Max Date: Mon, 29 May 2023 21:39:27 -0700 Subject: [PATCH 05/29] working on getting the coords from getBestCameraToTarget() instead of getting them myself from like (x, y) (getYaw(), getPitch()) --- .../com/team766/robot/mechanisms/januaryTag.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index c798f6ac..be9b3f8b 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -18,6 +18,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import java.io.IOException; @@ -52,7 +53,8 @@ public PhotonTrackedTarget getBestTrackedTarget(){ if(hasTargets){ List targets = result.getTargets(); // getting targets PhotonPipelineResult photonPipelineResult = new PhotonPipelineResult(2, targets); // TODO: Replace latencyMillis with real latency time - + + PhotonTrackedTarget bestTrackedTarget = result.getBestTarget(); // getting the best target that is currently being picked up by the camera so that it can know where it is return bestTrackedTarget; }else{ @@ -79,6 +81,13 @@ public double getArea(){ public int getTargetID(){ PhotonTrackedTarget theTarget = getBestTrackedTarget(); return theTarget.getFiducialId(); + + } + + // work in progress + public void CTC(){ + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + Transform3d CTC = theTarget.getBestCameraToTarget(); } public void setDeadzoneX(double dz){ @@ -89,6 +98,9 @@ public void setDeadzoneY(double dz){ deadzoneY = dz; } + + + public void setDeadzones(double x, double y){ deadzoneX = x; deadzoneY = y; From 003230684f350d42af7a5ba61e043008882d06d3 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 31 May 2023 20:01:23 -0700 Subject: [PATCH 06/29] added new formulas --- .../team766/robot/mechanisms/januaryTag.java | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index be9b3f8b..c821c44e 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -63,6 +63,10 @@ public PhotonTrackedTarget getBestTrackedTarget(){ } } + public Transform3d getBestCameraToTarget(PhotonTrackedTarget target){ + return target.getBestCameraToTarget(); + } + public double getYaw(){ PhotonTrackedTarget theTarget = getBestTrackedTarget(); return theTarget.getYaw(); @@ -121,16 +125,18 @@ public void go(double x_scoring, double y_scoring){ log("Outtaking"); break; }else{ - double robotX_pos = getYaw(); - double robotY_pos = getPitch(); + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); - double delta_x = x_scoring - robotX_pos; - double forward = x_scoring; - double turn = y_scoring - (0.2) * Math.abs(delta_x); + double x_target = x_scoring + Math.cos(yaw) * (k * y_scoring); + double y_target = y_scoring + Math.sin(yaw) * (y_scoring); + + + double forward = Math.sqrt((y_target * y_target) + (x_target * x_target)); + double turn = -Math.tan(y_target/x_target); - double leftMotorPower = (0.2) * turn + (0.2) * forward; - double rightMotorPower = (-0.2) * turn + (0.2) * forward; + double leftMotorPower = (0.2) * turn + (0.2) * clampToRange(forward, -1, 1); + double rightMotorPower = (-0.2) * turn + (0.2) * clampToRange(forward, -1, 1); leftMotor.set(leftMotorPower); rightMotor.set(rightMotorPower); @@ -138,6 +144,15 @@ public void go(double x_scoring, double y_scoring){ } } + public double clampToRange(double value, double min, double max){ + if(value > max){ + return max; + }else if(value< min){ + return min; + } + return value; + } + From cbc5ba62b344a8b2fdb97df6f4e840d3572b46af Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 1 Jun 2023 16:31:23 +0000 Subject: [PATCH 07/29] added Transform3d capability and a few other util --- .../team766/robot/mechanisms/januaryTag.java | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index c821c44e..6c3f5877 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -15,16 +15,20 @@ import com.team766.library.ValueProvider; //import com.team766.logging.Category; import de.erichseifert.gral.util.GeometryUtils; +import de.erichseifert.gral.util.MathUtils; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.util.*; import java.io.IOException; import java.util.ArrayList; import java.util.List; import java.util.Optional; +import javax.tools.ForwardingFileObject; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -40,10 +44,17 @@ public class januaryTag extends Mechanism{ private double y_target; private MotorController leftMotor; private MotorController rightMotor; + private double x_targetConstant; + private double turnConstant; + private double forwardConstant; public januaryTag(){ camera1 = new PhotonCamera("januaryTag.camera1"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); rightMotor = RobotProvider.instance.getMotor("rightMotor"); + + x_targetConstant = 0.5; + turnConstant = 0.2; + forwardConstant = 0.2; } public PhotonTrackedTarget getBestTrackedTarget(){ @@ -63,6 +74,23 @@ public PhotonTrackedTarget getBestTrackedTarget(){ } } + public void setXtargetConstant(double constant){ + x_targetConstant = constant; + } + + public void setTurnConstant(double constant){ + turnConstant = constant; + } + + public void setForwardConstant(double constant){ + forwardConstant = constant; + } + + public void setArcadeDriveVisionConstants(double turn, double forward){ + turnConstant = turn; + forwardConstant = forward; + } + public Transform3d getBestCameraToTarget(PhotonTrackedTarget target){ return target.getBestCameraToTarget(); } @@ -127,16 +155,16 @@ public void go(double x_scoring, double y_scoring){ }else{ Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); - double x_target = x_scoring + Math.cos(yaw) * (k * y_scoring); - double y_target = y_scoring + Math.sin(yaw) * (y_scoring); + double x_target = x_scoring + Math.cos(targetTransform.getX()) * (x_targetConstant * y_scoring); + double y_target = y_scoring + Math.sin(targetTransform.getX()) * (y_scoring); double forward = Math.sqrt((y_target * y_target) + (x_target * x_target)); double turn = -Math.tan(y_target/x_target); - double leftMotorPower = (0.2) * turn + (0.2) * clampToRange(forward, -1, 1); - double rightMotorPower = (-0.2) * turn + (0.2) * clampToRange(forward, -1, 1); + double leftMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); + double rightMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); leftMotor.set(leftMotorPower); rightMotor.set(rightMotorPower); @@ -144,19 +172,6 @@ public void go(double x_scoring, double y_scoring){ } } - public double clampToRange(double value, double min, double max){ - if(value > max){ - return max; - }else if(value< min){ - return min; - } - return value; - } - - - - - public void debugLogs(){ } From c31a909526b938b2ba3ca99dfe55a761f3f2d42e Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 1 Jun 2023 16:41:41 +0000 Subject: [PATCH 08/29] fixed some stuff with old changes conflicting new ones and fixed the deadzone thingy --- .../team766/robot/mechanisms/januaryTag.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 6c3f5877..b84dc7f8 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -47,14 +47,19 @@ public class januaryTag extends Mechanism{ private double x_targetConstant; private double turnConstant; private double forwardConstant; + private double offsetX; + private double offsetY; public januaryTag(){ camera1 = new PhotonCamera("januaryTag.camera1"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); rightMotor = RobotProvider.instance.getMotor("rightMotor"); + //Sample values, we don't know these work at all yet x_targetConstant = 0.5; turnConstant = 0.2; forwardConstant = 0.2; + offsetX = 10; + offsetY = 10; } public PhotonTrackedTarget getBestTrackedTarget(){ @@ -138,22 +143,17 @@ public void setDeadzones(double x, double y){ deadzoneY = y; } - public void findTargets(double x_scoring, double y_scoring){ - double robotX_pos = getYaw(); - double robotY_pos = getPitch(); - - double delta_x = x_scoring - robotX_pos; - x_target = x_scoring; - y_target = y_scoring - (0.2) * Math.abs(delta_x); - } - - public void go(double x_scoring, double y_scoring){ + public void go(){ + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); while(2>1){ - if(getYaw() + deadzoneX > x_target && getYaw() - deadzoneX < x_target && getPitch() + deadzoneY > y_target && getPitch() - deadzoneY < y_target){ + if(targetTransform.getX() + deadzoneX - offsetX > 0 && targetTransform.getX() - deadzoneX - offsetX < 0 && targetTransform.getY() + deadzoneY - offsetY > 0 && targetTransform.getY() - deadzoneY -offsetY < 0){ log("Outtaking"); break; }else{ - Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + + + double x_scoring = targetTransform.getX() - offsetX; + double y_scoring = targetTransform.getY() - offsetY; double x_target = x_scoring + Math.cos(targetTransform.getX()) * (x_targetConstant * y_scoring); double y_target = y_scoring + Math.sin(targetTransform.getX()) * (y_scoring); From 07177069621552a521fd0e4213fcd05e9716c1a1 Mon Sep 17 00:00:00 2001 From: Max Date: Thu, 1 Jun 2023 21:53:17 -0700 Subject: [PATCH 09/29] fixed (1), added comments and again pretended like I was a linter --- .../team766/robot/mechanisms/januaryTag.java | 70 +++++++++++-------- 1 file changed, 39 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index b84dc7f8..dc60b068 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -62,15 +62,19 @@ public januaryTag(){ offsetY = 10; } + + /* + * This method returns the best target that the camera is currently tracking + * This is useful for the transform3d data + * @return PhotonTrackedTarget - the best target that the camera is currently tracking + */ public PhotonTrackedTarget getBestTrackedTarget(){ var result = camera1.getLatestResult(); //getting the result from the camera boolean hasTargets = result.hasTargets(); // checking to see if there are any targets in the camera's view. IF THERE ISN'T AND YOU USE result.getTargets() YOU WILL GET AN ERROR if(hasTargets){ List targets = result.getTargets(); // getting targets - PhotonPipelineResult photonPipelineResult = new PhotonPipelineResult(2, targets); // TODO: Replace latencyMillis with real latency time - PhotonTrackedTarget bestTrackedTarget = result.getBestTarget(); // getting the best target that is currently being picked up by the camera so that it can know where it is return bestTrackedTarget; }else{ @@ -83,6 +87,11 @@ public void setXtargetConstant(double constant){ x_targetConstant = constant; } + /* + * For all next three methods, the constants are the constants that are used in the arcade drive method + * You can set them individually or together + */ + public void setTurnConstant(double constant){ turnConstant = constant; } @@ -96,35 +105,32 @@ public void setArcadeDriveVisionConstants(double turn, double forward){ forwardConstant = forward; } + /* + * This method returns the Transform3d data from the best target that the camera is currently tracking + * @param target - the target that you want the camera to get the data from (use getBestTrackedTarget() to get the best target that the camera is currently tracking) + * @return Transform3d - the Transform3d from the best target that the camera is currently tracking + */ + public Transform3d getBestCameraToTarget(PhotonTrackedTarget target){ return target.getBestCameraToTarget(); } - public double getYaw(){ - PhotonTrackedTarget theTarget = getBestTrackedTarget(); - return theTarget.getYaw(); - } - - public double getPitch(){ - PhotonTrackedTarget theTarget = getBestTrackedTarget(); - return theTarget.getPitch(); - } - - public double getArea(){ + /* + * This method returns the data from the best target that the camera is currently tracking, not the Transform3d data + * @return double[] - the data from the best target that the camera is currently tracking + * [0] is the yaw + * [1] is the pitch + * [2] is the area that the apriltag fills the camera view with + * [3] is the fiducial id of the april tag + */ + public double[] getPhotonTrackedTargetData(){ + double[] arr = new double[4]; PhotonTrackedTarget theTarget = getBestTrackedTarget(); - return theTarget.getArea(); - } - - public int getTargetID(){ - PhotonTrackedTarget theTarget = getBestTrackedTarget(); - return theTarget.getFiducialId(); - - } - - // work in progress - public void CTC(){ - PhotonTrackedTarget theTarget = getBestTrackedTarget(); - Transform3d CTC = theTarget.getBestCameraToTarget(); + arr[0] = theTarget.getYaw(); + arr[1] = theTarget.getPitch(); + arr[2] = theTarget.getArea(); + arr[3] = theTarget.getFiducialId(); + return arr; } public void setDeadzoneX(double dz){ @@ -135,17 +141,19 @@ public void setDeadzoneY(double dz){ deadzoneY = dz; } - - - public void setDeadzones(double x, double y){ deadzoneX = x; deadzoneY = y; } - + /* + * This method is the method that you should call to make the robot go to the target. Currently, it will go to any apriltag that it sees, but we can change that later. + */ public void go(){ - Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + while(2>1){ + + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + if(targetTransform.getX() + deadzoneX - offsetX > 0 && targetTransform.getX() - deadzoneX - offsetX < 0 && targetTransform.getY() + deadzoneY - offsetY > 0 && targetTransform.getY() - deadzoneY -offsetY < 0){ log("Outtaking"); break; From 2f96adb18c50f4aa992e22c20a0cb6dd02302c09 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 2 Jun 2023 15:29:54 +0000 Subject: [PATCH 10/29] fixed (3); added offsets using a transform3d --- .../team766/robot/mechanisms/januaryTag.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index dc60b068..bd9656aa 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -20,8 +20,10 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.math.util.*; import java.io.IOException; @@ -49,6 +51,7 @@ public class januaryTag extends Mechanism{ private double forwardConstant; private double offsetX; private double offsetY; + private Transform3d offset; public januaryTag(){ camera1 = new PhotonCamera("januaryTag.camera1"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); @@ -60,6 +63,7 @@ public januaryTag(){ forwardConstant = 0.2; offsetX = 10; offsetY = 10; + offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); } @@ -153,18 +157,19 @@ public void go(){ while(2>1){ Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); - - if(targetTransform.getX() + deadzoneX - offsetX > 0 && targetTransform.getX() - deadzoneX - offsetX < 0 && targetTransform.getY() + deadzoneY - offsetY > 0 && targetTransform.getY() - deadzoneY -offsetY < 0){ + Transform3d scoring = targetTransform.plus(offset); + + if(scoring.getX() + deadzoneX > 0 && scoring.getX() - deadzoneX < 0 && scoring.getY() + deadzoneY > 0 && scoring.getY() - deadzoneY < 0){ log("Outtaking"); break; }else{ - double x_scoring = targetTransform.getX() - offsetX; - double y_scoring = targetTransform.getY() - offsetY; + double x_scoring = scoring.getX(); + double y_scoring = scoring.getY(); - double x_target = x_scoring + Math.cos(targetTransform.getX()) * (x_targetConstant * y_scoring); - double y_target = y_scoring + Math.sin(targetTransform.getX()) * (y_scoring); + double x_target = x_scoring + Math.cos(scoring.getX()) * (x_targetConstant * y_scoring); + double y_target = y_scoring + Math.sin(scoring.getX()) * (y_scoring); double forward = Math.sqrt((y_target * y_target) + (x_target * x_target)); From c1d4c1134bf514e18590aad6a6c136505a73f555 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 2 Jun 2023 15:41:08 +0000 Subject: [PATCH 11/29] added more (better) getter methods --- .../team766/robot/mechanisms/januaryTag.java | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index bd9656aa..ddf782ff 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -121,19 +121,35 @@ public Transform3d getBestCameraToTarget(PhotonTrackedTarget target){ /* * This method returns the data from the best target that the camera is currently tracking, not the Transform3d data - * @return double[] - the data from the best target that the camera is currently tracking + * @return ArrayList - the data from the best target that the camera is currently tracking * [0] is the yaw * [1] is the pitch * [2] is the area that the apriltag fills the camera view with * [3] is the fiducial id of the april tag */ - public double[] getPhotonTrackedTargetData(){ - double[] arr = new double[4]; + public ArrayList getPhotonTrackedTargetData(){ + ArrayList arr = new ArrayList(); PhotonTrackedTarget theTarget = getBestTrackedTarget(); - arr[0] = theTarget.getYaw(); - arr[1] = theTarget.getPitch(); - arr[2] = theTarget.getArea(); - arr[3] = theTarget.getFiducialId(); + arr.add(theTarget.getYaw()); + arr.add(theTarget.getPitch()); + arr.add(theTarget.getArea()); + arr.add((double) theTarget.getFiducialId()); + return arr; + } + + /* + * This is a method to return data given from a Transform3d + * @return an ArrayList with values about the Transform3d, assuming the camera is (0,0) + * [0] is the x value of the target relative to the camera + * [1] is the y value of the target relative to the camera + * [2] is the z value of the target relative to the camera + */ + public ArrayList getTransform3dData(){ + ArrayList arr = new ArrayList(); + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + arr.add(targetTransform.getX()); + arr.add(targetTransform.getY()); + arr.add(targetTransform.getZ()); return arr; } From 9b0ed31eef41750e2aa575776ded80dbe1cf1642 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 3 Jun 2023 12:10:16 -0700 Subject: [PATCH 12/29] resolve comment (a.1) Co-authored-by: Ryan Cahoon --- src/main/java/com/team766/robot/mechanisms/januaryTag.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index ddf782ff..16d3537b 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -184,7 +184,8 @@ public void go(){ double x_scoring = scoring.getX(); double y_scoring = scoring.getY(); - double x_target = x_scoring + Math.cos(scoring.getX()) * (x_targetConstant * y_scoring); + double yaw_scoring = scoring.getRotation().getZ(); + double x_target = x_scoring + Math.cos(yaw_scoring) * (x_targetConstant * y_scoring); double y_target = y_scoring + Math.sin(scoring.getX()) * (y_scoring); From 46782ed5b63136ce4d2cab017d0424fc486b60a7 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 3 Jun 2023 12:10:38 -0700 Subject: [PATCH 13/29] resolve comment (a.2) Co-authored-by: Ryan Cahoon --- src/main/java/com/team766/robot/mechanisms/januaryTag.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 16d3537b..13c42e76 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -186,7 +186,7 @@ public void go(){ double yaw_scoring = scoring.getRotation().getZ(); double x_target = x_scoring + Math.cos(yaw_scoring) * (x_targetConstant * y_scoring); - double y_target = y_scoring + Math.sin(scoring.getX()) * (y_scoring); + double y_target = y_scoring + Math.sin(yaw_scoring) * (x_targetConstant * y_scoring); double forward = Math.sqrt((y_target * y_target) + (x_target * x_target)); From b883173a8077c4ae50226ab6896958b4076ce1eb Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 3 Jun 2023 13:05:19 -0700 Subject: [PATCH 14/29] deleted a lot so I can actually use on a burrowbot --- .../com/team766/framework/AutonomousMode.java | 2 + .../com/team766/hal/GenericRobotMain.java | 4 +- .../java/com/team766/odometry/Odometry.java | 201 ------ .../com/team766/robot/AutonomousModes.java | 4 +- src/main/java/com/team766/robot/OI.java | 22 +- src/main/java/com/team766/robot/Robot.java | 31 +- .../com/team766/robot/mechanisms/Arms.java | 411 ----------- .../robot/mechanisms/ArmsAnimation.java | 67 -- .../robot/mechanisms/ArmsAntiGrav.java | 55 -- .../team766/robot/mechanisms/ArmsUtil.java | 58 -- .../team766/robot/mechanisms/CANdleMech.java | 50 -- .../com/team766/robot/mechanisms/Drive.java | 676 ------------------ .../robot/mechanisms/ExampleMechanism.java | 22 - .../com/team766/robot/mechanisms/Grabber.java | 30 - .../com/team766/robot/mechanisms/Gyro.java | 68 -- .../com/team766/robot/mechanisms/Intake.java | 79 -- .../team766/robot/mechanisms/januaryTag.java | 7 +- .../robot/procedures/AlignCharger.java | 36 - .../team766/robot/procedures/AutoScoring.java | 86 --- .../procedures/ChargeStationPathFinder.java | 86 --- .../team766/robot/procedures/DoNothing.java | 10 +- .../robot/procedures/FollowPoints.java | 482 ------------- .../team766/robot/procedures/GyroBalance.java | 180 ----- .../team766/robot/procedures/IntakeIn.java | 14 - .../team766/robot/procedures/IntakeOut.java | 14 - .../team766/robot/procedures/IntakeStop.java | 14 - .../team766/robot/procedures/OPECHelper.java | 27 - .../robot/procedures/OnePieceBalance.java | 33 - .../procedures/OnePieceExitCommunity.java | 32 - .../OnePieceExitCommunityBalance.java | 35 - .../robot/procedures/ReverseIntake.java | 12 - .../team766/robot/procedures/setCross.java | 16 - 32 files changed, 50 insertions(+), 2814 deletions(-) delete mode 100644 src/main/java/com/team766/odometry/Odometry.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/Arms.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/ArmsUtil.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/CANdleMech.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/Drive.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/Grabber.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/Gyro.java delete mode 100644 src/main/java/com/team766/robot/mechanisms/Intake.java delete mode 100644 src/main/java/com/team766/robot/procedures/AlignCharger.java delete mode 100644 src/main/java/com/team766/robot/procedures/AutoScoring.java delete mode 100644 src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java delete mode 100644 src/main/java/com/team766/robot/procedures/FollowPoints.java delete mode 100644 src/main/java/com/team766/robot/procedures/GyroBalance.java delete mode 100644 src/main/java/com/team766/robot/procedures/IntakeIn.java delete mode 100644 src/main/java/com/team766/robot/procedures/IntakeOut.java delete mode 100644 src/main/java/com/team766/robot/procedures/IntakeStop.java delete mode 100644 src/main/java/com/team766/robot/procedures/OPECHelper.java delete mode 100644 src/main/java/com/team766/robot/procedures/OnePieceBalance.java delete mode 100644 src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java delete mode 100644 src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java delete mode 100644 src/main/java/com/team766/robot/procedures/ReverseIntake.java delete mode 100644 src/main/java/com/team766/robot/procedures/setCross.java diff --git a/src/main/java/com/team766/framework/AutonomousMode.java b/src/main/java/com/team766/framework/AutonomousMode.java index d29337be..a7159049 100644 --- a/src/main/java/com/team766/framework/AutonomousMode.java +++ b/src/main/java/com/team766/framework/AutonomousMode.java @@ -11,6 +11,8 @@ public AutonomousMode(String name, Supplier constructor) { m_name = name; } + + public Procedure instantiate() { return m_constructor.get(); } diff --git a/src/main/java/com/team766/hal/GenericRobotMain.java b/src/main/java/com/team766/hal/GenericRobotMain.java index e4b72e15..4cb900c7 100755 --- a/src/main/java/com/team766/hal/GenericRobotMain.java +++ b/src/main/java/com/team766/hal/GenericRobotMain.java @@ -98,8 +98,8 @@ public void autonomousInit() { m_oiContext = null; } - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); + //Robot.gyro.resetGyro180(); + //Robot.drive.setGyro(Robot.gyro.getGyroYaw()); if (m_autonomous != null) { Logger.get(Category.AUTONOMOUS).logRaw(Severity.INFO, "Continuing previous autonomus procedure " + m_autonomous.getContextName()); diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java deleted file mode 100644 index 26f1b134..00000000 --- a/src/main/java/com/team766/odometry/Odometry.java +++ /dev/null @@ -1,201 +0,0 @@ -package com.team766.odometry; - -import com.team766.framework.LoggingBase; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; -import com.ctre.phoenix.sensors.CANCoder; -import com.team766.logging.Category; -import com.team766.robot.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -/** - * Method which calculates the position of the robot based on wheel positions. - */ -public class Odometry extends LoggingBase { - - private RateLimiter odometryLimiter; - - private MotorController[] motorList; - //The order of CANCoders should be the same as in motorList - private CANCoder[] CANCoderList; - private int motorCount; - - private PointDir[] prevPositions; - private PointDir[] currPositions; - private double[] prevEncoderValues; - private double[] currEncoderValues; - private double gyroPosition; - - private PointDir currentPosition; - - //In Meters - private static double WHEEL_CIRCUMFERENCE; - public static double GEAR_RATIO; - public static int ENCODER_TO_REVOLUTION_CONSTANT; - - //In the same order as motorList, relative to the center of the robot - private Point[] wheelPositions; - - /** - * Constructor for Odometry, taking in several defines for the robot. - * @param motors A list of every wheel-controlling motor on the robot. - * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors. - * @param wheelLocations A list of the locations of each wheel, in the same order as motors. - * @param wheelCircumference The circumfrence of the wheels, including treads. - * @param gearRatio The gear ratio of the wheels. - * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels. - * @param rateLimiterTime How often odometry should run. - */ - public Odometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime) { - loggerCategory = Category.ODOMETRY; - - odometryLimiter = new RateLimiter(rateLimiterTime); - motorList = motors; - CANCoderList = CANCoders; - motorCount = motorList.length; - log("Motor count " + motorCount); - prevPositions = new PointDir[motorCount]; - currPositions = new PointDir[motorCount]; - prevEncoderValues = new double[motorCount]; - currEncoderValues = new double[motorCount]; - - wheelPositions = wheelLocations; - WHEEL_CIRCUMFERENCE = wheelCircumference; - GEAR_RATIO = gearRatio; - ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant; - - currentPosition = new PointDir(0, 0, 0); - for (int i = 0; i < motorCount; i++) { - prevPositions[i] = new PointDir(0,0, 0); - currPositions[i] = new PointDir(0,0, 0); - prevEncoderValues[i] = 0; - currEncoderValues[i] = 0; - } - } - - public String getName() { - return "Odometry"; - } - - /** - * Sets the current position of the robot to Point P - * @param P The point to set the current robot position to - */ - public void setCurrentPosition(Point P) { - currentPosition.set(P); - log("Set Current Position to: " + P.toString()); - for (int i = 0; i < motorCount; i++) { - prevPositions[i].set(currentPosition.add(wheelPositions[i])); - currPositions[i].set(currentPosition.add(wheelPositions[i])); - } - log("Current Position: " + currentPosition.toString()); - } - - - /** - * Updates the odometry encoder values to the robot encoder values. - */ - private void setCurrentEncoderValues() { - for (int i = 0; i < motorCount; i++) { - prevEncoderValues[i] = currEncoderValues[i]; - currEncoderValues[i] = motorList[i].getSensorPosition(); - currEncoderValues[i] *= (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1); - } - } - - private static Vector2D rotate(Vector2D v, double angle) { - return new Vector2D( - v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(), - v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle)); - } - - /** - * Updates the position of each wheel of the robot by assuming each wheel moved in an arc. - */ - private void updateCurrentPositions() { - double angleChange; - double radius; - double deltaX; - double deltaY; - gyroPosition = -Robot.gyro.getGyroYaw(); - - /* - Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))), - Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())))); - */ - - for (int i = 0; i < motorCount; i++) { - //prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currPositions[i].getHeading()); - //This following line only works if the average of wheel positions is (0,0) - prevPositions[i].set(currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); - currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); - angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); - - double yaw = -Math.toRadians(Robot.gyro.getGyroYaw()); - double roll = Math.toRadians(Robot.gyro.getGyroRoll()); - double pitch = Math.toRadians(Robot.gyro.getGyroPitch()); - - double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); - Vector2D u = new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); - Vector2D v = new Vector2D(Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) - Math.sin(yaw) * Math.cos(roll), - Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + Math.cos(yaw) * Math.cos(roll)); - Vector2D a = u.scalarMultiply(Math.cos(w)).add(v.scalarMultiply(Math.sin(w))); - Vector2D b = u.scalarMultiply(-Math.sin(w)).add(v.scalarMultiply(Math.cos(w))); - Vector2D wheelMotion; - - //log("u: " + u + " v: " + v + " a: " + a + " b: " + b); - - //double oldWheelX; - //double oldWheelY; - - if (angleChange != 0) { - radius = 180 * (currEncoderValues[i] - prevEncoderValues[i]) / (Math.PI * angleChange); - deltaX = radius * Math.sin(Math.toRadians(angleChange)); - deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange))); - - wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY)); - - //oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - - } else { - wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i])); - - //oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - } - wheelMotion = wheelMotion.scalarMultiply(WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); - //wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition)); - //log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY - wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current Method: " + wheelMotion.getX() + ", " + wheelMotion.getY()); - //log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " + currPositions[i].add(wheelMotion)); - currPositions[i].set(currPositions[i].subtract(wheelMotion)); - } - } - - /** - * Calculates the position of the robot by finding the average of the wheel positions. - */ - private void findRobotPosition() { - double sumX = 0; - double sumY = 0; - for (int i = 0; i < motorCount; i++) { - sumX += currPositions[i].getX(); - sumY += currPositions[i].getY(); - //log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " + currPositions[i]); - } - currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition); - } - - //Intended to be placed inside Robot.drive.run() - public PointDir run() { - if (odometryLimiter.next()) { - setCurrentEncoderValues(); - updateCurrentPositions(); - findRobotPosition(); - log(currentPosition.toString()); - } - return currentPosition; - } -} diff --git a/src/main/java/com/team766/robot/AutonomousModes.java b/src/main/java/com/team766/robot/AutonomousModes.java index fb9085a4..7665c660 100644 --- a/src/main/java/com/team766/robot/AutonomousModes.java +++ b/src/main/java/com/team766/robot/AutonomousModes.java @@ -21,11 +21,11 @@ public class AutonomousModes { // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), // new AutonomousMode("FollowPoints", () -> new FollowPoints()), // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), - new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + //new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), // new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), // new AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), // new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")), // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})), - // new AutonomousMode("DoNothing", () -> new DoNothing()), + //new AutonomousMode("DoNothing", () -> new DoNothing()), }; } \ No newline at end of file diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 871d7174..e7f3f69a 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -8,7 +8,7 @@ import com.team766.robot.constants.InputConstants; import com.team766.robot.constants.InputConstants.IntakeState; import com.team766.robot.procedures.*; -import com.team766.robot.mechanisms.Drive; +import com.team766.robot.mechanisms.*; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; @@ -20,6 +20,19 @@ */ public class OI extends Procedure { + private JoystickReader joystickOne; + public OI(){ + joystickOne =RobotProvider.instance.getJoystick(0); + } + + public void run(Context context){ + context.takeOwnership(Robot.JanuaryTag); + + if(joystickOne.getButton(1)){ + Robot.JanuaryTag.debugLogs(); + } + } +/* private JoystickReader leftJoystick; private JoystickReader rightJoystick; private JoystickReader controlPanel; @@ -155,7 +168,7 @@ public void run(Context context) { Robot.storage.beltIdle(); intakeState = IntakeState.IDLE; } - } */ + } if (controlPanel.getButtonPressed(InputConstants.OUTTAKE)){ if (intakeState == IntakeState.IDLE){ Robot.intake.reverseIntake(); @@ -220,7 +233,7 @@ public void run(Context context) { /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ Robot.arms.pidForArmOne(22.73); Robot.arms.pidForArmTwo(-73.664); - } */ + } if(controlPanel.getButton(InputConstants.NUDGE_UP)){ Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); } @@ -249,5 +262,6 @@ public void run(Context context) { } } - } + }*/ + } diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index a9bf6946..6b746399 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -3,21 +3,22 @@ import com.team766.robot.mechanisms.*; public class Robot { - // Declare mechanisms here - public static Intake intake; - public static Storage storage; - public static Drive drive; - public static Arms arms; - public static Gyro gyro; - public static Grabber grabber; - + //Declare mechanisms here + // public static Intake intake; + // public static Storage storage; + // public static Drive drive; + // public static Arms arms; + // public static Gyro gyro; + // public static Grabber grabber; + public static januaryTag JanuaryTag; public static void robotInit() { - // Initialize mechanisms here - intake = new Intake(); - storage = new Storage(); - drive = new Drive(); - arms = new Arms(); - gyro = new Gyro(); - grabber = new Grabber(); + //Initialize mechanisms here + // intake = new Intake(); + // storage = new Storage(); + // drive = new Drive(); + // arms = new Arms(); + // gyro = new Gyro(); + // grabber = new Grabber(); + JanuaryTag = new januaryTag(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java deleted file mode 100644 index 63a09cd9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ /dev/null @@ -1,411 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.revrobotics.CANSparkMax; -import com.team766.config.ConfigFileReader; -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.logging.Category; -//This is for the motor that controls the pulley - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Arms extends Mechanism { - /* - * This defines the motors and casts them to CanSparkMaxs (CSMs) so we can use REV Robotics PID SmartMotion. - * Next, it also defines the absolute encoder. - */ - private MotorController firstJoint = RobotProvider.instance.getMotor("arms.firstJoint"); - private CANSparkMax firstJointCANSparkMax = (CANSparkMax)firstJoint; - private SparkMaxPIDController firstJointPIDController = firstJointCANSparkMax.getPIDController(); - private SparkMaxAbsoluteEncoder altEncoder1 = firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle); - - private MotorController secondJoint = RobotProvider.instance.getMotor("arms.secondJoint"); - private CANSparkMax secondJointCANSparkMax = (CANSparkMax)secondJoint; - private SparkMaxPIDController secondJointPIDController = secondJointCANSparkMax.getPIDController(); - private SparkMaxAbsoluteEncoder altEncoder2 = secondJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle); - - // Non-motor constants - // This is the deadzone, so that the arm(s) don't oscillate. For example, a value of 5 means a 5 relitive encoder unit deadzone in each direction. - private static final double doubleDeadZone = 2; - - // We want firstJoint/secondJoint being straight-up to be 0 rel encoder units - // and counter-clockwise to be positive. - // All the following variables are in degrees - private double firstJointPosition = 0; - private double secondJointPosition = 0; - private double firstJointCombo = 0; - private double secondJointCombo = 0; - - // This sets the maximum locations so we can use them in code to make sure the arm joints dont go past there. - private static final double FIRST_JOINT_MAX_LOCATION = 27.3; - private static final double FIRST_JOINT_MIN_LOCATION = -45; - private static final double SECOND_JOINT_MAX_LOCATION = 45; - private static final double SECOND_JOINT_MIN_LOCATION = -160; - - private RateLimiter runRateLimiter = new RateLimiter(0.05); - - enum ArmState { - PID, - ANTIGRAV, - OFF - } - - boolean jointOneCanContinue = false; - - ArmState theStateOf1 = ArmState.OFF; - ArmState theStateOf2 = ArmState.OFF; - - - private ArmsAntiGrav antiGrav; - - public Arms() { - loggerCategory = Category.MECHANISMS; - - firstJoint.setNeutralMode(NeutralMode.Brake); - secondJoint.setNeutralMode(NeutralMode.Brake); - - // PID Constants - ValueProvider firstJointP = ConfigFileReader.getInstance().getDouble("arms.firstJointP"); - ValueProvider firstJointI = ConfigFileReader.getInstance().getDouble("arms.firstJointI"); - ValueProvider firstJointD = ConfigFileReader.getInstance().getDouble("arms.firstJointD"); - ValueProvider firstJointFF = ConfigFileReader.getInstance().getDouble("arms.firstJointFF"); - - firstJointPIDController.setP(firstJointP.valueOr(0.0006)); - firstJointPIDController.setI(firstJointI.valueOr(0.0)); - firstJointPIDController.setD(firstJointD.valueOr(0.0)); - // FF was 0.002 - firstJointPIDController.setFF(firstJointFF.valueOr(0.001)); - // firstJointPIDController.setSmartMotionAllowedClosedLoopError(3, 0); - - // More PID constants - ValueProvider secondJointP = ConfigFileReader.getInstance().getDouble("arms.secondJointP"); - ValueProvider secondJointI = ConfigFileReader.getInstance().getDouble("arms.secondJointI"); - ValueProvider secondJointD = ConfigFileReader.getInstance().getDouble("arms.secondJointD"); - ValueProvider secondJointFF = ConfigFileReader.getInstance().getDouble("arms.secondJointFF"); - - // TODO: use secondJointO/I/D/FF.valueOr - secondJointPIDController.setP(0.0005); - secondJointPIDController.setI(secondJointI.valueOr(0.0)); - // D was 0.00001 - secondJointPIDController.setD(0.0000); - // FF was 0.00109 - secondJointPIDController.setFF(0.0008); - - // These next things deal a lot with the PID SmartMotion - firstJointCANSparkMax.setInverted(false); - // TODO : consider decrease velocity instead of decreasing power - firstJointPIDController.setSmartMotionMaxVelocity(4000, 0); - firstJointPIDController.setSmartMotionMinOutputVelocity(0, 0); - firstJointPIDController.setSmartMotionMaxAccel(3000, 0); - // firstJointPIDController.setOutputRange(-0.75, 0.75); - firstJointPIDController.setOutputRange(-0.65, 0.65); - firstJointCANSparkMax.setSmartCurrentLimit(40); - // Do not use setSmartMotionAllowedClosedLoopError(5, 0) unless it is safe to test without destorying anything - - // These too - secondJointPIDController.setSmartMotionMaxVelocity(4000, 0); - secondJointPIDController.setSmartMotionMinOutputVelocity(0, 0); - secondJointPIDController.setSmartMotionMaxAccel(3000, 0); - secondJointPIDController.setOutputRange(-1, 1); - secondJointCANSparkMax.setSmartCurrentLimit(40); - - // This resets the degrees and stuff so that we dont have to have the arm at certain positions to reset... - firstJointPIDController.setFeedbackDevice(firstJointCANSparkMax.getEncoder()); - secondJointPIDController.setFeedbackDevice(secondJointCANSparkMax.getEncoder()); - - // absolute encoder zero offsets are set to positions which will never be used - // to avoid wrap-around errors - // first joint zero is horizontal parallel to ground - // second joint zero is when the relative angle is 0 degrees from the first arm segment - altEncoder1.setZeroOffset(0.655); // TODO: these need tweaking from altEncoder1Offset - altEncoder2.setZeroOffset(0.627); - SmartDashboard.putNumber("Alt Encoder 1", altEncoder1.getPosition()); - SmartDashboard.putNumber("Alt Encoder 2", altEncoder2.getPosition()); - - antiGrav = new ArmsAntiGrav(firstJoint, secondJoint); - - // We only want to resetEncoders after configs are loaded, offsets are set, etc - resetFirstEncoders(); - resetSecondEncoders(); - } - - // This allows the pulley motor power to be changed, usually manually - // The magnitude ranges from 0.0-1.0, and sign (positive/negative) determines the direction - - // manual changing of arm 1 - public void manuallySetArmOnePower(double power) { - checkContextOwnership(); - firstJoint.set(power); - } - - // manual changing of arm 2. - public void manuallySetArmTwoPower(double power) { - checkContextOwnership(); - secondJoint.set(power); - } - - public void coast(){ - firstJoint.setNeutralMode(NeutralMode.Coast); - secondJoint.setNeutralMode(NeutralMode.Coast); - } - - public void brake(){ - firstJoint.setNeutralMode(NeutralMode.Brake); - secondJoint.setNeutralMode(NeutralMode.Brake); - } - public double nudgeArm2up(){ - return (ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) +1); - } - - public double nudgeArm2down(){ - return (ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) -1); - } - // Resets encoders - public void resetFirstEncoders() { - checkContextOwnership(); - - // TODO: this offset is to factor in the difference between the - // "zero" for alt encoder and the "zero" when we use degrees - // Offset tuning should be done above in `altEncoder1.setZeroOffset` - final double altEncoder1Offset = 0.25; - // final double altEncoder1Offset = 0.22calc5; - final double altEncoder2Offset = 0.5; - //final double altEncoder2Offset = 0.493; - - // altEncoder1Offset = what is the value of altEncoder1 when firstJoint is vertical - double firstJointAbsEncoder = altEncoder1.getPosition(); - double firstJointRelEncoder = ArmsUtil.AbsToEU(firstJointAbsEncoder - altEncoder1Offset); - - // set the sensor positions and setpoint of our rel encoders - firstJoint.setSensorPosition(firstJointRelEncoder); - firstJointPosition = ArmsUtil.EUTodegrees(firstJointRelEncoder); - } - - public void resetSecondEncoders() { - checkContextOwnership(); - - // TODO: this offset is to factor in the difference between the - // "zero" for alt encoder and the "zero" when we use degrees - // Offset tuning should be done above in `altEncoder1.setZeroOffset` - final double altEncoder1Offset = 0.25; - // final double altEncoder1Offset = 0.22calc5; - final double altEncoder2Offset = 0.5; - //final double altEncoder2Offset = 0.493; - - // altEncoder1Offset = what is the value of altEncoder1 when firstJoint is vertical - // altEncoder2Offset = what is the value of altEncoder2 when secondJoint is colinear w/firstJoint - - double firstJointAbsEncoder = altEncoder1.getPosition(); - double secondJointAbsEncoder = altEncoder2.getPosition(); - double secondJointRelEncoder = ArmsUtil.AbsToEU( - firstJointAbsEncoder - altEncoder1Offset - + secondJointAbsEncoder - altEncoder2Offset); - - // set the sensor positions and setpoint of our rel encoders - secondJoint.setSensorPosition(secondJointRelEncoder); - secondJointPosition = ArmsUtil.EUTodegrees(secondJointRelEncoder); - } - - public void armStop(){ - theStateOf1 = ArmState.OFF; - theStateOf2 = ArmState.OFF; - } - - // for first arm - /** - * Set PID for the first joint. - * - * @param value desired position in degrees. - */ - public void pidForArmOne(double value) { - // This will be run once - // log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - value = ArmsUtil.clampValueToRange(value, FIRST_JOINT_MIN_LOCATION, FIRST_JOINT_MAX_LOCATION); - - firstJointPosition = value; - // if(Math.abs(EUTodegrees(firstJoint.getSensorPosition() ))) - firstJointPIDController.setReference(ArmsUtil.degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getFirstJointPower()); - theStateOf1 = ArmState.PID; - firstJointCombo = 0; - - } - - // PID for second arm - public void pidForArmTwo(double value) { - // log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - - value = ArmsUtil.clampValueToRange(value, SECOND_JOINT_MIN_LOCATION, SECOND_JOINT_MAX_LOCATION); - - secondJointPosition = value; - secondJointPIDController.setReference( - ArmsUtil.degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getSecondJointPower()); - theStateOf2 = ArmState.PID; - secondJointCombo = 0; - - } - - // Use these for manual pid based angle increment/decrement - public void alterArmOneAngle(double degrees) { - pidForArmOne(firstJointPosition + degrees); - } - - public void alterArmTwoAngle(double degrees) { - pidForArmTwo(firstJointPosition + degrees); - } - - // PID For arm one - - // public void checkJointTwo(double value){ - // if((value + doubleDeadZone > secondJoint.getSensorPosition() && value - doubleDeadZone < secondJoint.getSensorPosition())){ - // jointOneCanContinue = true; - // }else{ - // jointOneCanContinue = false; - // } - // } - - //This is our portion with antigrav - // These next 3 antiGrav aren't used. - public void antiGravBothJoints(){ - antiGravFirstJoint(); - antiGravSecondJoint(); - } - - public void antiGravFirstJoint() { - antiGrav.updateFirstJoint(); - theStateOf1 = ArmState.ANTIGRAV; - } - - public void antiGravSecondJoint() { - antiGrav.updateSecondJoint(); - theStateOf2 = ArmState.ANTIGRAV; - } - - public void logs(){ - log("E1: " + ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - log("E2: " + ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - log("AE1: " + Math.toDegrees(altEncoder1.getPosition())); - log("AE2: " + Math.toDegrees(altEncoder2.getPosition())); - SmartDashboard.putNumber("Degree Val 1: ", ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - SmartDashboard.putNumber("Degree Val 2: ", ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - SmartDashboard.putNumber("Abs Encoder 1: ", Math.toDegrees(altEncoder1.getPosition())); - SmartDashboard.putNumber("Abs Encoder 2: ", Math.toDegrees(altEncoder2.getPosition())); - - } - - @Override - public void run() { - if(!runRateLimiter.next()) return; - IdleMode idleMode = ((CANSparkMax) secondJoint).getIdleMode(); - SmartDashboard.putString("Idle Mode", (idleMode != null) ? idleMode.toString(): "null"); - if (theStateOf1 == ArmState.PID || theStateOf2 == ArmState.PID) { - log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("First Joint Relative Encoder: " + firstJoint.getSensorPosition()); - // log("Second Joint Relative Encoder: " + secondJoint.getSensorPosition()); - // log("First Joint Difference: " + (EUTodegrees(firstJoint.getSensorPosition())-firstJointPosition)); - // log("Second Joint Difference: " + (EUTodegrees(secondJoint.getSensorPosition())-secondJointPosition)); - log("Degrees Joint 1: "+ ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - log("Degrees Joint 2: "+ ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - log("First Joint State: " + theStateOf1); - log("Second Joint State: " + theStateOf2); - log("First Joint Combo: " + firstJointCombo); - log("Second Joint Combo: " + secondJointCombo); - } - - // log("First Joint AntiGrav: "+getAntiGravFirstJoint()); - // log("Second Joint AntiGrav: "+getAntiGravSecondJoint()); - switch(theStateOf1) { - case OFF: - break; - case ANTIGRAV: - antiGravFirstJoint(); - break; - case PID: - firstJointPIDController.setReference( - ArmsUtil.degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getFirstJointPower()); - - if (Math.abs(ArmsUtil.EUTodegrees(firstJoint.getSensorPosition()) - firstJointPosition) <= doubleDeadZone){ - firstJointCombo ++; - } else { - firstJointCombo = 0; - } - - // TODO: we can actually remove this 'combo' logic since we have found that the lack of EUTodegrees made the deadzone calculation wonky - - if (firstJointCombo >= 6){ - firstJointCombo = 0; - // TODO: we do not want to do this here as arm may still be moving due to inertia - resetFirstEncoders(); - theStateOf1 = ArmState.ANTIGRAV; - } - - break; - } - - switch(theStateOf2) { - case OFF: - break; - case ANTIGRAV: - antiGravSecondJoint(); - break; - case PID: - secondJointPIDController.setReference( - ArmsUtil.degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getSecondJointPower()); - - if (Math.abs(ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) - secondJointPosition) <= doubleDeadZone){ - secondJointCombo ++; - } else { - secondJointCombo = 0; - } - - // TODO: we can actually remove this 'combo' logic since we have found that the lack of EUTodegrees made the deadzone calculation wonky - - if (secondJointCombo >= 6){ - secondJointCombo = 0; - // TODO: we do not want to do this here as arm may still be moving due to inertia - resetSecondEncoders(); - theStateOf2 = ArmState.ANTIGRAV; - } - - break; - } - - // log("First" + EUTodegrees(firstJoint.getSensorPosition()) ); - // log(" Second" + EUTodegrees(secondJoint.getSensorPosition())); - // log(theStateOf2 + ""); - // log("Difference: " + EUTodegrees(firstJoint.getSensorPosition())); - } -} - -/* ~~ Code Review ~~ - Use Voltage Control Mode when setting power (refer to CANSparkMaxMotorController.java) - Maybe use Nicholas's formula for degrees to EU - "Use break mode" - Ronald the not programmer - */ diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java b/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java deleted file mode 100644 index 93238e9b..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java +++ /dev/null @@ -1,67 +0,0 @@ -package com.team766.robot.mechanisms; - -import java.util.HashMap; - -public class ArmsAnimation { - - public enum AnimationState { - DISABLED, // valid next states (depending on STOW or scoring): UNSTOWED or PREP - STOWED, // valid next states: UNSTOWED - UNSTOWED, // valid next states: STOWED, PREP - PREP, // valid next states: UNSTOWED, PLAYER_STATION_CONE, MID_GOAL, HIGH_GOAL - PLAYER_STATION_CONE, // valid next states: PREP - MID_GOAL, // valid next states: PREP - HIGH_GOAL // valid next states: PREP - } - - public enum ProceedCondition { - BOTH_JOINTS_REACHED, - JOINT_ONE_REACHED_ONLY, - JOINT_TWO_REACHED_ONLY - } - - public class Waypoint { - public double jointOneAngleDegrees; - public double jointTwoAngleDegrees; - public ProceedCondition proceedCondition; - public double entryVelocity; - - public Waypoint(double jointOneAngleDegrees, double jointTwoAngleDegrees, ProceedCondition proceedCondition, double entryVelocity) { - this.jointOneAngleDegrees = jointOneAngleDegrees; - this.jointTwoAngleDegrees = jointTwoAngleDegrees; - this.proceedCondition = proceedCondition; - this.entryVelocity = entryVelocity; - } - } - - private AnimationState currentAnimationState = AnimationState.DISABLED; - private AnimationState targetAnimationState = AnimationState.DISABLED; - - public final HashMap waypoints = new HashMap(); - - public ArmsAnimation() { - // TODO: these are just estimates, calibrate on before using otherwise something may break :D - //waypoints.put(AnimationState.DISABLED, null); - //waypoints.put(AnimationState.STOWED, new Waypoint(0, -150, ProceedCondition.BOTH_JOINTS_REACHED, 500 )); - waypoints.put(AnimationState.UNSTOWED, new Waypoint(17.269, -144.387,ProceedCondition.JOINT_ONE_REACHED_ONLY, 3000)); - waypoints.put(AnimationState.PREP, new Waypoint(17.269, -90, ProceedCondition.BOTH_JOINTS_REACHED, 4000)); - waypoints.put(AnimationState.PLAYER_STATION_CONE, new Waypoint(22.73, -69.664, ProceedCondition.BOTH_JOINTS_REACHED, 3000)); - waypoints.put(AnimationState.MID_GOAL, new Waypoint(7.7765, -88.703, ProceedCondition.BOTH_JOINTS_REACHED, 3000)); - waypoints.put(AnimationState.HIGH_GOAL, new Waypoint(-17.379, -66.61, ProceedCondition. BOTH_JOINTS_REACHED, 3000)); - } - - public void update() { - // TODO - // ensure that position for desired state is kept - } - - public void changeState(AnimationState targetState) { - // valid states: DISABLED, STOWED, PLAYER_STATION_CONE, MID_GOAL, HIGH_GOAL - // switch(targetState) { - // case - // } - - // TODO : ensure that state transitions are allowed - // TODO : special case for stowing, to keep applying power - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java b/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java deleted file mode 100644 index 0c8260fe..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java +++ /dev/null @@ -1,55 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.config.ConfigFileReader; -import com.team766.hal.MotorController; -import com.team766.library.ValueProvider; - -public class ArmsAntiGrav { - - private ValueProvider ANTI_GRAV_FIRST_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravFirstJoint"); - private ValueProvider ANTI_GRAV_SECOND_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravSecondJoint"); - private static final double ANTI_GRAV_FIRSTSECOND_JOINT = 0.001; - - private MotorController firstJoint; - private MotorController secondJoint; - - public ArmsAntiGrav(MotorController j1, MotorController j2) { - firstJoint = j1; - secondJoint = j2; - } - - public void updateFirstJoint() { - firstJoint.set(getFirstJointPower()); - } - - public void updateSecondJoint() { - secondJoint.set(getSecondJointPower()); - } - - // Unused - /*private double betterGetAntiGravFirstJoint(){ - double firstRelEncoderAngle = EUTodegrees(firstJoint.getSensorPosition()); - double secondRelEncoderAngle = EUTodegrees(secondJoint.getSensorPosition()); - double massRatio = 2; //ratio between firstJoint and secondJoint - double triangleSide1 = 38; // firstJoint length - double triangleSide2 = 38; // half secondJoint length - double middleAngle = 180-(secondRelEncoderAngle-firstRelEncoderAngle); - double triangleSide3 = lawOfCosines(triangleSide1,triangleSide2,middleAngle); - double firstSecondJointAngle = firstRelEncoderAngle+lawOfSines(triangleSide3,middleAngle,triangleSide2); - double firstJointAngle = 90-Math.abs(firstRelEncoderAngle); - return (-1*Math.signum(firstRelEncoderAngle) * Math.cos((Math.PI / 180) * firstJointAngle) * ANTI_GRAV_FIRST_JOINT.valueOr(0.0)) + (-1*Math.signum(firstSecondJointAngle)*triangleSide3 * Math.sin((Math.PI / 180)*firstSecondJointAngle) * ANTI_GRAV_FIRSTSECOND_JOINT); - }*/ - - public double getFirstJointPower() { - double firstRelEncoderAngle = ArmsUtil.EUTodegrees(firstJoint.getSensorPosition()); - double firstJointAngle = 90-Math.abs(firstRelEncoderAngle); - return -1*Math.signum(firstRelEncoderAngle) * (Math.cos((Math.PI / 180) * firstJointAngle) * ANTI_GRAV_FIRST_JOINT.valueOr(0.0)); - } - - public double getSecondJointPower() { - double secondRelEncoderAngle = ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()); - double secondJointAngle = 90-Math.abs(secondRelEncoderAngle); - return -1*Math.signum(secondRelEncoderAngle) * (Math.cos((Math.PI / 180) * secondJointAngle) * ANTI_GRAV_SECOND_JOINT.valueOr(0.0)); - } - -} diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java b/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java deleted file mode 100644 index ec731d24..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java +++ /dev/null @@ -1,58 +0,0 @@ -package com.team766.robot.mechanisms; - -/** - * Helper classes - */ -public final class ArmsUtil { - - /** - * Static class only - */ - private ArmsUtil() { - } - - public static double degreesToEU(double angle) { - // return angle * (44.0 / 90) * (25.0 / 16.0); - // Nicholas' accuracy fix - return angle * (75.0 / 94.0); - } - - public static double EUTodegrees(double EU) { - // return EU * (90 / 44.0) * (16.0 / 25.0); - // Nicholas' accuracy fix - return EU * (94.0 / 75.0); - } - - public static double AbsToEU(double abs) { - return degreesToEU(360 * abs); - } - - public static double EUToAbs(double EU) { - return EUTodegrees(EU) / 360.0; - } - - /** - * Cosine law - * @param side1 - * @param side2 - * @param angle in degrees - * @return - */ - public static double lawOfCosines(double side1, double side2, double angle) { - double side3Squared = (Math.pow(side1,2.0)+Math.pow(side2,2.0)-2*side1*side2*Math.cos(Math.toRadians(angle))); - return Math.sqrt(side3Squared); - } - - public static double lawOfSines(double side1, double angle1, double side2) { - return Math.asin(side2*Math.sin(angle1)/side1); - } - - public static double clampValueToRange(double value, double min, double max) { - if(value > max){ - value = max; - } else if( value < min){ - value = min; - } - return value; - } -} diff --git a/src/main/java/com/team766/robot/mechanisms/CANdleMech.java b/src/main/java/com/team766/robot/mechanisms/CANdleMech.java deleted file mode 100644 index 1b8f6788..00000000 --- a/src/main/java/com/team766/robot/mechanisms/CANdleMech.java +++ /dev/null @@ -1,50 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.RobotProvider; -import com.team766.logging.Severity; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.util.Color8Bit; - -import com.team766.hal.MotorController; - -import java.io.*; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.util.*; - -import javax.imageio.ImageIO; - -import com.ctre.phoenix.led.*; -import com.ctre.phoenix.led.CANdle.LEDStripType; -import com.ctre.phoenix.led.CANdle.VBatOutputMode; -import com.ctre.phoenix.led.ColorFlowAnimation.Direction; -import com.ctre.phoenix.led.LarsonAnimation.BounceMode; -import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; -import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent; -import java.io.File; -import java.io.IOException; -import java.nio.file.Files; -import java.awt.image.BufferedImage; -import java.awt.image.Raster; - -public class CANdleMech extends Mechanism { - - private final CANdle m_candle = new CANdle(5); - - public CANdleMech() { - - } - - public void setColor(short r, short g, short b, int index, int count) { - checkContextOwnership(); - m_candle.setLEDs(r / 5, g / 5, b / 5, 0, index, count); - } - - public void setColor(int r, int g, int b) { - checkContextOwnership(); - m_candle.setLEDs(r / 5, g / 5, b / 5); - } - -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java deleted file mode 100644 index 8bad93c9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ /dev/null @@ -1,676 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.EncoderReader; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; -import com.team766.hal.MotorController.ControlMode; -import com.team766.hal.simulator.Encoder; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.logging.Category; -import com.team766.simulator.ProgramInterface.RobotMode; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import com.team766.config.ConfigFileReader; -import com.team766.framework.Mechanism; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderConfiguration; -import com.team766.odometry.Odometry; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.hal.MotorControllerCommandFailedException; -import com.team766.robot.constants.*; - -public class Drive extends Mechanism { - - private MotorController m_DriveFrontRight; - private MotorController m_DriveFrontLeft; - private MotorController m_DriveBackRight; - private MotorController m_DriveBackLeft; - - private MotorController m_SteerFrontRight; - private MotorController m_SteerFrontLeft; - private MotorController m_SteerBackRight; - private MotorController m_SteerBackLeft; - - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - private ValueProvider drivePower; - - private double gyroValue; - - private static PointDir currentPosition; - - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - private Odometry swerveOdometry; - - public Drive() { - - loggerCategory = Category.DRIVE; - // Initializations of motors - // Initialize the drive motors - m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); - m_DriveFrontLeft = RobotProvider.instance.getMotor("drive.DriveFrontLeft"); - m_DriveBackRight = RobotProvider.instance.getMotor("drive.DriveBackRight"); - m_DriveBackLeft = RobotProvider.instance.getMotor("drive.DriveBackLeft"); - // Initialize the steering motors - m_SteerFrontRight = RobotProvider.instance.getMotor("drive.SteerFrontRight"); - m_SteerFrontLeft = RobotProvider.instance.getMotor("drive.SteerFrontLeft"); - m_SteerBackRight = RobotProvider.instance.getMotor("drive.SteerBackRight"); - m_SteerBackLeft = RobotProvider.instance.getMotor("drive.SteerBackLeft"); - - // Setting up the "config" - CANCoderConfiguration config = new CANCoderConfiguration(); - config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; - // The encoders output "encoder" values, so we need to convert that to degrees (because that - // is what the cool kids are using) - config.sensorCoefficient = 360.0 / 4096.0; - // The offset is going to be changed in ctre, but we can change it here too. - // config.magnetOffsetDegrees = Math.toDegrees(configuration.getOffset()); - config.sensorDirection = true; - - // initialize the encoders - e_FrontRight = new CANCoder(22, "Swervavore"); - // e_FrontRight.configAllSettings(config, 250); - e_FrontLeft = new CANCoder(23, "Swervavore"); - // e_FrontLeft.configAllSettings(config, 250); - e_BackRight = new CANCoder(21, "Swervavore"); - // e_BackRight.configAllSettings(config, 250); - e_BackLeft = new CANCoder(24, "Swervavore"); - // e_BackLeft.configAllSettings(config, 250); - - - // Current limit for motors to avoid breaker problems (mostly to avoid getting electrical - // people to yell at us) - m_DriveFrontRight.setCurrentLimit(35); - m_DriveFrontLeft.setCurrentLimit(35); - m_DriveBackRight.setCurrentLimit(35); - m_DriveBackLeft.setCurrentLimit(35); - m_DriveBackLeft.setInverted(true); - m_DriveBackRight.setInverted(true); - m_SteerFrontRight.setCurrentLimit(30); - m_SteerFrontLeft.setCurrentLimit(30); - m_SteerBackRight.setCurrentLimit(30); - m_SteerBackLeft.setCurrentLimit(30); - - // Setting up the connection between steering motors and cancoders - // m_SteerFrontRight.setRemoteFeedbackSensor(e_FrontRight, 0); - // m_SteerFrontLeft.setRemoteFeedbackSensor(e_FrontLeft, 0); - // m_SteerBackRight.setRemoteFeedbackSensor(e_BackRight, 0); - // m_SteerBackLeft.setRemoteFeedbackSensor(e_BackLeft, 0); - - m_SteerFrontRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerFrontLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerBackRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerBackLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - configPID(); - - // Sets up odometry - currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFrontRight, m_DriveFrontLeft, m_DriveBackLeft, - m_DriveBackRight}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = - new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; - log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); - swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - setFrontRightEncoders(); - setFrontLeftEncoders(); - setBackRightEncoders(); - setBackLeftEncoders(); - } - - // A set of simple functions for the sake of adding vectors - /** - * Returns the pythagorean theorem of two numbers - * - * @param x First number - * @param y Second number - * @return - */ - public double pythagorean(double x, double y) { - return Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); - } - - /** - * Returns the angle of a vector, used by joystick inputs - * - * @param LR The x value of the vector - * @param FB The y value of the vector - * @return The angle of the vector - */ - public double getAngle(double LR, double FB) { - return Math.toDegrees(Math.atan2(LR, -FB)); - } - - /** - * Returns whether two angles are within 90 degrees of each other, used to see if the wheels - * should move backwards or not - * - * @param angle1 The first angle - * @param angle2 The second angle - * @return If they are within 90 degrees of each other - */ - public boolean withinHalfACircle(double angle1, double angle2) { - angle1 = mod(angle1, 360); - angle2 = mod(angle2, 360); - if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) { - angle2 += 360; - } - if (Math.abs(angle2 - angle1) > Math.abs(angle2 - 360 - angle1)) { - angle2 -= 360; - } - return Math.abs(angle2 - angle1) <= 90; - } - - // Returns mod(d1, d2), to use to circumvent java's weird % function - private static double mod(double d1, double d2) { - return d1 % d2 + (d1 < 0 ? d2 : 0); - } - - /** - * Rounds a number based on its value and places - * - * @param value The number to be rounded - * @param places The number of places to round to - * @return The rounded number - */ - public double round(double value, int places) { - double scale = Math.pow(10, places); - return Math.round(value * scale) / scale; - } - - /** - * Adds two vectors together - * - * @param FirstMag The magnitude of the first vector - * @param FirstAng The angle of the first vector - * @param SecondMag The magnitude of the second vector - * @param SecondAng The angle of the second vector - * @return New angle of the vector - */ - public double NewAng(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { - double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) - + SecondMag * Math.cos(Math.toRadians(SecondAng)); - double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) - + SecondMag * Math.sin(Math.toRadians(SecondAng)); - return round(Math.toDegrees(Math.atan2(FinalY, FinalX)), 5); - } - - /** - * Adds two vectors together - * - * @param FirstMag The magnitude of the first vector - * @param FirstAng The angle of the first vector - * @param SecondMag The magnitude of the second vector - * @param SecondAng The angle of the second vector - * @return New magnitude of the vector - */ - public double NewMag(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { - double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) - + SecondMag * Math.cos(Math.toRadians(SecondAng)); - double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) - + SecondMag * Math.sin(Math.toRadians(SecondAng)); - return round(Math.sqrt(Math.pow(FinalX, 2) + Math.pow(FinalY, 2)), 5); - } - - /** - * Corrects the joystick inputs to make them more accurate, currently unused - * - * @param Joystick The joystick value to be corrected - * @returnThe corrected joystick value - */ - public static double correctedJoysticks(double Joystick) { - if (Joystick >= 0) { - return (3.0 * Math.pow(Joystick, 2) - 2.0 * Math.pow(Joystick, 3)); - } else { - return (-1 * 3.0 * Math.pow(-1 * Joystick, 2) + 2.0 * Math.pow(-1 * Joystick, 3)); - } - } - - /** - * Converts the angle of the joystick to the angle of the robot compared to the field by using - * the gyro - * - * @param angle The angle of the joystick - * @param gyro The angle of the gyro - * @return The angle of the robot compared to the field - */ - public static double fieldAngle(double angle, double gyro) { - double newAngle; - newAngle = angle - gyro; - if (newAngle < 0) { - newAngle = newAngle + 360; - } - if (newAngle >= 180) { - newAngle = newAngle - 360; - } - return newAngle; - } - - /** - * Method to correct angles for the falcon encoders - * - * @param newAngle The given angle value - * @param lastAngle The last angle value - * @return The corrected angle value - */ - public static double newAngle(double newAngle, double lastAngle) { - while (newAngle < 0) - newAngle += 360; - while (newAngle < (lastAngle - 180)) - newAngle += 360; - while (newAngle > (lastAngle + 180)) - newAngle -= 360; - return newAngle; - } - - /** - * Sets the gyro value, used to switch between field and robot orientation - * - * @param value The value to set the gyro to - */ - public void setGyro(double value) { - gyroValue = value; - } - - /** - * This method is used to drive the robot in 2D without turning, using the joystick values. - * - * @param JoystickX The x value of the joystick - * @param JoystickY The y value of the joystick - */ - public void drive2D(double JoystickX, double JoystickY) { - checkContextOwnership(); - // double power = pythagorean((JoystickX), correctedJoysticks(JoystickY))/Math.sqrt(2); - double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); - double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); - - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontRight))) { - m_DriveFrontRight.set(power); - setFrontRightAngle(newAngle(angle, getCurrentAngle(m_SteerFrontRight))); - } else { - m_DriveFrontRight.set(-power); - setFrontRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontRight))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontLeft))) { - m_DriveFrontLeft.set(power); - setFrontLeftAngle(newAngle(angle, getCurrentAngle(m_SteerFrontLeft))); - } else { - m_DriveFrontLeft.set(-power); - setFrontLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontLeft))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackRight))) { - m_DriveBackRight.set(power); - setBackRightAngle(newAngle(angle, getCurrentAngle(m_SteerBackRight))); - } else { - m_DriveBackRight.set(-power); - setBackRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackRight))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackLeft))) { - m_DriveBackLeft.set(power); - setBackLeftAngle(newAngle(angle, getCurrentAngle(m_SteerBackLeft))); - } else { - m_DriveBackLeft.set(-power); - setBackLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackLeft))); - } - } - - /** - * This method is used to drive the robot in 2D without turning, using a point. - * - * @param joystick The point to use for the joystick values - */ - public void drive2D(Point joystick) { - drive2D(joystick.getX(), joystick.getY()); - } - - /** - * This method stops all of the drive motors - */ - public void stopDriveMotors() { - checkContextOwnership(); - m_DriveFrontRight.stopMotor(); - m_DriveFrontLeft.stopMotor(); - m_DriveBackRight.stopMotor(); - m_DriveBackLeft.stopMotor(); - } - - /** - * This method stops all of the steer motors - */ - public void stopSteerMotors() { - checkContextOwnership(); - m_SteerFrontRight.stopMotor(); - m_SteerFrontLeft.stopMotor(); - m_SteerBackRight.stopMotor(); - m_SteerBackLeft.stopMotor(); - } - - /** - * This method is the main method for driving the robot, using the joystick values. - * - * @param JoystickX The x value of the joystick - * @param JoystickY The y value of the joystick - * @param JoystickTheta The theta value of the joystick (for turning) - */ - public void swerveDrive(double JoystickX, double JoystickY, double JoystickTheta) { - checkContextOwnership(); - double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); - double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); - double frPower; - double flPower; - double brPower; - double blPower; - double frAngle; - double flAngle; - double brAngle; - double blAngle; - if (JoystickTheta >= 0) { - frPower = NewMag(power, angle, JoystickTheta, 45); - flPower = NewMag(power, angle, JoystickTheta, -45); - brPower = NewMag(power, angle, JoystickTheta, 135); - blPower = NewMag(power, angle, JoystickTheta, -135); - frAngle = NewAng(power, angle, JoystickTheta, 45); - flAngle = NewAng(power, angle, JoystickTheta, 135); - brAngle = NewAng(power, angle, JoystickTheta, -45); - blAngle = NewAng(power, angle, JoystickTheta, -135); - } else { - frPower = NewMag(power, angle, Math.abs(JoystickTheta), -135); - flPower = NewMag(power, angle, Math.abs(JoystickTheta), 135); - brPower = NewMag(power, angle, Math.abs(JoystickTheta), -45); - blPower = NewMag(power, angle, Math.abs(JoystickTheta), 45); - frAngle = NewAng(power, angle, Math.abs(JoystickTheta), -135); - flAngle = NewAng(power, angle, Math.abs(JoystickTheta), -45); - brAngle = NewAng(power, angle, Math.abs(JoystickTheta), 135); - blAngle = NewAng(power, angle, Math.abs(JoystickTheta), 45); - } - if (Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)) > 1) { - frPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - flPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - brPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - blPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - } - - if (withinHalfACircle(frAngle, getCurrentAngle(m_SteerFrontRight))) { - m_DriveFrontRight.set(frPower); - setFrontRightAngle(newAngle(frAngle, getCurrentAngle(m_SteerFrontRight))); - } else { - m_DriveFrontRight.set(-frPower); - setFrontRightAngle(newAngle(180 + frAngle, getCurrentAngle(m_SteerFrontRight))); - } - - if (withinHalfACircle(flAngle, getCurrentAngle(m_SteerFrontLeft))) { - m_DriveFrontLeft.set(flPower); - setFrontLeftAngle(newAngle(flAngle, getCurrentAngle(m_SteerFrontLeft))); - } else { - m_DriveFrontLeft.set(-flPower); - setFrontLeftAngle(newAngle(180 + flAngle, getCurrentAngle(m_SteerFrontLeft))); - } - - if (withinHalfACircle(brAngle, getCurrentAngle(m_SteerBackRight))) { - m_DriveBackRight.set(brPower); - setBackRightAngle(newAngle(brAngle, getCurrentAngle(m_SteerBackRight))); - } else { - m_DriveBackRight.set(-brPower); - setBackRightAngle(newAngle(180 + brAngle, getCurrentAngle(m_SteerBackRight))); - } - - if (withinHalfACircle(blAngle, getCurrentAngle(m_SteerBackLeft))) { - m_DriveBackLeft.set(blPower); - setBackLeftAngle(newAngle(blAngle, getCurrentAngle(m_SteerBackLeft))); - } else { - m_DriveBackLeft.set(-blPower); - setBackLeftAngle(newAngle(180 + blAngle, getCurrentAngle(m_SteerBackLeft))); - } - } - - /** - * This method is used to drive the robot with swerve using a PointDir - * - * @param joystick The PointDir to use for the joystick values - */ - public void swerveDrive(PointDir joystick) { - swerveDrive(-1 * joystick.getY(), -1 * joystick.getX(), joystick.getHeading()); - } - - /** - * This method is used to simply turn the robot without moving it - * - * @param Joystick The joystick value to use for turning - */ - public void turning(double Joystick) { - checkContextOwnership(); - if (Joystick > 0) { - setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); - setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); - setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); - setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); - m_DriveFrontRight.set(Math.abs(Joystick)); - m_DriveFrontLeft.set(Math.abs(Joystick)); - m_DriveBackRight.set(Math.abs(Joystick)); - m_DriveBackLeft.set(Math.abs(Joystick)); - } - if (Joystick < 0) { - setFrontRightAngle(newAngle(-45, getCurrentAngle(m_SteerFrontRight))); - setFrontLeftAngle(newAngle(-135, getCurrentAngle(m_SteerFrontLeft))); - setBackRightAngle(newAngle(45, getCurrentAngle(m_SteerBackRight))); - setBackLeftAngle(newAngle(135, getCurrentAngle(m_SteerBackLeft))); - m_DriveFrontRight.set(Math.abs(Joystick)); - m_DriveFrontLeft.set(Math.abs(Joystick)); - m_DriveBackRight.set(Math.abs(Joystick)); - m_DriveBackLeft.set(Math.abs(Joystick)); - } - } - - private double getCurrentAngle(MotorController motor) { - return Math.pow((2048.0 / 360.0 * (150.0 / 7.0)), -1) * motor.getSensorPosition(); - } - - /** - * Simple encoder logging method - */ - public void logs() { - /*log("Front Right Encoder: " + getFrontRight() + " Front Left Encoder: " + getFrontLeft() - + " Back Right Encoder: " + getBackRight() + " Back Left Encoder: " - + getBackLeft());*/ - } - - /** - * This method is used to set the front right encoder to the true position - */ - public void setFrontRightEncoders() { - //log("Steer FR Before: " + m_SteerFrontRight.getSensorPosition()); - m_SteerFrontRight.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontRight.getAbsolutePosition())); - //log("Steer FR After: " + m_SteerFrontRight.getSensorPosition()); - - } - - /** - * This method is used to set the front left encoder to the true position - */ - public void setFrontLeftEncoders() { - m_SteerFrontLeft.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontLeft.getAbsolutePosition())); - - } - - /** - * This method is used to set the back right encoder to the true position - */ - public void setBackRightEncoders() { - m_SteerBackRight.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackRight.getAbsolutePosition())); - } - - /** - * This method is used to set the back left encoder to the true position - */ - public void setBackLeftEncoders() { - m_SteerBackLeft.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackLeft.getAbsolutePosition())); - } - - // To control each steering individually with a PID - - /** - * This method is used to set the front right steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the front right wheel to - */ - public void setFrontRightAngle(double angle) { - // log("Angle: " + getFrontRight() + " || Motor angle: " + 360.0/ 2048.0 * - // m_SteerFrontRight.getSensorPosition()); - m_SteerFrontRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the front left steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the front left wheel to - */ - public void setFrontLeftAngle(double angle) { - // log("Angle: " + getFrontLeft() + " || Motor angle: " + Math.pow((2048.0/360.0 * - // (150.0/7.0)),-1) * m_SteerFrontLeft.getSensorPosition()); - m_SteerFrontLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the back right steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the back right wheel to - */ - public void setBackRightAngle(double angle) { - // log("Angle: " + getBackRight() + " || Motor angle: " + - // m_SteerBackRight.getSensorPosition()); - m_SteerBackRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the back left steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the back left wheel to - */ - public void setBackLeftAngle(double angle) { - // log("Angle: " + getBackLeft() + " || Motor angle: " + - // m_SteerBackLeft.getSensorPosition()); - m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * Method to configure PID values. The values were pre-tuned and are not expected to change. - */ - public void configPID() { - // PID for turning the various steering motors. Here is a good link to a tuning website: - // https://www.robotsforroboticists.com/pid-control/ - m_SteerFrontRight.setP(0.2); - m_SteerFrontRight.setI(0); - m_SteerFrontRight.setD(0.1); - m_SteerFrontRight.setFF(0); - - m_SteerFrontLeft.setP(0.2); - m_SteerFrontLeft.setI(0); - m_SteerFrontLeft.setD(0.1); - m_SteerFrontLeft.setFF(0); - - m_SteerBackRight.setP(0.2); - m_SteerBackRight.setI(0); - m_SteerBackRight.setD(0.1); - m_SteerBackRight.setFF(0); - - m_SteerBackLeft.setP(0.2); - m_SteerBackLeft.setI(0); - m_SteerBackLeft.setD(0.1); - // m_SteerBackLeft.setFF(0); - - // pid values from sds for Flacons 500: P = 0.2 I = 0.0 D = 0.1 FF = 0.0 - - // Code to invert sensors if needed. Recommended to do this in phoenix tuner. - // m_SteerFrontRight.setSensorInverted(false); - // m_SteerFrontLeft.setSensorInverted(false); - // m_SteerBackRight.setSensorInverted(false); - // m_SteerBackLeft.setSensorInverted(false); - } - - // Methods to get the encoder values, the encoders are in degrees from -180 to 180. To change - // that, we need to change the syntax and use getPosition() - public double getFrontRight() { - return e_FrontRight.getAbsolutePosition(); - } - - public double getFrontLeft() { - return e_FrontLeft.getAbsolutePosition(); - } - - public double getBackRight() { - return e_BackRight.getAbsolutePosition(); - } - - public double getBackLeft() { - return e_BackLeft.getAbsolutePosition(); - } - - public PointDir getCurrentPosition() { - return currentPosition; - } - - public void setCross() { - checkContextOwnership(); - setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); - setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); - setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); - setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); - } - - public void setCurrentPosition(Point P) { - swerveOdometry.setCurrentPosition(P); - } - - public void resetCurrentPosition() { - swerveOdometry.setCurrentPosition(new Point(0, 0)); - } - - /** - * This method is used to reset the drive encoders. - */ - public void resetDriveEncoders() { - m_DriveBackLeft.setSensorPosition(0); - m_DriveBackRight.setSensorPosition(0); - m_DriveFrontLeft.setSensorPosition(0); - m_DriveFrontRight.setSensorPosition(0); - } - - // Odometry - @Override - public void run() { - currentPosition = swerveOdometry.run(); - log(currentPosition.toString()); - SmartDashboard.putNumber("Front Right Motor Encoder", m_SteerFrontRight.getSensorPosition()); - SmartDashboard.putNumber("Front Left Motor Encoder", m_SteerFrontLeft.getSensorPosition()); - SmartDashboard.putNumber("Back Right Motor Encoder", m_SteerBackRight.getSensorPosition()); - SmartDashboard.putNumber("Back Left Motor Encoder", m_SteerBackLeft.getSensorPosition()); - } -} - -// AS diff --git a/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java b/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java deleted file mode 100644 index 75f24df9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java +++ /dev/null @@ -1,22 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; - -public class ExampleMechanism extends Mechanism { - private MotorController leftMotor; - private MotorController rightMotor; - - public ExampleMechanism() { - leftMotor = RobotProvider.instance.getMotor("exampleMechanism.leftMotor"); - rightMotor = RobotProvider.instance.getMotor("exampleMechanism.rightMotor"); - } - - public void setMotorPower(double leftPower, double rightPower){ - checkContextOwnership(); - - leftMotor.set(leftPower); - rightMotor.set(rightPower); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Grabber.java b/src/main/java/com/team766/robot/mechanisms/Grabber.java deleted file mode 100644 index 4befedb7..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Grabber.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; - -public class Grabber extends Mechanism { - private MotorController grabby; - - public Grabber(){ - grabby = RobotProvider.instance.getMotor("arms.grabber"); - grabby.setCurrentLimit(10.0); - } - - public void grabberPickUp(){ - checkContextOwnership(); - grabby.set(1.0); - - } - - public void grabberLetGo(){ - checkContextOwnership(); - grabby.set(-0.5); - } - - public void grabberStop(){ - checkContextOwnership(); - grabby.set(0.0); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Gyro.java b/src/main/java/com/team766/robot/mechanisms/Gyro.java deleted file mode 100644 index d96ec9b9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Gyro.java +++ /dev/null @@ -1,68 +0,0 @@ -package com.team766.robot.mechanisms; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.I2C.Port; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import com.team766.framework.Mechanism; -import com.team766.hal.EncoderReader; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import com.team766.hal.MotorController; -import com.team766.logging.Category; -//import edu.wpi.first.wpilibj.I2C.Port; -//import com.team766.hal.GyroReader; -//import com.kauailabs.navx.frc.*; -import com.ctre.phoenix.sensors.Pigeon2; - -public class Gyro extends Mechanism { - Pigeon2 g_gyro = new Pigeon2(0, "Swervavore"); - double[] gyroArray = new double[3]; - private RateLimiter l_loggingRate = new RateLimiter(0.05); - public Gyro() { - loggerCategory = Category.GYRO; - } - public void resetGyro(){ - g_gyro.setYaw(0); - } - - public void resetGyro180() { - g_gyro.setYaw(180); - } - - public double getGyroPitch() { - double angle = g_gyro.getPitch(); - return angle; - } - - public double getGyroYaw() { - double angle = -1* g_gyro.getYaw(); - return angle; - } - - public double getGyroRoll() { - double angle = g_gyro.getRoll(); - return angle; - } - - @Override - public void run() { - if (l_loggingRate.next()) { - gyroArray[0] = getGyroYaw(); - gyroArray[1] = getGyroPitch(); - gyroArray[2] = getGyroRoll(); - SmartDashboard.putNumber("Yaw", gyroArray[0]); - SmartDashboard.putNumber("Pitch", gyroArray[1]); - SmartDashboard.putNumber("Roll", gyroArray[2]); - g_gyro.getYawPitchRoll(gyroArray); - } - } - - /** - * @return combined pitch and roll values of gyro - */ - public double getAbsoluteTilt() { - return Math.toDegrees(Math.acos(Math.cos(Math.toRadians(getGyroRoll()))*Math.cos(Math.toRadians(getGyroPitch())))); - } - -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java deleted file mode 100644 index 5fec31d6..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Intake.java +++ /dev/null @@ -1,79 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; -import com.team766.hal.SolenoidController; -import com.team766.hal.wpilib.CANSparkMaxMotorController; -import com.team766.hal.wpilib.Solenoid; -import edu.wpi.first.wpilibj.PneumaticHub; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Intake extends Mechanism { - - private MotorController bottomWheels; - private MotorController topBelt; - private SolenoidController leftPiston; - private SolenoidController rightPiston; - private PneumaticHub ph; - - public Intake() { - topBelt = RobotProvider.instance.getMotor("intake.topWheels"); - bottomWheels = RobotProvider.instance.getMotor("intake.bottomWheels"); - - leftPiston = RobotProvider.instance.getSolenoid("intake.leftPiston"); - rightPiston = RobotProvider.instance.getSolenoid("intake.rightPiston"); - ph = new PneumaticHub(); - - } - - public void startIntake() { - checkContextOwnership(); - - pistonsOut(); - topBelt.set(1.0); - bottomWheels.set(1.0); - - } - - public void intakePistonless() { - topBelt.set(1.0); - bottomWheels.set(1.0); - } - - public void stopIntake() { - checkContextOwnership(); - - topBelt.set(0.0); - bottomWheels.set(0.0); - pistonsIn(); - } - - public void pistonsOut() { - checkContextOwnership(); - log("pistons out"); - - leftPiston.set(true); - rightPiston.set(true); - } - - public void pistonsIn() { - checkContextOwnership(); - - leftPiston.set(false); - rightPiston.set(false); - } - - public void reverseIntake() { - checkContextOwnership(); - - topBelt.set(-1.0); - bottomWheels.set(-1.0); - - pistonsIn(); - } - - public void run(){ - SmartDashboard.putNumber("Storage PSI",ph.getPressure(0)); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 13c42e76..abbc0767 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -13,6 +13,7 @@ import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; import com.team766.library.ValueProvider; +import com.team766.logging.Category; //import com.team766.logging.Category; import de.erichseifert.gral.util.GeometryUtils; import de.erichseifert.gral.util.MathUtils; @@ -53,6 +54,7 @@ public class januaryTag extends Mechanism{ private double offsetY; private Transform3d offset; public januaryTag(){ + loggerCategory = Category.MECHANISMS; camera1 = new PhotonCamera("januaryTag.camera1"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); rightMotor = RobotProvider.instance.getMotor("rightMotor"); @@ -64,6 +66,7 @@ public januaryTag(){ offsetX = 10; offsetY = 10; offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); + checkContextOwnership(); } @@ -203,7 +206,9 @@ public void go(){ } public void debugLogs(){ - + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + log("Yaw: " + targetTransform.getX()); + log("Pitch: " + targetTransform.getY()); } } diff --git a/src/main/java/com/team766/robot/procedures/AlignCharger.java b/src/main/java/com/team766/robot/procedures/AlignCharger.java deleted file mode 100644 index 90cc2a3b..00000000 --- a/src/main/java/com/team766/robot/procedures/AlignCharger.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.framework.Context; -import com.team766.robot.Robot; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// Procedure to align robot to a position where it can easily balance on the charge station -// See AlignCharger.md for more details -public class AlignCharger extends Procedure { - - private Alliance alliance; - - - - public void run(Context context) { - context.takeOwnership(Robot.drive); - // Uses the ChargeStationPathFinder helper class to find correct points - ChargeStationPathFinder finder = new ChargeStationPathFinder(alliance); - PointDir[] points = finder.calculatePoints(Robot.drive.getCurrentPosition()); - - // Calls FollowPoints on the calculated points - context.startAsync(new FollowPoints(points)); - } - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public AlignCharger(Alliance alliance) { - this.alliance = alliance; - } - - -} diff --git a/src/main/java/com/team766/robot/procedures/AutoScoring.java b/src/main/java/com/team766/robot/procedures/AutoScoring.java deleted file mode 100644 index a68a5f1f..00000000 --- a/src/main/java/com/team766/robot/procedures/AutoScoring.java +++ /dev/null @@ -1,86 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.robot.procedures.FollowPoints; -import com.team766.robot.Robot; -import com.team766.robot.RobotTargets; -/** - * This procedure will eventually be able to score game pieces automatically. - * Currently, it moves the robot to the nearest scoring node. - * Once methods for other scoring mechanisms are done, they should be added here so this procedure wil be able to score completely autonomously. - */ -public class AutoScoring extends Procedure{ - Point currentPos; - double minDistance; - Point targetPoint; - Nodes targetNode; - Piece targetPiece; - Point[] pointList; - - public enum Nodes { - HYBRID, - MEDIUM, - HIGH - } - - public enum Piece { - HYBRID, - CUBE, - CONE - } - - public AutoScoring(Nodes node) { - this(node, Piece.HYBRID); - - if (node == Nodes.HYBRID) { - targetPiece = Piece.HYBRID; - } else { - targetPiece = Piece.CONE; - for (int i = 0; i < RobotTargets.CUBE_ROWS.length; i++) { - if (targetPoint.getY() == RobotTargets.CUBE_ROWS[i]) { - targetPiece = Piece.CUBE; - } - } - } - } - - public AutoScoring(Nodes node, Piece piece) { - - - switch (piece) { - case CUBE: pointList = RobotTargets.CUBE_NODES; - case CONE: pointList = RobotTargets.CONE_NODES; - default: pointList = RobotTargets.NODES; - } - - targetPiece = piece; - targetPoint = pointList[0]; - targetNode = node; - - } - - public void run(Context context) { - log("Starting AutoScoring " + targetPoint.toString()); - - currentPos = Robot.drive.getCurrentPosition().clone(); - minDistance = currentPos.distance(pointList[0]); - log("First Distance: " + minDistance); - - - for (int i = 1; i < pointList.length; i++) { - if (currentPos.distance(pointList[i]) < minDistance) { - minDistance = currentPos.distance(pointList[i]); - targetPoint = pointList[i]; - } - } - log("Final Distance: " + minDistance); - - - context.waitFor(context.startAsync(new FollowPoints(Robot.drive.getCurrentPosition().clone(), new PointDir[]{new PointDir(targetPoint, 0)}))); - //Do the actual scoring (arm movements, etc.) - log("Finishing AutoScoring"); - } -} diff --git a/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java deleted file mode 100644 index 62c7b13d..00000000 --- a/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java +++ /dev/null @@ -1,86 +0,0 @@ -package com.team766.robot.procedures; - -import java.util.ArrayList; -import java.util.List; -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.odometry.PointDir; -import com.team766.robot.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// Helper class for AlignCharger, see AlignCharger.md for more details -public class ChargeStationPathFinder { - - // Since this class does not extend Procedure, we need to instantiate logger - private static final Logger logger = Logger.get(Category.PROCEDURES); - private final Alliance alliance; - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public ChargeStationPathFinder(Alliance alliance) { - this.alliance = alliance; - } - - /** - * Calculates points that the robot needs to follow based on alliance and current position - * @param curPos the current robot position, represented by a pointDir - * @return PointDir[]: list of the calculated points that can be entered into FollowPoints - */ - public PointDir[] calculatePoints(PointDir curPos) { - - // Sets curX and curY variables based on curPos param - double curX = curPos.getX(); - double curY = curPos.getY(); - - // Creates ArrayList for points - List points = new ArrayList(); - switch (alliance) { - case Red: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.RED_BALANCE_TARGET_X, ChargeConstants.RED_LEFT_PT, ChargeConstants.RED_RIGHT_PT, ChargeConstants.MIDDLE); - break; - - case Blue: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.BLUE_BALANCE_TARGET_X, ChargeConstants.BLUE_LEFT_PT, ChargeConstants.BLUE_RIGHT_PT, ChargeConstants.MIDDLE); - break; - - case Invalid: // drop down - default: - logger.logRaw(null, "Invalid Alliance"); - } - - // Converts pointDir arrayList to array and returns it - return points.toArray(new PointDir[points.size()]); - } - - /** - * Adds a set of points to an arrayList based on passed charge station coordinates - * @param points ArrayList to add points to - * @param curX Current robot X value - * @param curY Current robot Y value - * @param target X value of center of target charge station - * @param left X value of left side of target charge station - * @param right X value of right side of target charge station - * @param height Y value of center of target charge station - */ - private void addPoints(List points, double curX, double curY, double target, double left, double right, double height) { - // If on the right side of the target, check if robot is blocked by the right side of the charge station, and add points accordingly - if (curX > target) { - if (curX < right) - points.add(new PointDir(right, curY)); - - points.add(new PointDir(right, height)); - - // Otherwise, it will be on the left side of the target, so check if robot is blocked by the left side of the charge station, and add points accordingly - } else { - if (curX > left) - points.add(new PointDir(left, curY)); - - points.add(new PointDir(left, height)); - } - } - -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/DoNothing.java b/src/main/java/com/team766/robot/procedures/DoNothing.java index 566ad756..1e2e3177 100644 --- a/src/main/java/com/team766/robot/procedures/DoNothing.java +++ b/src/main/java/com/team766/robot/procedures/DoNothing.java @@ -1,11 +1,9 @@ package com.team766.robot.procedures; -import com.team766.framework.Context; -import com.team766.framework.Procedure; +public class DoNothing { -public class DoNothing extends Procedure { - - public void run(Context context) { + public DoNothing(){ + } -} \ No newline at end of file +} diff --git a/src/main/java/com/team766/robot/procedures/FollowPoints.java b/src/main/java/com/team766/robot/procedures/FollowPoints.java deleted file mode 100644 index be571a11..00000000 --- a/src/main/java/com/team766/robot/procedures/FollowPoints.java +++ /dev/null @@ -1,482 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Procedure; -import com.team766.framework.Context; -import com.team766.framework.LaunchedContext; -import com.team766.robot.Robot; -import com.team766.robot.constants.*; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.hal.RobotProvider; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.hal.PositionReader; -import java.io.File; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import com.team766.config.ConfigFileReader; -import com.team766.logging.Category; -import com.team766.logging.Severity; -import com.team766.controllers.PIDController; -import com.team766.robot.procedures.*; -import edu.wpi.first.wpilibj.Filesystem; -import org.json.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -/** - * {@link Procedure} to follow a set of waypoints. Waypoint files can be passed in via - * the {@link #FollowPoints(String)} constructor. - */ -public class FollowPoints extends Procedure { - - //Steps combine possible data types into one object for flexibility and ease-of-use purposes - public static class Step { - - public PointDir wayPoint; - public boolean criticalPoint; - public Procedure procedure; - public boolean stopRobot; - // ... - - public Step(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) { - this.wayPoint = wayPoint; - this.procedure = procedure; - this.criticalPoint = criticalPoint; - this.stopRobot = stopRobot; - } - } - - private List steps = new ArrayList(); - - private static PointDir currentPos = new PointDir(0.0, 0.0, 0.0); - private static PointDir lastPos = new PointDir(0.0, 0.0, 0.0); - private static PointDir lastPos2 = new PointDir(0.0, 0.0, 0.0); - - private PointDir[] pointList; - private Procedure[] proceduresAtPoints; - private boolean[] criticalPointList; - private boolean[] stopRobotList; - private Point startingPoint = new Point(0, 0); - - private int targetNum = 0; - private RateLimiter followLimiter = new RateLimiter(FollowPointsInputConstants.RATE_LIMITER_TIME); - - //Radius defines the radius of the circle around the robot - private static double radius = FollowPointsInputConstants.RADIUS; - private static double speed = FollowPointsInputConstants.SPEED; - private static PointDir driveSettings = new PointDir(0, 0, 0); - - private static HashMap mapOfProcedures = new HashMap(); - - /*public FollowPoints() { - parsePointL$ist(); - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - loggerCategory = Category.AUTONOMOUS; - }*/ - - /** - * Constructor to create a new robot-oriented FollowPoints instance. - * @param file filename for the waypoints file to load and use. - * @throws IOException Thrown if file is not found. - */ - public FollowPoints(String file) { - mapOfProcedures.put("DoNothing()", new DoNothing()); - mapOfProcedures.put(null, new DoNothing()); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.HIGH)", new AutoScoring(AutoScoring.Nodes.HIGH)); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.MEDIUM)", new AutoScoring(AutoScoring.Nodes.MEDIUM)); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.HYBRID)", new AutoScoring(AutoScoring.Nodes.HYBRID)); - mapOfProcedures.put("setCross()", new setCross()); - - String str; - Path path = Filesystem.getDeployDirectory().toPath().resolve(file); - try { - str = Files.readString(path); - } catch (IOException e) { - e.printStackTrace(); - log(Severity.ERROR, "Could not load " + file); - return; - } - - JSONArray points = new JSONObject(str).getJSONArray("points"); - JSONArray start = new JSONObject(str).getJSONArray("start"); - - startingPoint.set(start.getDouble(0), start.getDouble(1)); - - for (int i = 0; i < points.length(); i++) { - JSONObject procedure = points.getJSONObject(i).getJSONObject("procedure"); - Procedure pointProcedure = null; - boolean stopAtProcedure = false; - if (procedure != null) { - pointProcedure = mapOfProcedures.get(procedure.getString("name")); - stopAtProcedure = procedure.getBoolean("stop"); - } - addStep(new PointDir(points.getJSONObject(i).getJSONArray("coordinates").getDouble(0), points.getJSONObject(i).getJSONArray("coordinates").getDouble(1), points.getJSONObject(i).getJSONArray("coordinates").getDouble(2)), points.getJSONObject(i).getBoolean("critical"), pointProcedure, stopAtProcedure); - } - addWaypoints(); - } - - /** - * Constructor to create a new field-oriented FollowPoints instance. - * @param startingPoint The original position of the robot - * @param file filename for the waypoints file to load and use. - * @throws IOException Thrown if file is not found. - */ - public FollowPoints(Point startingPoint, String file) { - this.startingPoint = startingPoint; - String str; - Path path = Filesystem.getDeployDirectory().toPath().resolve(file); - try { - str = Files.readString(path); - } catch (IOException e) { - e.printStackTrace(); - log(Severity.ERROR, "Could not load " + file); - return; - } - - JSONArray points = new JSONObject(str).getJSONArray("points"); - for (int i = 0; i < points.length(); i++) { - JSONObject procedure = points.getJSONObject(i).getJSONObject("procedure"); - Procedure pointProcedure = null; - boolean stopAtProcedure = false; - if (procedure != null) { - pointProcedure = mapOfProcedures.get(procedure.getString("name")); - stopAtProcedure = procedure.getBoolean("stop"); - } - addStep(new PointDir(points.getJSONObject(i).getJSONArray("coordinates").getDouble(0), points.getJSONObject(i).getJSONArray("coordinates").getDouble(1), points.getJSONObject(i).getJSONArray("coordinates").getDouble(2)), points.getJSONObject(i).getBoolean("critical"), pointProcedure, stopAtProcedure); - } - addWaypoints(); - } - - /** - * Method which creates a new Step object from its constituents. - * @param wayPoint PointDir consisting of the x- and y- coordinates of the point, as well as the target header for the robot at that point. - * @param criticalPoint Boolean determining whether the robot should travel to the point (true) or if it should use the FollowPoints circle method (false). - * @param procedure Procedure for the robot to run when reaching the point. If null is inputted, it is interpreted as "doNothing()". - * @param stopRobot Boolean determining whether the robot should wait for the procedure to finish before continuing on its path. - */ - private void addStep(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) { - steps.add(new Step(wayPoint, criticalPoint, procedure, stopRobot)); - } - - /** - * Default FollowPoints Constructor. Steps must be added in-code. This is robot-oriented. - */ - public FollowPoints() { - addStep(new PointDir(0,0, 0), false, new DoNothing(), false); - addStep(new PointDir(2,0, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(2,2, 0), false, new DoNothing(), false); - addStep(new PointDir(0,2, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(0,0, 0), true, new DoNothing(), false); - addWaypoints(); - } - - /** - * Default FollowPoints Constructor but with starting point. Steps must be added in-code. This is field-oriented. - */ - public FollowPoints(Point startingPoint) { - this.startingPoint = startingPoint; - addStep(new PointDir(0,0, 0), false, new DoNothing(), false); - addStep(new PointDir(2,0, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(2,2, 0), false, new DoNothing(), false); - addStep(new PointDir(0,2, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(0,0, 0), true, new DoNothing(), false); - addWaypoints(); - } - - /** - * When using steps (in the default constructor), this sets up the arrays to be used by the FollowPoints method. - */ - private void addWaypoints() { - pointList = new PointDir[steps.size()]; - proceduresAtPoints = new Procedure[steps.size()]; - stopRobotList = new boolean[pointList.length]; - criticalPointList = new boolean[pointList.length]; - for (int i = 0; i < steps.size(); i++) { - if (steps.get(i).wayPoint == null) continue; - else { - pointList[i] = steps.get(i).wayPoint; - } - if (steps.get(i).procedure != null) { - proceduresAtPoints[i] = steps.get(i).procedure; - // run procedure - // TODO: make sure we're handling contexts correctly - } else { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList[i] = steps.get(i).criticalPoint; - - stopRobotList[i] = steps.get(i).stopRobot; - } - } - - - /** - * Constructor which takes an array of points, and is robot-oriented. - * @param points Array of PointDir objects for the robot to follow, consisting of x- and y- coordinates, as well as a target header for the robot. - */ - public FollowPoints(PointDir[] points) { - pointList = points; - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList = new boolean[pointList.length]; - stopRobotList = new boolean[pointList.length]; - loggerCategory = Category.AUTONOMOUS; - } - - /** - * Constructor which takes an array of points, and is field-oriented. - * @param points Array of PointDir objects for the robot to follow, consisting of x- and y- coordinates, as well as a target header for the robot. - */ - public FollowPoints(Point startingPoint, PointDir[] points) { - this.startingPoint = startingPoint; - pointList = points; - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList = new boolean[pointList.length]; - stopRobotList = new boolean[pointList.length]; - loggerCategory = Category.AUTONOMOUS; - } - - //Takes pairs of points from pointDoubles (set in the config file) and converts them to Points, which are placed in pointList. - /*private void parsePointList() { - Double[] pointDoubles = ConfigFileReader.getInstance().getDoubles("trajectory.points").get(); - pointList = new PointDir[pointDoubles.length / 2]; - double pointX = 0; - double pointY = 0; - for (int i = 0; i < pointDoubles.length / 2 * 2; i++) { - if (i % 2 == 0) - pointX = pointDoubles[i]; - else { - pointY = pointDoubles[i]; - pointList[i / 2] = new PointDir(pointX, pointY); - } - } - }*/ - - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - log("Starting FollowPoints"); - - if (startingPoint == null) { - for (int i = 0; i < pointList.length; i++) { - pointList[i].add(Robot.drive.getCurrentPosition()); - } - } else { - Robot.drive.setCurrentPosition(startingPoint); - } - targetNum = 0; - - for (int i = 0; i < pointList.length; i++) { - log(pointList[i].toString()); - log("Stop: " + stopRobotList[i]); - log("Procedure: " + proceduresAtPoints[i]); - } - if (pointList.length > 0) { - Point targetPoint = new Point(0.0, 0.0); - currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading()); - while (targetNum < pointList.length - 1 || !passedPoint(pointList[pointList.length - 1])) { - if (followLimiter.next()) { - lastPos2 = lastPos.clone(); - lastPos = currentPos.clone(); - currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading()); - //If the next point is a critical point, the robot will wait until it has passed that point for it to move to the next point - //Otherwise, it uses the checkIntersection() method to follow the circle - if (criticalPointList[targetNum]? (targetNum < pointList.length - 1 && passedPoint(pointList[targetNum])) : checkIntersection(pointList)) { - if (targetNum != stopRobotList.length - 1 && proceduresAtPoints.length > targetNum) { - if (stopRobotList[targetNum]) { - driveSettings.set(0, 0); - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - Robot.drive.setCross(); - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - context.waitFor(context.startAsync(proceduresAtPoints[targetNum])); - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - } else { - context.startAsync(proceduresAtPoints[targetNum]); - } - } - targetNum++; - log("Going to Next Point! " + pointList[targetNum]); - } - targetPoint = selectTargetPoint(targetNum, pointList); - //double diff = currentPos.getAngleDifference(targetPoint); - //Robot.drive.setDrivePower(straightVelocity + Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity), straightVelocity - Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity)); - - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - driveSettings.set(currentPos.scaleVector(targetPoint, speed), rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())); - driveSettings.set((DriverStation.getAlliance() == Alliance.Blue ? -1 : 1) * driveSettings.getX(), (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1) * driveSettings.getY()); - Robot.drive.swerveDrive(driveSettings); - //log("Current Position: " + currentPos.toString()); - //log("Target Point: " + targetPoint.toString()); - //log("Unit Vector: " + new PointDir(currentPos.scaleVector(targetPoint, speed), rotationSpeed(Robot.gyro.getGyroYaw(), pointList[targetNum].getHeading())).toString()); - - context.yield(); - } else { - updateRotation(); - } - } - if (proceduresAtPoints.length > targetNum) { - if (stopRobotList[stopRobotList.length - 1]) { - driveSettings.set(0, 0); - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[stopRobotList.length - 1].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - Robot.drive.setCross(); - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - context.waitFor(context.startAsync(proceduresAtPoints[stopRobotList.length - 1])); - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - } else { - context.startAsync(proceduresAtPoints[stopRobotList.length - 1]); - } - } - Robot.drive.drive2D(0, 0); - driveSettings.set(0, 0); - targetNum = pointList.length - 1; - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - context.releaseOwnership(Robot.drive); - context.startAsync(new setCross()); - log("Finished method!"); - } else { - log("No points!"); - } - } - - /** - * Method which keeps updating how much the robot should turn between rateLimiter calls. - */ - public void updateRotation() { - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - driveSettings.setHeading(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())); - Robot.drive.swerveDrive(driveSettings); - } - - /** - * Method which returns whether the circle around the robot intersects the line connecting the two next points. - * @param pointList The list of points the robot is following. - * @return Boolean: whether or not the circle with given radius centered at currentPos intersects with the line between the next two points. - */ - private boolean checkIntersection(Point[] pointList) { - double a; - double b; - double c; - double slope; - if (targetNum < pointList.length - 1) { - slope = pointList[targetNum].slope(pointList[targetNum + 1]); - a = slope * slope + 1; - b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum].getX() + 2 * slope * pointList[targetNum].getY() - 2 * currentPos.getY() * slope; - c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum].getX() * pointList[targetNum].getX() - 2 * slope * pointList[targetNum].getX() * pointList[targetNum].getY() + pointList[targetNum].getY() * pointList[targetNum].getY() + 2 * currentPos.getY() * slope * pointList[targetNum].getX() - 2 * currentPos.getY() * pointList[targetNum].getY() + currentPos.getY() * currentPos.getY() - radius * radius; - if (b * b - 4 * a * c > 0) { - return true; - } - } - return false; - } - - /** - * Method returning which point the robot should move towards. - * @param targetNum The id value of the next point in pointList. - * @param currentPos The current position of the robot. - * @return If the circle around the robot intersects the line connecting the previous and next points, returns whichever intersection point is closest to the next point. Otherwise, returns the next point. - */ - private static Point selectTargetPoint(int targetNum, Point[] pointList) { - double a; - double b; - double c; - double slope; - double potentialX1; - double potentialX2; - double potentialY1; - double potentialY2; - Point potentialPoint1 = new Point(0.0, 0.0); - Point potentialPoint2 = new Point(0.0, 0.0); - if (targetNum == 0) { - return pointList[0]; - } else { - slope = pointList[targetNum - 1].slope(pointList[targetNum]); - a = slope * slope + 1; - b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum - 1].getX() + 2 * slope * pointList[targetNum - 1].getY() - 2 * currentPos.getY() * slope; - c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getX() - 2 * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getY() + pointList[targetNum - 1].getY() * pointList[targetNum - 1].getY() + 2 * currentPos.getY() * slope * pointList[targetNum - 1].getX() - 2 * currentPos.getY() * pointList[targetNum - 1].getY() + currentPos.getY() * currentPos.getY() - radius * radius; - if (b * b - 4 * a * c < 0) { - return pointList[targetNum]; - } else { - potentialX1 = (-1 * b + Math.sqrt(b * b - 4 * a * c))/ (2 * a); - potentialX2 = (-1 * b - Math.sqrt(b * b - 4 * a * c))/ (2 * a); - potentialY1 = slope * (potentialX1 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY(); - potentialY2 = slope * (potentialX2 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY(); - potentialPoint1.set(potentialX1, potentialY1); - potentialPoint2.set(potentialX2, potentialY2); - if (potentialPoint1.distance(pointList[targetNum]) <= potentialPoint2.distance(pointList[targetNum])) { - return potentialPoint1; - } else { - return potentialPoint2; - } - } - } - } - - //Returns if the robot has passed a certain point - /** - * Method returning whether the robot has passed a given point. - * @param P Point which this method determines has been passed. - * @return If the robot is within a certain distance of P, and the distance to P is increasing, returns true. Otherwise, returns false. - */ - private boolean passedPoint(Point P) { - //log(currentPos + " " + P + " " + currentPos.distance(P) + " " + ((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.2) ? " true" : " false")); - //log(((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.4)) ? "true " : "false " + targetNum); - return (currentPos.distance(P) <= 0.03 || (currentPos.distance(P) <= 0.15 && currentPos.distance(P) > lastPos.distance(P) && lastPos.distance(P) > lastPos2.distance(P))); - } - - /** - * Method which returns how much the robot should turn to reach its target heading. - * @param currentRot The current heading of the robot. - * @param targetRot The target heading of the robot. - * @return Returns a value between -1 and 1 corresponding to how much the robot should turn to reach the target point. - */ - private double rotationSpeed(double currentRot, double targetRot) { - double maxSpeed = FollowPointsInputConstants.MAX_ROTATION_SPEED; - double angleDistanceForMaxSpeed = FollowPointsInputConstants.ANGLE_DISTANCE_FOR_MAX_SPEED; - currentRot = mod(currentRot, 360); - targetRot = mod(targetRot, 360); - if (Math.abs(targetRot - currentRot) > Math.abs(targetRot + 360 - currentRot)) { - targetRot += 360; - } - if (Math.abs(targetRot - currentRot) > Math.abs(targetRot - 360 - currentRot)) { - targetRot -= 360; - } - if (Math.abs(targetRot - currentRot) <= angleDistanceForMaxSpeed) { - if (Math.abs(targetRot - currentRot) >= 3) { - return ((currentRot - targetRot) / angleDistanceForMaxSpeed) * maxSpeed; - } - return 0; - } - return maxSpeed * Math.signum(currentRot - targetRot); - } - - //Returns mod(d1, d2), to use to circumvent java's weird % function - private static double mod(double d1, double d2) { - return d1 % d2 + (d1 < 0 ? d2 : 0); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/GyroBalance.java b/src/main/java/com/team766/robot/procedures/GyroBalance.java deleted file mode 100644 index 40b6571a..00000000 --- a/src/main/java/com/team766/robot/procedures/GyroBalance.java +++ /dev/null @@ -1,180 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; -import com.team766.robot.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * {@link Procedure} to use the gyro to automatically balance on the charge station. - * See GyroBalance.md for more details - */ -public class GyroBalance extends Procedure { - - // State machine with 4 states for position on ramp - private enum State { - GROUND, - RAMP_TRANSITION, - RAMP_TILT, - RAMP_LEVEL - } - - // Direction determines which direction the robot moves - private enum Direction { - LEFT, - RIGHT, - STOP, - } - - // tilt is the overall combination of pitch and roll - private double tilt = Robot.gyro.getAbsoluteTilt(); - - // absSpeed is unsigned speed value - private double absSpeed; - private State prevState; - private State curState; - private Direction direction; - private final Alliance alliance; - - private final double TOP_TILT = 15.0; - private final double FLAP_TILT = 11; - - // Tweak these values to adjust how the robot balances - private final double LEVEL = 7; - private final double CORRECTION_DELAY = 0.5; - private final double SPEED_GROUND = .3; - private final double SPEED_TRANSITION = .25; - private final double SPEED_TILT = .18; - - /** - * Constructor to create a new GyroBalance instance - * @param alliance Alliance for setting direction towards charge station - */ - public GyroBalance(Alliance alliance) { - this.alliance = alliance; - } - - public void run(Context context) { - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.drive); - - // curX is current robot x position - double curX = Robot.drive.getCurrentPosition().getX(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - - // driveSpeed is actual value of speed passed into the swerveDrive method - double driveSpeed = 1; - - // Sets movement direction ground state if on ground - setDir(curX); - - // sets starting state if not on ground - if (tilt < LEVEL && curState != State.GROUND) { - curState = State.RAMP_LEVEL; - } else if (tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - } else if (tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - } - - do { - // Sets prevState to the current state and calculates curState - prevState = curState; - curX = Robot.drive.getCurrentPosition().getX(); - tilt = Robot.gyro.getAbsoluteTilt(); - //log("curX:" + curX); - //log("direction: " + direction); - setState(); - - // Both being on Red alliance and needing to move right would make the movement direction negative, so this expression corrects for that - if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) { - driveSpeed = -absSpeed; - } else { - driveSpeed = absSpeed; - } - - // Drives the robot with the calculated speed and direction - Robot.drive.swerveDrive(0, -driveSpeed, 0); - context.yield(); - } - // Loops until robot is level or until a call to the abort() method - while (!(curState == State.RAMP_LEVEL)); - - // After the robot is level, drives for correctionDelay seconds. - // Direction is opposite due to inversion of speed in setState() so it corrects for overshooting - context.waitForSeconds(CORRECTION_DELAY); - - // Locks wheels once balanced - context.startAsync(new setCross()); - - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - } - - // Sets state in state machine, see more details in GyroBalance.md - private void setState() { - if (prevState == State.GROUND && tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - absSpeed = SPEED_TRANSITION; - log("Transition, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - absSpeed = SPEED_TILT; - log("Tilt, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { - curState = State.RAMP_LEVEL; - // If level, sets speed to negative to correct for overshooting - absSpeed = -absSpeed; - log("Level, prevState: " + prevState + ", curState: " + curState); - } - if (curState == State.GROUND) { - absSpeed = SPEED_GROUND; - } - } - - /** - * Sets direction towards desired charge station - * If robot is level and outside of charge station boundaries, sets state to ground - * @param curX current robot x position - */ - private void setDir(double curX) { - switch (alliance) { - case Red: - // If to the right of the charge station, go left - if (curX > ChargeConstants.RED_BALANCE_TARGET_X) { - // If level and outside of charge station boundaries, set state to ground - if (tilt < LEVEL && curX > ChargeConstants.RED_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - // If to the left of the charge station, go right - } else { - if (tilt < LEVEL && curX < ChargeConstants.RED_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - case Blue: - // Same logic for blue alliance coordinates - if (curX > ChargeConstants.BLUE_BALANCE_TARGET_X) { - if (tilt < LEVEL && curX > ChargeConstants.BLUE_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - } else { - if (tilt < LEVEL && curX < ChargeConstants.BLUE_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - case Invalid: //drop down - default: - log("Invalid alliance"); - } - } - -} diff --git a/src/main/java/com/team766/robot/procedures/IntakeIn.java b/src/main/java/com/team766/robot/procedures/IntakeIn.java deleted file mode 100644 index b9a18c1b..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeIn.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeIn extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.startIntake(); - Robot.storage.beltIn(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/IntakeOut.java b/src/main/java/com/team766/robot/procedures/IntakeOut.java deleted file mode 100644 index 3a891e7f..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeOut.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeOut extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.reverseIntake(); - Robot.storage.beltOut(); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/IntakeStop.java b/src/main/java/com/team766/robot/procedures/IntakeStop.java deleted file mode 100644 index ca22867b..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeStop.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeStop extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OPECHelper.java b/src/main/java/com/team766/robot/procedures/OPECHelper.java deleted file mode 100644 index 141dd256..00000000 --- a/src/main/java/com/team766/robot/procedures/OPECHelper.java +++ /dev/null @@ -1,27 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; -import com.team766.robot.constants.FollowPointsInputConstants; -import java.util.function.BooleanSupplier; -import edu.wpi.first.wpilibj.DriverStation; -import com.team766.odometry.*; - -public class OPECHelper extends Procedure { - - private static final double DIST = 4; - - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - double startX = Robot.drive.getCurrentPosition().getX(); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - new ReverseIntake().run(context); - Robot.drive.swerveDrive(0, FollowPointsInputConstants.SPEED, 0); - context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getX() - startX) > DIST); - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceBalance.java deleted file mode 100644 index df391362..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceBalance extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 2.7)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 2.7)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - - } - new ReverseIntake().run(context); - new GyroBalance(DriverStation.getAlliance()).run(context); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java deleted file mode 100644 index 7e5231c7..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java +++ /dev/null @@ -1,32 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceExitCommunity extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java deleted file mode 100644 index cfc1b60f..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceExitCommunityBalance extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 2.7)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 2.7)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - log("Transitioning"); - new GyroBalance(DriverStation.getAlliance()).run(context); - } - -} diff --git a/src/main/java/com/team766/robot/procedures/ReverseIntake.java b/src/main/java/com/team766/robot/procedures/ReverseIntake.java deleted file mode 100644 index 53fc329f..00000000 --- a/src/main/java/com/team766/robot/procedures/ReverseIntake.java +++ /dev/null @@ -1,12 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; - -public class ReverseIntake extends Procedure{ - public void run(Context context) { - context.startAsync(new IntakeOut()); - context.waitForSeconds(2); - context.startAsync(new IntakeStop()); - } -} diff --git a/src/main/java/com/team766/robot/procedures/setCross.java b/src/main/java/com/team766/robot/procedures/setCross.java deleted file mode 100644 index e481c027..00000000 --- a/src/main/java/com/team766/robot/procedures/setCross.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class setCross extends Procedure { - - public void run(Context context) { - context.takeOwnership(Robot.drive); - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - Robot.drive.setCross(); - } - -} From 70f5df057bf77a21f744377837679905be1bdfd5 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 3 Jun 2023 14:48:27 -0700 Subject: [PATCH 15/29] added the -turn + forward for right motor --- src/main/java/com/team766/robot/mechanisms/januaryTag.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index abbc0767..eacf8691 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -197,7 +197,7 @@ public void go(){ double leftMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); - double rightMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); + double rightMotorPower = turnConstant * -turn + forwardConstant * MathUtil.clamp(forward, -1, 1); leftMotor.set(leftMotorPower); rightMotor.set(rightMotorPower); From 51ebfb36d788bbd4a9fd69ca23a42c61f25793e1 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 5 Jun 2023 15:49:31 +0000 Subject: [PATCH 16/29] jFrame Joystick Buttons?!?!?!?! --- src/main/java/com/team766/robot/Robot.java | 2 + .../com/team766/robot/mechanisms/jFrame.java | 37 +++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 src/main/java/com/team766/robot/mechanisms/jFrame.java diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 6b746399..efbc6a9d 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -11,6 +11,7 @@ public class Robot { // public static Gyro gyro; // public static Grabber grabber; public static januaryTag JanuaryTag; + public static void robotInit() { //Initialize mechanisms here // intake = new Intake(); @@ -20,5 +21,6 @@ public static void robotInit() { // gyro = new Gyro(); // grabber = new Grabber(); JanuaryTag = new januaryTag(); + jFrame JFrame = new jFrame(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/jFrame.java b/src/main/java/com/team766/robot/mechanisms/jFrame.java new file mode 100644 index 00000000..194f6d97 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/jFrame.java @@ -0,0 +1,37 @@ +package com.team766.robot.mechanisms; + +import javax.swing.*; +import java.awt.*; +import java.awt.event.*; +import java.io.File; +import java.util.ArrayList; +import java.util.*; +import com.team766.logging.*; + +public class jFrame extends JFrame implements ActionListener { + public januaryTag JanuaryTag; + public JFrame frame; + JButton click; + public jFrame(){ + super("MotorController"); + loggerCategory = Category.MECHANISMS; + JanuaryTag = new januaryTag(); + frame = new JFrame(); + click = new JButton("Play Again"); + frame.add(click); + + click.addActionListener(this); + frame.getRootPane().setDefaultButton(click); // sets default button + frame.setVisible(true); + } + + public void actionPerformed(ActionEvent e) { + JanuaryTag.debugLogs(); + ArrayList arr = new ArrayList(); + arr = JanuaryTag.getTransform3dData(); + for(int i = 0; i < arr.size(); i++){ + log("" + arr.get(i)); + } + } + +} From ba305c18e4672c78599c0f8273e89a3f5a73d62d Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 6 Jun 2023 08:38:56 -0700 Subject: [PATCH 17/29] Update jFrame.java logs --- src/main/java/com/team766/robot/mechanisms/jFrame.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/jFrame.java b/src/main/java/com/team766/robot/mechanisms/jFrame.java index 194f6d97..ffb963b2 100644 --- a/src/main/java/com/team766/robot/mechanisms/jFrame.java +++ b/src/main/java/com/team766/robot/mechanisms/jFrame.java @@ -14,7 +14,7 @@ public class jFrame extends JFrame implements ActionListener { JButton click; public jFrame(){ super("MotorController"); - loggerCategory = Category.MECHANISMS; + //loggerCategory = Category.MECHANISMS; JanuaryTag = new januaryTag(); frame = new JFrame(); click = new JButton("Play Again"); @@ -30,7 +30,7 @@ public void actionPerformed(ActionEvent e) { ArrayList arr = new ArrayList(); arr = JanuaryTag.getTransform3dData(); for(int i = 0; i < arr.size(); i++){ - log("" + arr.get(i)); + //log("" + arr.get(i)); } } From a2aeb5f91fc0114cca64122fd57ca6dbd59aae02 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 7 Jun 2023 19:15:48 -0700 Subject: [PATCH 18/29] im really good at coding --- src/main/java/com/team766/robot/OI.java | 4 +- src/main/java/com/team766/robot/Robot.java | 2 +- .../com/team766/robot/mechanisms/jFrame.java | 37 ------------------- .../team766/robot/mechanisms/januaryTag.java | 14 ++++--- 4 files changed, 12 insertions(+), 45 deletions(-) delete mode 100644 src/main/java/com/team766/robot/mechanisms/jFrame.java diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index e7f3f69a..bdb16315 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -27,9 +27,9 @@ public OI(){ public void run(Context context){ context.takeOwnership(Robot.JanuaryTag); - + Robot.JanuaryTag.setDeadzones(0.2, 0.2); if(joystickOne.getButton(1)){ - Robot.JanuaryTag.debugLogs(); + Robot.JanuaryTag.go(); } } /* diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index efbc6a9d..2c21f6b7 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -21,6 +21,6 @@ public static void robotInit() { // gyro = new Gyro(); // grabber = new Grabber(); JanuaryTag = new januaryTag(); - jFrame JFrame = new jFrame(); + //jFrame JFrame = new jFrame(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/jFrame.java b/src/main/java/com/team766/robot/mechanisms/jFrame.java deleted file mode 100644 index ffb963b2..00000000 --- a/src/main/java/com/team766/robot/mechanisms/jFrame.java +++ /dev/null @@ -1,37 +0,0 @@ -package com.team766.robot.mechanisms; - -import javax.swing.*; -import java.awt.*; -import java.awt.event.*; -import java.io.File; -import java.util.ArrayList; -import java.util.*; -import com.team766.logging.*; - -public class jFrame extends JFrame implements ActionListener { - public januaryTag JanuaryTag; - public JFrame frame; - JButton click; - public jFrame(){ - super("MotorController"); - //loggerCategory = Category.MECHANISMS; - JanuaryTag = new januaryTag(); - frame = new JFrame(); - click = new JButton("Play Again"); - frame.add(click); - - click.addActionListener(this); - frame.getRootPane().setDefaultButton(click); // sets default button - frame.setVisible(true); - } - - public void actionPerformed(ActionEvent e) { - JanuaryTag.debugLogs(); - ArrayList arr = new ArrayList(); - arr = JanuaryTag.getTransform3dData(); - for(int i = 0; i < arr.size(); i++){ - //log("" + arr.get(i)); - } - } - -} diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index eacf8691..314e57fc 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -55,7 +55,7 @@ public class januaryTag extends Mechanism{ private Transform3d offset; public januaryTag(){ loggerCategory = Category.MECHANISMS; - camera1 = new PhotonCamera("januaryTag.camera1"); + camera1 = new PhotonCamera("januaryTag"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); rightMotor = RobotProvider.instance.getMotor("rightMotor"); @@ -63,8 +63,8 @@ public januaryTag(){ x_targetConstant = 0.5; turnConstant = 0.2; forwardConstant = 0.2; - offsetX = 10; - offsetY = 10; + offsetX = 0.1; + offsetY = 0.1; offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); checkContextOwnership(); } @@ -199,14 +199,18 @@ public void go(){ double leftMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); double rightMotorPower = turnConstant * -turn + forwardConstant * MathUtil.clamp(forward, -1, 1); - leftMotor.set(leftMotorPower); - rightMotor.set(rightMotorPower); + leftMotor.set((-1) * leftMotorPower); + rightMotor.set((-1) * rightMotorPower); } } } public void debugLogs(){ Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + + log("Yaw PTT " + theTarget.getYaw()); + log("Pitch PTT " + theTarget.getPitch()); log("Yaw: " + targetTransform.getX()); log("Pitch: " + targetTransform.getY()); } From d4995c47715bea01a3ade609cc3a34bc45516b0f Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 7 Jun 2023 20:26:44 -0700 Subject: [PATCH 19/29] fixed it and it works!!! --- src/main/java/com/team766/robot/OI.java | 4 +++- .../team766/robot/mechanisms/januaryTag.java | 17 ++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index bdb16315..53cb8e18 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -27,9 +27,11 @@ public OI(){ public void run(Context context){ context.takeOwnership(Robot.JanuaryTag); - Robot.JanuaryTag.setDeadzones(0.2, 0.2); + Robot.JanuaryTag.setDeadzones(2, 2); if(joystickOne.getButton(1)){ Robot.JanuaryTag.go(); + } else{ + Robot.JanuaryTag.manual(joystickOne.getAxis(1), joystickOne.getAxis(1)); } } /* diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 314e57fc..5fb3ddbb 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.team766.config.ConfigFileReader; +import com.team766.framework.Context; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; @@ -61,14 +62,19 @@ public januaryTag(){ //Sample values, we don't know these work at all yet x_targetConstant = 0.5; - turnConstant = 0.2; + turnConstant = 0.3; forwardConstant = 0.2; - offsetX = 0.1; - offsetY = 0.1; + offsetX = 0.75; + offsetY = -0.03; offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); checkContextOwnership(); } + public void manual(double left, double right){ + leftMotor.set(left); + rightMotor.set(right); + } + /* * This method returns the best target that the camera is currently tracking @@ -180,6 +186,8 @@ public void go(){ if(scoring.getX() + deadzoneX > 0 && scoring.getX() - deadzoneX < 0 && scoring.getY() + deadzoneY > 0 && scoring.getY() - deadzoneY < 0){ log("Outtaking"); + log("FINAL X: " + scoring.getX()); + log("FINAL Y: " + scoring.getY()); break; }else{ @@ -201,6 +209,9 @@ public void go(){ leftMotor.set((-1) * leftMotorPower); rightMotor.set((-1) * rightMotorPower); + + log("X: " + scoring.getX()); + log("Y: " + scoring.getY()); } } } From dc946ddcf5bf1e94f613950b5ac23e29c1b44e9d Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 8 Jun 2023 16:40:39 +0000 Subject: [PATCH 20/29] general documentation --- src/main/java/com/team766/robot/mechanisms/januaryTag.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 5fb3ddbb..f655d6bd 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -65,7 +65,7 @@ public januaryTag(){ turnConstant = 0.3; forwardConstant = 0.2; offsetX = 0.75; - offsetY = -0.03; + offsetY = 0; offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); checkContextOwnership(); } @@ -176,6 +176,10 @@ public void setDeadzones(double x, double y){ } /* * This method is the method that you should call to make the robot go to the target. Currently, it will go to any apriltag that it sees, but we can change that later. + * It uses the getBestTrackedTarget() method to get the best target that the camera is currently tracking, and then uses it in the Transform3d object. + * It then uses the Transform3d object to get the x and y values of the target relative to the camera, and then uses those values to calculate the x and y values of the target relative to the robot. + * If the x and y values of the target relative to the robot are within the deadzone, set in OI, then the robot will stop moving. + * If the x and y values of the target relative to the robot are not within the deadzone, then the robot will move towards the target using arcade drive formulas. */ public void go(){ From 93d08f56b89b748e0e0add5eded334ba52e410c1 Mon Sep 17 00:00:00 2001 From: Max Date: Fri, 9 Jun 2023 09:03:57 -0700 Subject: [PATCH 21/29] documentation! --- .../team766/robot/mechanisms/januaryTag.java | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index f655d6bd..2f70c151 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -69,7 +69,7 @@ public januaryTag(){ offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); checkContextOwnership(); } - + //Manually move the robot public void manual(double left, double right){ leftMotor.set(left); rightMotor.set(right); @@ -145,6 +145,14 @@ public ArrayList getPhotonTrackedTargetData(){ arr.add((double) theTarget.getFiducialId()); return arr; } + /* + * This method returns the ID of the target that the camera is currently tracking + * It could be used to tell which target it is aimed at so we know where the robot is + * @return the ID of the target that the camera is currently tracking + */ + public int getJanuaryTagId(PhotonTrackedTarget theTarget){ + return theTarget.getFiducialId(); + } /* * This is a method to return data given from a Transform3d @@ -161,6 +169,12 @@ public ArrayList getTransform3dData(){ arr.add(targetTransform.getZ()); return arr; } + + /* + * These next three methods are for setting deadzones for the go() method. + * For the first two methods @param dz - the deadzone that you want to set + * For the last method @param x - the x deadzone that you want to set, @param y - the y deadzone that you want to set + */ public void setDeadzoneX(double dz){ deadzoneX = dz; @@ -220,6 +234,9 @@ public void go(){ } } + /* + * These are debug logs + */ public void debugLogs(){ Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); PhotonTrackedTarget theTarget = getBestTrackedTarget(); From c1907509c008dab8f93c5eddee488bf6e82552b3 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 19 Jul 2023 20:21:19 -0700 Subject: [PATCH 22/29] hehe swerve --- .../java/com/team766/odometry/Odometry.java | 201 ++++++ src/main/java/com/team766/robot/Robot.java | 8 +- .../com/team766/robot/mechanisms/Drive.java | 676 ++++++++++++++++++ .../com/team766/robot/mechanisms/Gyro.java | 68 ++ 4 files changed, 949 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/team766/odometry/Odometry.java create mode 100644 src/main/java/com/team766/robot/mechanisms/Drive.java create mode 100644 src/main/java/com/team766/robot/mechanisms/Gyro.java diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java new file mode 100644 index 00000000..16f9890b --- /dev/null +++ b/src/main/java/com/team766/odometry/Odometry.java @@ -0,0 +1,201 @@ +package com.team766.odometry; + +import com.team766.framework.LoggingBase; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.logging.Category; +import com.team766.robot.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** + * Method which calculates the position of the robot based on wheel positions. + */ +public class Odometry extends LoggingBase { + + private RateLimiter odometryLimiter; + + private MotorController[] motorList; + //The order of CANCoders should be the same as in motorList + private CANCoder[] CANCoderList; + private int motorCount; + + private PointDir[] prevPositions; + private PointDir[] currPositions; + private double[] prevEncoderValues; + private double[] currEncoderValues; + private double gyroPosition; + + private PointDir currentPosition; + + //In Meters + private static double WHEEL_CIRCUMFERENCE; + public static double GEAR_RATIO; + public static int ENCODER_TO_REVOLUTION_CONSTANT; + + //In the same order as motorList, relative to the center of the robot + private Point[] wheelPositions; + + /** + * Constructor for Odometry, taking in several defines for the robot. + * @param motors A list of every wheel-controlling motor on the robot. + * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors. + * @param wheelLocations A list of the locations of each wheel, in the same order as motors. + * @param wheelCircumference The circumfrence of the wheels, including treads. + * @param gearRatio The gear ratio of the wheels. + * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels. + * @param rateLimiterTime How often odometry should run. + */ + public Odometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime) { + loggerCategory = Category.ODOMETRY; + + odometryLimiter = new RateLimiter(rateLimiterTime); + motorList = motors; + CANCoderList = CANCoders; + motorCount = motorList.length; + log("Motor count " + motorCount); + prevPositions = new PointDir[motorCount]; + currPositions = new PointDir[motorCount]; + prevEncoderValues = new double[motorCount]; + currEncoderValues = new double[motorCount]; + + wheelPositions = wheelLocations; + WHEEL_CIRCUMFERENCE = wheelCircumference; + GEAR_RATIO = gearRatio; + ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant; + + currentPosition = new PointDir(0, 0, 0); + for (int i = 0; i < motorCount; i++) { + prevPositions[i] = new PointDir(0,0, 0); + currPositions[i] = new PointDir(0,0, 0); + prevEncoderValues[i] = 0; + currEncoderValues[i] = 0; + } + } + + public String getName() { + return "Odometry"; + } + + /** + * Sets the current position of the robot to Point P + * @param P The point to set the current robot position to + */ + public void setCurrentPosition(Point P) { + currentPosition.set(P); + log("Set Current Position to: " + P.toString()); + for (int i = 0; i < motorCount; i++) { + prevPositions[i].set(currentPosition.add(wheelPositions[i])); + currPositions[i].set(currentPosition.add(wheelPositions[i])); + } + log("Current Position: " + currentPosition.toString()); + } + + + /** + * Updates the odometry encoder values to the robot encoder values. + */ + private void setCurrentEncoderValues() { + for (int i = 0; i < motorCount; i++) { + prevEncoderValues[i] = currEncoderValues[i]; + currEncoderValues[i] = motorList[i].getSensorPosition(); + currEncoderValues[i] *= (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1); + } + } + + private static Vector2D rotate(Vector2D v, double angle) { + return new Vector2D( + v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(), + v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle)); + } + + /** + * Updates the position of each wheel of the robot by assuming each wheel moved in an arc. + */ + private void updateCurrentPositions() { + double angleChange; + double radius; + double deltaX; + double deltaY; + gyroPosition = -Robot.gyro.getGyroYaw(); + + /* + Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))), + Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())))); + */ + + for (int i = 0; i < motorCount; i++) { + //prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currPositions[i].getHeading()); + //This following line only works if the average of wheel positions is (0,0) + prevPositions[i].set(currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); + currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); + angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); + + double yaw = -Math.toRadians(Robot.gyro.getGyroYaw()); + double roll = Math.toRadians(Robot.gyro.getGyroRoll()); + double pitch = Math.toRadians(Robot.gyro.getGyroPitch()); + + double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); + Vector2D u = new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); + Vector2D v = new Vector2D(Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) - Math.sin(yaw) * Math.cos(roll), + Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + Math.cos(yaw) * Math.cos(roll)); + Vector2D a = u.scalarMultiply(Math.cos(w)).add(v.scalarMultiply(Math.sin(w))); + Vector2D b = u.scalarMultiply(-Math.sin(w)).add(v.scalarMultiply(Math.cos(w))); + Vector2D wheelMotion; + + //log("u: " + u + " v: " + v + " a: " + a + " b: " + b); + + //double oldWheelX; + //double oldWheelY; + + if (angleChange != 0) { + radius = 180 * (currEncoderValues[i] - prevEncoderValues[i]) / (Math.PI * angleChange); + deltaX = radius * Math.sin(Math.toRadians(angleChange)); + deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange))); + + wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY)); + + //oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + + } else { + wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i])); + + //oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) * Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + } + wheelMotion = wheelMotion.scalarMultiply(WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT)); + //wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition)); + //log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY - wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current Method: " + wheelMotion.getX() + ", " + wheelMotion.getY()); + //log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " + currPositions[i].add(wheelMotion)); + currPositions[i].set(currPositions[i].subtract(wheelMotion)); + } + } + + /** + * Calculates the position of the robot by finding the average of the wheel positions. + */ + private void findRobotPosition() { + double sumX = 0; + double sumY = 0; + for (int i = 0; i < motorCount; i++) { + sumX += currPositions[i].getX(); + sumY += currPositions[i].getY(); + //log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " + currPositions[i]); + } + currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition); + } + + //Intended to be placed inside Robot.drive.run() + public PointDir run() { + if (odometryLimiter.next()) { + setCurrentEncoderValues(); + updateCurrentPositions(); + findRobotPosition(); + log(currentPosition.toString()); + } + return currentPosition; + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 2c21f6b7..06ef61b7 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -6,9 +6,9 @@ public class Robot { //Declare mechanisms here // public static Intake intake; // public static Storage storage; - // public static Drive drive; + public static Drive drive; // public static Arms arms; - // public static Gyro gyro; + public static Gyro gyro; // public static Grabber grabber; public static januaryTag JanuaryTag; @@ -16,9 +16,9 @@ public static void robotInit() { //Initialize mechanisms here // intake = new Intake(); // storage = new Storage(); - // drive = new Drive(); + drive = new Drive(); // arms = new Arms(); - // gyro = new Gyro(); + gyro = new Gyro(); // grabber = new Grabber(); JanuaryTag = new januaryTag(); //jFrame JFrame = new jFrame(); diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java new file mode 100644 index 00000000..82528825 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -0,0 +1,676 @@ +package com.team766.robot.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.EncoderReader; +import com.team766.hal.RobotProvider; +import com.team766.hal.MotorController; +import com.team766.hal.MotorController.ControlMode; +import com.team766.hal.simulator.Encoder; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +import com.team766.simulator.ProgramInterface.RobotMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.team766.config.ConfigFileReader; +import com.team766.framework.Mechanism; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; +import com.team766.odometry.Odometry; +import com.team766.odometry.Point; +import com.team766.odometry.PointDir; +import com.team766.hal.MotorControllerCommandFailedException; +import com.team766.robot.constants.*; + +public class Drive extends Mechanism { + + private MotorController m_DriveFrontRight; + private MotorController m_DriveFrontLeft; + private MotorController m_DriveBackRight; + private MotorController m_DriveBackLeft; + + private MotorController m_SteerFrontRight; + private MotorController m_SteerFrontLeft; + private MotorController m_SteerBackRight; + private MotorController m_SteerBackLeft; + + private CANCoder e_FrontRight; + private CANCoder e_FrontLeft; + private CANCoder e_BackRight; + private CANCoder e_BackLeft; + + private ValueProvider drivePower; + + private double gyroValue; + + private static PointDir currentPosition; + + private MotorController[] motorList; + private CANCoder[] CANCoderList; + private Point[] wheelPositions; + private Odometry swerveOdometry; + + public Drive() { + + loggerCategory = Category.DRIVE; + // Initializations of motors + // Initialize the drive motors + m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); + m_DriveFrontLeft = RobotProvider.instance.getMotor("drive.DriveFrontLeft"); + m_DriveBackRight = RobotProvider.instance.getMotor("drive.DriveBackRight"); + m_DriveBackLeft = RobotProvider.instance.getMotor("drive.DriveBackLeft"); + // Initialize the steering motors + m_SteerFrontRight = RobotProvider.instance.getMotor("drive.SteerFrontRight"); + m_SteerFrontLeft = RobotProvider.instance.getMotor("drive.SteerFrontLeft"); + m_SteerBackRight = RobotProvider.instance.getMotor("drive.SteerBackRight"); + m_SteerBackLeft = RobotProvider.instance.getMotor("drive.SteerBackLeft"); + + // Setting up the "config" + CANCoderConfiguration config = new CANCoderConfiguration(); + config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; + // The encoders output "encoder" values, so we need to convert that to degrees (because that + // is what the cool kids are using) + config.sensorCoefficient = 360.0 / 4096.0; + // The offset is going to be changed in ctre, but we can change it here too. + // config.magnetOffsetDegrees = Math.toDegrees(configuration.getOffset()); + config.sensorDirection = true; + + // initialize the encoders + e_FrontRight = new CANCoder(22, "Swervavore"); + // e_FrontRight.configAllSettings(config, 250); + e_FrontLeft = new CANCoder(23, "Swervavore"); + // e_FrontLeft.configAllSettings(config, 250); + e_BackRight = new CANCoder(21, "Swervavore"); + // e_BackRight.configAllSettings(config, 250); + e_BackLeft = new CANCoder(24, "Swervavore"); + // e_BackLeft.configAllSettings(config, 250); + + + // Current limit for motors to avoid breaker problems (mostly to avoid getting electrical + // people to yell at us) + m_DriveFrontRight.setCurrentLimit(35); + m_DriveFrontLeft.setCurrentLimit(35); + m_DriveBackRight.setCurrentLimit(35); + m_DriveBackLeft.setCurrentLimit(35); + m_DriveBackLeft.setInverted(true); + m_DriveBackRight.setInverted(true); + m_SteerFrontRight.setCurrentLimit(30); + m_SteerFrontLeft.setCurrentLimit(30); + m_SteerBackRight.setCurrentLimit(30); + m_SteerBackLeft.setCurrentLimit(30); + + // Setting up the connection between steering motors and cancoders + // m_SteerFrontRight.setRemoteFeedbackSensor(e_FrontRight, 0); + // m_SteerFrontLeft.setRemoteFeedbackSensor(e_FrontLeft, 0); + // m_SteerBackRight.setRemoteFeedbackSensor(e_BackRight, 0); + // m_SteerBackLeft.setRemoteFeedbackSensor(e_BackLeft, 0); + + m_SteerFrontRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + m_SteerFrontLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + m_SteerBackRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + m_SteerBackLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + configPID(); + + // Sets up odometry + currentPosition = new PointDir(0, 0, 0); + motorList = new MotorController[] {m_DriveFrontRight, m_DriveFrontLeft, m_DriveBackLeft, + m_DriveBackRight}; + CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; + wheelPositions = + new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; + log("MotorList Length: " + motorList.length); + log("CANCoderList Length: " + CANCoderList.length); + swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); + setFrontRightEncoders(); + setFrontLeftEncoders(); + setBackRightEncoders(); + setBackLeftEncoders(); + } + + // A set of simple functions for the sake of adding vectors + /** + * Returns the pythagorean theorem of two numbers + * + * @param x First number + * @param y Second number + * @return + */ + public double pythagorean(double x, double y) { + return Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); + } + + /** + * Returns the angle of a vector, used by joystick inputs + * + * @param LR The x value of the vector + * @param FB The y value of the vector + * @return The angle of the vector + */ + public double getAngle(double LR, double FB) { + return Math.toDegrees(Math.atan2(LR, -FB)); + } + + /** + * Returns whether two angles are within 90 degrees of each other, used to see if the wheels + * should move backwards or not + * + * @param angle1 The first angle + * @param angle2 The second angle + * @return If they are within 90 degrees of each other + */ + public boolean withinHalfACircle(double angle1, double angle2) { + angle1 = mod(angle1, 360); + angle2 = mod(angle2, 360); + if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) { + angle2 += 360; + } + if (Math.abs(angle2 - angle1) > Math.abs(angle2 - 360 - angle1)) { + angle2 -= 360; + } + return Math.abs(angle2 - angle1) <= 90; + } + + // Returns mod(d1, d2), to use to circumvent java's weird % function + private static double mod(double d1, double d2) { + return d1 % d2 + (d1 < 0 ? d2 : 0); + } + + /** + * Rounds a number based on its value and places + * + * @param value The number to be rounded + * @param places The number of places to round to + * @return The rounded number + */ + public double round(double value, int places) { + double scale = Math.pow(10, places); + return Math.round(value * scale) / scale; + } + + /** + * Adds two vectors together + * + * @param FirstMag The magnitude of the first vector + * @param FirstAng The angle of the first vector + * @param SecondMag The magnitude of the second vector + * @param SecondAng The angle of the second vector + * @return New angle of the vector + */ + public double NewAng(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { + double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) + + SecondMag * Math.cos(Math.toRadians(SecondAng)); + double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) + + SecondMag * Math.sin(Math.toRadians(SecondAng)); + return round(Math.toDegrees(Math.atan2(FinalY, FinalX)), 5); + } + + /** + * Adds two vectors together + * + * @param FirstMag The magnitude of the first vector + * @param FirstAng The angle of the first vector + * @param SecondMag The magnitude of the second vector + * @param SecondAng The angle of the second vector + * @return New magnitude of the vector + */ + public double NewMag(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { + double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) + + SecondMag * Math.cos(Math.toRadians(SecondAng)); + double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) + + SecondMag * Math.sin(Math.toRadians(SecondAng)); + return round(Math.sqrt(Math.pow(FinalX, 2) + Math.pow(FinalY, 2)), 5); + } + + /** + * Corrects the joystick inputs to make them more accurate, currently unused + * + * @param Joystick The joystick value to be corrected + * @returnThe corrected joystick value + */ + public static double correctedJoysticks(double Joystick) { + if (Joystick >= 0) { + return (3.0 * Math.pow(Joystick, 2) - 2.0 * Math.pow(Joystick, 3)); + } else { + return (-1 * 3.0 * Math.pow(-1 * Joystick, 2) + 2.0 * Math.pow(-1 * Joystick, 3)); + } + } + + /** + * Converts the angle of the joystick to the angle of the robot compared to the field by using + * the gyro + * + * @param angle The angle of the joystick + * @param gyro The angle of the gyro + * @return The angle of the robot compared to the field + */ + public static double fieldAngle(double angle, double gyro) { + double newAngle; + newAngle = angle - gyro; + if (newAngle < 0) { + newAngle = newAngle + 360; + } + if (newAngle >= 180) { + newAngle = newAngle - 360; + } + return newAngle; + } + + /** + * Method to correct angles for the falcon encoders + * + * @param newAngle The given angle value + * @param lastAngle The last angle value + * @return The corrected angle value + */ + public static double newAngle(double newAngle, double lastAngle) { + while (newAngle < 0) + newAngle += 360; + while (newAngle < (lastAngle - 180)) + newAngle += 360; + while (newAngle > (lastAngle + 180)) + newAngle -= 360; + return newAngle; + } + + /** + * Sets the gyro value, used to switch between field and robot orientation + * + * @param value The value to set the gyro to + */ + public void setGyro(double value) { + gyroValue = value; + } + + /** + * This method is used to drive the robot in 2D without turning, using the joystick values. + * + * @param JoystickX The x value of the joystick + * @param JoystickY The y value of the joystick + */ + public void drive2D(double JoystickX, double JoystickY) { + checkContextOwnership(); + // double power = pythagorean((JoystickX), correctedJoysticks(JoystickY))/Math.sqrt(2); + double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); + double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); + + + if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontRight))) { + m_DriveFrontRight.set(power); + setFrontRightAngle(newAngle(angle, getCurrentAngle(m_SteerFrontRight))); + } else { + m_DriveFrontRight.set(-power); + setFrontRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontRight))); + } + + if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontLeft))) { + m_DriveFrontLeft.set(power); + setFrontLeftAngle(newAngle(angle, getCurrentAngle(m_SteerFrontLeft))); + } else { + m_DriveFrontLeft.set(-power); + setFrontLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontLeft))); + } + + if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackRight))) { + m_DriveBackRight.set(power); + setBackRightAngle(newAngle(angle, getCurrentAngle(m_SteerBackRight))); + } else { + m_DriveBackRight.set(-power); + setBackRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackRight))); + } + + if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackLeft))) { + m_DriveBackLeft.set(power); + setBackLeftAngle(newAngle(angle, getCurrentAngle(m_SteerBackLeft))); + } else { + m_DriveBackLeft.set(-power); + setBackLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackLeft))); + } + } + + /** + * This method is used to drive the robot in 2D without turning, using a point. + * + * @param joystick The point to use for the joystick values + */ + public void drive2D(Point joystick) { + drive2D(joystick.getX(), joystick.getY()); + } + + /** + * This method stops all of the drive motors + */ + public void stopDriveMotors() { + checkContextOwnership(); + m_DriveFrontRight.stopMotor(); + m_DriveFrontLeft.stopMotor(); + m_DriveBackRight.stopMotor(); + m_DriveBackLeft.stopMotor(); + } + + /** + * This method stops all of the steer motors + */ + public void stopSteerMotors() { + checkContextOwnership(); + m_SteerFrontRight.stopMotor(); + m_SteerFrontLeft.stopMotor(); + m_SteerBackRight.stopMotor(); + m_SteerBackLeft.stopMotor(); + } + + /** + * This method is the main method for driving the robot, using the joystick values. + * + * @param JoystickX The x value of the joystick + * @param JoystickY The y value of the joystick + * @param JoystickTheta The theta value of the joystick (for turning) + */ + public void swerveDrive(double JoystickX, double JoystickY, double JoystickTheta) { + checkContextOwnership(); + double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); + double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); + double frPower; + double flPower; + double brPower; + double blPower; + double frAngle; + double flAngle; + double brAngle; + double blAngle; + if (JoystickTheta >= 0) { + frPower = NewMag(power, angle, JoystickTheta, 45); + flPower = NewMag(power, angle, JoystickTheta, -45); + brPower = NewMag(power, angle, JoystickTheta, 135); + blPower = NewMag(power, angle, JoystickTheta, -135); + frAngle = NewAng(power, angle, JoystickTheta, 45); + flAngle = NewAng(power, angle, JoystickTheta, 135); + brAngle = NewAng(power, angle, JoystickTheta, -45); + blAngle = NewAng(power, angle, JoystickTheta, -135); + } else { + frPower = NewMag(power, angle, Math.abs(JoystickTheta), -135); + flPower = NewMag(power, angle, Math.abs(JoystickTheta), 135); + brPower = NewMag(power, angle, Math.abs(JoystickTheta), -45); + blPower = NewMag(power, angle, Math.abs(JoystickTheta), 45); + frAngle = NewAng(power, angle, Math.abs(JoystickTheta), -135); + flAngle = NewAng(power, angle, Math.abs(JoystickTheta), -45); + brAngle = NewAng(power, angle, Math.abs(JoystickTheta), 135); + blAngle = NewAng(power, angle, Math.abs(JoystickTheta), 45); + } + if (Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)) > 1) { + frPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); + flPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); + brPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); + blPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); + } + + if (withinHalfACircle(frAngle, getCurrentAngle(m_SteerFrontRight))) { + m_DriveFrontRight.set(frPower); + setFrontRightAngle(newAngle(frAngle, getCurrentAngle(m_SteerFrontRight))); + } else { + m_DriveFrontRight.set(-frPower); + setFrontRightAngle(newAngle(180 + frAngle, getCurrentAngle(m_SteerFrontRight))); + } + + if (withinHalfACircle(flAngle, getCurrentAngle(m_SteerFrontLeft))) { + m_DriveFrontLeft.set(flPower); + setFrontLeftAngle(newAngle(flAngle, getCurrentAngle(m_SteerFrontLeft))); + } else { + m_DriveFrontLeft.set(-flPower); + setFrontLeftAngle(newAngle(180 + flAngle, getCurrentAngle(m_SteerFrontLeft))); + } + + if (withinHalfACircle(brAngle, getCurrentAngle(m_SteerBackRight))) { + m_DriveBackRight.set(brPower); + setBackRightAngle(newAngle(brAngle, getCurrentAngle(m_SteerBackRight))); + } else { + m_DriveBackRight.set(-brPower); + setBackRightAngle(newAngle(180 + brAngle, getCurrentAngle(m_SteerBackRight))); + } + + if (withinHalfACircle(blAngle, getCurrentAngle(m_SteerBackLeft))) { + m_DriveBackLeft.set(blPower); + setBackLeftAngle(newAngle(blAngle, getCurrentAngle(m_SteerBackLeft))); + } else { + m_DriveBackLeft.set(-blPower); + setBackLeftAngle(newAngle(180 + blAngle, getCurrentAngle(m_SteerBackLeft))); + } + } + + /** + * This method is used to drive the robot with swerve using a PointDir + * + * @param joystick The PointDir to use for the joystick values + */ + public void swerveDrive(PointDir joystick) { + swerveDrive(-1 * joystick.getY(), -1 * joystick.getX(), joystick.getHeading()); + } + + /** + * This method is used to simply turn the robot without moving it + * + * @param Joystick The joystick value to use for turning + */ + public void turning(double Joystick) { + checkContextOwnership(); + if (Joystick > 0) { + setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); + setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); + setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); + setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); + m_DriveFrontRight.set(Math.abs(Joystick)); + m_DriveFrontLeft.set(Math.abs(Joystick)); + m_DriveBackRight.set(Math.abs(Joystick)); + m_DriveBackLeft.set(Math.abs(Joystick)); + } + if (Joystick < 0) { + setFrontRightAngle(newAngle(-45, getCurrentAngle(m_SteerFrontRight))); + setFrontLeftAngle(newAngle(-135, getCurrentAngle(m_SteerFrontLeft))); + setBackRightAngle(newAngle(45, getCurrentAngle(m_SteerBackRight))); + setBackLeftAngle(newAngle(135, getCurrentAngle(m_SteerBackLeft))); + m_DriveFrontRight.set(Math.abs(Joystick)); + m_DriveFrontLeft.set(Math.abs(Joystick)); + m_DriveBackRight.set(Math.abs(Joystick)); + m_DriveBackLeft.set(Math.abs(Joystick)); + } + } + + private double getCurrentAngle(MotorController motor) { + return Math.pow((2048.0 / 360.0 * (150.0 / 7.0)), -1) * motor.getSensorPosition(); + } + + /** + * Simple encoder logging method + */ + public void logs() { + /*log("Front Right Encoder: " + getFrontRight() + " Front Left Encoder: " + getFrontLeft() + + " Back Right Encoder: " + getBackRight() + " Back Left Encoder: " + + getBackLeft());*/ + } + + /** + * This method is used to set the front right encoder to the true position + */ + public void setFrontRightEncoders() { + //log("Steer FR Before: " + m_SteerFrontRight.getSensorPosition()); + m_SteerFrontRight.setSensorPosition((int) Math + .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontRight.getAbsolutePosition())); + //log("Steer FR After: " + m_SteerFrontRight.getSensorPosition()); + + } + + /** + * This method is used to set the front left encoder to the true position + */ + public void setFrontLeftEncoders() { + m_SteerFrontLeft.setSensorPosition((int) Math + .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontLeft.getAbsolutePosition())); + + } + + /** + * This method is used to set the back right encoder to the true position + */ + public void setBackRightEncoders() { + m_SteerBackRight.setSensorPosition((int) Math + .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackRight.getAbsolutePosition())); + } + + /** + * This method is used to set the back left encoder to the true position + */ + public void setBackLeftEncoders() { + m_SteerBackLeft.setSensorPosition((int) Math + .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackLeft.getAbsolutePosition())); + } + + // To control each steering individually with a PID + + /** + * This method is used to set the front right steering motor to a certain angle. This uses a PID + * controller. + * + * @param angle The angle to set the front right wheel to + */ + public void setFrontRightAngle(double angle) { + // log("Angle: " + getFrontRight() + " || Motor angle: " + 360.0/ 2048.0 * + // m_SteerFrontRight.getSensorPosition()); + m_SteerFrontRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); + } + + /** + * This method is used to set the front left steering motor to a certain angle. This uses a PID + * controller. + * + * @param angle The angle to set the front left wheel to + */ + public void setFrontLeftAngle(double angle) { + // log("Angle: " + getFrontLeft() + " || Motor angle: " + Math.pow((2048.0/360.0 * + // (150.0/7.0)),-1) * m_SteerFrontLeft.getSensorPosition()); + m_SteerFrontLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); + } + + /** + * This method is used to set the back right steering motor to a certain angle. This uses a PID + * controller. + * + * @param angle The angle to set the back right wheel to + */ + public void setBackRightAngle(double angle) { + // log("Angle: " + getBackRight() + " || Motor angle: " + + // m_SteerBackRight.getSensorPosition()); + m_SteerBackRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); + } + + /** + * This method is used to set the back left steering motor to a certain angle. This uses a PID + * controller. + * + * @param angle The angle to set the back left wheel to + */ + public void setBackLeftAngle(double angle) { + // log("Angle: " + getBackLeft() + " || Motor angle: " + + // m_SteerBackLeft.getSensorPosition()); + m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); + } + + /** + * Method to configure PID values. The values were pre-tuned and are not expected to change. + */ + public void configPID() { + // PID for turning the various steering motors. Here is a good link to a tuning website: + // https://www.robotsforroboticists.com/pid-control/ + m_SteerFrontRight.setP(0.2); + m_SteerFrontRight.setI(0); + m_SteerFrontRight.setD(0.1); + m_SteerFrontRight.setFF(0); + + m_SteerFrontLeft.setP(0.2); + m_SteerFrontLeft.setI(0); + m_SteerFrontLeft.setD(0.1); + m_SteerFrontLeft.setFF(0); + + m_SteerBackRight.setP(0.2); + m_SteerBackRight.setI(0); + m_SteerBackRight.setD(0.1); + m_SteerBackRight.setFF(0); + + m_SteerBackLeft.setP(0.2); + m_SteerBackLeft.setI(0); + m_SteerBackLeft.setD(0.1); + // m_SteerBackLeft.setFF(0); + + // pid values from sds for Flacons 500: P = 0.2 I = 0.0 D = 0.1 FF = 0.0 + + // Code to invert sensors if needed. Recommended to do this in phoenix tuner. + // m_SteerFrontRight.setSensorInverted(false); + // m_SteerFrontLeft.setSensorInverted(false); + // m_SteerBackRight.setSensorInverted(false); + // m_SteerBackLeft.setSensorInverted(false); + } + + // Methods to get the encoder values, the encoders are in degrees from -180 to 180. To change + // that, we need to change the syntax and use getPosition() + public double getFrontRight() { + return e_FrontRight.getAbsolutePosition(); + } + + public double getFrontLeft() { + return e_FrontLeft.getAbsolutePosition(); + } + + public double getBackRight() { + return e_BackRight.getAbsolutePosition(); + } + + public double getBackLeft() { + return e_BackLeft.getAbsolutePosition(); + } + + public PointDir getCurrentPosition() { + return currentPosition; + } + + public void setCross() { + checkContextOwnership(); + setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); + setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); + setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); + setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); + } + + public void setCurrentPosition(Point P) { + swerveOdometry.setCurrentPosition(P); + } + + public void resetCurrentPosition() { + swerveOdometry.setCurrentPosition(new Point(0, 0)); + } + + /** + * This method is used to reset the drive encoders. + */ + public void resetDriveEncoders() { + m_DriveBackLeft.setSensorPosition(0); + m_DriveBackRight.setSensorPosition(0); + m_DriveFrontLeft.setSensorPosition(0); + m_DriveFrontRight.setSensorPosition(0); + } + + // Odometry + @Override + public void run() { + currentPosition = swerveOdometry.run(); + log(currentPosition.toString()); + SmartDashboard.putNumber("Front Right Motor Encoder", m_SteerFrontRight.getSensorPosition()); + SmartDashboard.putNumber("Front Left Motor Encoder", m_SteerFrontLeft.getSensorPosition()); + SmartDashboard.putNumber("Back Right Motor Encoder", m_SteerBackRight.getSensorPosition()); + SmartDashboard.putNumber("Back Left Motor Encoder", m_SteerBackLeft.getSensorPosition()); + } +} + +// AS \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Gyro.java b/src/main/java/com/team766/robot/mechanisms/Gyro.java new file mode 100644 index 00000000..d96ec9b9 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Gyro.java @@ -0,0 +1,68 @@ +package com.team766.robot.mechanisms; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.I2C.Port; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.team766.framework.Mechanism; +import com.team766.hal.EncoderReader; +import com.team766.hal.RobotProvider; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; +import com.team766.hal.MotorController; +import com.team766.logging.Category; +//import edu.wpi.first.wpilibj.I2C.Port; +//import com.team766.hal.GyroReader; +//import com.kauailabs.navx.frc.*; +import com.ctre.phoenix.sensors.Pigeon2; + +public class Gyro extends Mechanism { + Pigeon2 g_gyro = new Pigeon2(0, "Swervavore"); + double[] gyroArray = new double[3]; + private RateLimiter l_loggingRate = new RateLimiter(0.05); + public Gyro() { + loggerCategory = Category.GYRO; + } + public void resetGyro(){ + g_gyro.setYaw(0); + } + + public void resetGyro180() { + g_gyro.setYaw(180); + } + + public double getGyroPitch() { + double angle = g_gyro.getPitch(); + return angle; + } + + public double getGyroYaw() { + double angle = -1* g_gyro.getYaw(); + return angle; + } + + public double getGyroRoll() { + double angle = g_gyro.getRoll(); + return angle; + } + + @Override + public void run() { + if (l_loggingRate.next()) { + gyroArray[0] = getGyroYaw(); + gyroArray[1] = getGyroPitch(); + gyroArray[2] = getGyroRoll(); + SmartDashboard.putNumber("Yaw", gyroArray[0]); + SmartDashboard.putNumber("Pitch", gyroArray[1]); + SmartDashboard.putNumber("Roll", gyroArray[2]); + g_gyro.getYawPitchRoll(gyroArray); + } + } + + /** + * @return combined pitch and roll values of gyro + */ + public double getAbsoluteTilt() { + return Math.toDegrees(Math.acos(Math.cos(Math.toRadians(getGyroRoll()))*Math.cos(Math.toRadians(getGyroPitch())))); + } + +} \ No newline at end of file From 318673e314801550924cfcfccf87e5549da9f8d4 Mon Sep 17 00:00:00 2001 From: Max <> Date: Fri, 21 Jul 2023 18:36:08 -0700 Subject: [PATCH 23/29] first swerve vision test --- .../team766/robot/mechanisms/januaryTag.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 2f70c151..8164bb35 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.team766.config.ConfigFileReader; +import com.team766.controllers.PIDController; import com.team766.framework.Context; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; @@ -54,7 +55,16 @@ public class januaryTag extends Mechanism{ private double offsetX; private double offsetY; private Transform3d offset; + private Drive drive; + + + private PIDController motionX; + private PIDController motionY; public januaryTag(){ + + motionX = new PIDController(0.0,0.0,0.0,-0.2,0.2,0.5); + motionY = new PIDController(0,0,0,-0.2,0.2,1); + drive = new Drive(); loggerCategory = Category.MECHANISMS; camera1 = new PhotonCamera("januaryTag"); leftMotor = RobotProvider.instance.getMotor("leftMotor"); @@ -68,6 +78,10 @@ public januaryTag(){ offsetY = 0; offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); checkContextOwnership(); + + motionX.setP(0.01); + motionY.setP(0.01); + } //Manually move the robot public void manual(double left, double right){ @@ -75,6 +89,25 @@ public void manual(double left, double right){ rightMotor.set(right); } + public void swerveCalculate(){ + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + + double xCurPos = targetTransform.getX(); + double yCurPos = targetTransform.getY(); + + motionX.setSetpoint(0); + motionY.setSetpoint(0); + + motionX.calculate(xCurPos); + motionY.calculate(yCurPos); + + double xSpeed = motionX.getOutput(); + double ySpeed = motionY.getOutput(); + + drive.drive2D(xSpeed, ySpeed); + + } + /* * This method returns the best target that the camera is currently tracking From 3278d8bd01134200fe81210c00af13a747d8f9ff Mon Sep 17 00:00:00 2001 From: Max Date: Wed, 26 Jul 2023 10:10:16 -0700 Subject: [PATCH 24/29] test P.V. swerve code --- .../com/team766/robot/mechanisms/Drive.java | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 82528825..c3d52ef6 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -11,9 +11,11 @@ import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.simulator.ProgramInterface.RobotMode; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; +import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; @@ -23,6 +25,7 @@ import com.team766.odometry.PointDir; import com.team766.hal.MotorControllerCommandFailedException; import com.team766.robot.constants.*; +import com.team766.controllers.*; public class Drive extends Mechanism { @@ -52,9 +55,20 @@ public class Drive extends Mechanism { private Point[] wheelPositions; private Odometry swerveOdometry; + + private PIDController pidX; + private PIDController pidY; + + private boolean yDone; + public Drive() { loggerCategory = Category.DRIVE; + + yDone = false; + + pidX = new PIDController(0, 0, 0, 0, -0.2, 0.2, 0.5); + pidY = new PIDController(0, 0, 0, 0, -0.2, 0.2, 0.5); // Initializations of motors // Initialize the drive motors m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); @@ -363,6 +377,55 @@ public void stopSteerMotors() { m_SteerBackLeft.stopMotor(); } + public void PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBestTarget){ + double curX = bestTrackedTarget.getX(); + double curY = bestTrackedTarget.getY(); + + double curAng = notBestTarget.getSkew(); + + pidX.setSetpoint(0); + pidY.setSetpoint(0); + + pidX.calculate(curX); + pidY.calculate(curY); + + double outX = pidX.getOutput(); + double outY = pidX.getOutput(); + + if(outY == 0){ + yDone = true; + }else{ + yDone = false; + } + + + + if(yDone){ + setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outX); + m_DriveBackRight.set(outX); + m_DriveFrontLeft.set(outX); + m_DriveFrontRight.set(outX); + }else{ + setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outY); + m_DriveBackRight.set(outY); + m_DriveFrontLeft.set(outY); + m_DriveFrontRight.set(outY); + } + + if(Math.abs(curAng) <= 2){ + //do nothing + }else if(outX == 0 && outY == 0){ + if(curAng > 0){ + setAllAngles(curAng - 0.15); + }else{ + setAllAngles(curAng + 0.15); + } + } + + } + /** * This method is the main method for driving the robot, using the joystick values. * @@ -578,6 +641,17 @@ public void setBackLeftAngle(double angle) { m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); } +/** + * This is a method to set all of the motors to a certian angle + * + * @param angle The angle to set all of the wheels to + */ + public void setAllAngles(double angle){ + setBackLeftAngle(angle); + setBackRightAngle(angle); + setFrontLeftAngle(angle); + setFrontRightAngle(angle); + } /** * Method to configure PID values. The values were pre-tuned and are not expected to change. */ From 4184a1ced0a41efea8a606e91f1b76d5ab467b70 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 26 Jul 2023 18:12:05 -0700 Subject: [PATCH 25/29] e --- src/main/java/com/team766/robot/OI.java | 330 +++++++++++------- src/main/java/com/team766/robot/Robot.java | 4 +- .../com/team766/robot/mechanisms/Drive.java | 2 +- .../robot/mechanisms/januaryTagException.java | 6 + 4 files changed, 204 insertions(+), 138 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 53cb8e18..285c9970 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -72,81 +72,89 @@ public void run(Context context){ public OI() { loggerCategory = Category.OPERATOR_INTERFACE; - - leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); - rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); - controlPanel = RobotProvider.instance.getJoystick(InputConstants.CONTROL_PANEL); + j1 = RobotProvider.instance.getJoystick(0); + //leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); + //rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); + //controlPanel = RobotProvider.instance.getJoystick(InputConstants.CONTROL_PANEL); } public void run(Context context) { + context.takeOwnership(Robot.JT); context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.arms); - context.takeOwnership(Robot.storage); - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.grabber); + + + Robot.JT.swerveCalculate(); + if(j1.getButton(1)){ + Robot.drive.drive2D(Robot.JT.getXSpeed(), Robot.JT.getYSpeed()); + } + + // context.takeOwnership(Robot.intake); + // context.takeOwnership(Robot.arms); + // context.takeOwnership(Robot.storage); + // context.takeOwnership(Robot.gyro); + // context.takeOwnership(Robot.grabber); - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); - while (true) { - context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); - RobotProvider.instance.refreshDriverStationData(); + // while (true) { + // context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); + // RobotProvider.instance.refreshDriverStationData(); - // Add driver controls here - make sure to take/release ownership - // of mechanisms when appropriate. + // // Add driver controls here - make sure to take/release ownership + // // of mechanisms when appropriate. - leftJoystickX = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); - leftJoystickY = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)); - rightJoystickX = Drive.correctedJoysticks(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); + // leftJoystickX = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); + // leftJoystickY = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)); + // rightJoystickX = Drive.correctedJoysticks(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); + // Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - // if (DriverStation.getAlliance() == Alliance.Red) { - // SmartDashboard.putString("Alliance", "RED"); - // } else if (DriverStation.getAlliance() == Alliance.Blue) { - // SmartDashboard.putString("Alliance", "BLUE"); - // } else { - // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); - // } + // // if (DriverStation.getAlliance() == Alliance.Red) { + // // SmartDashboard.putString("Alliance", "RED"); + // // } else if (DriverStation.getAlliance() == Alliance.Blue) { + // // SmartDashboard.putString("Alliance", "BLUE"); + // // } else { + // // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); + // // } - if (controlPanel.getButtonPressed(InputConstants.RESET_GYRO)) { - Robot.gyro.resetGyro(); - } + // if (controlPanel.getButtonPressed(InputConstants.RESET_GYRO)) { + // Robot.gyro.resetGyro(); + // } - if (controlPanel.getButtonPressed(InputConstants.RESET_POS)) { - Robot.drive.resetCurrentPosition(); - } + // if (controlPanel.getButtonPressed(InputConstants.RESET_POS)) { + // Robot.drive.resetCurrentPosition(); + // } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { - RightJoystick_Y = rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - } else { - RightJoystick_Y = 0; - } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)/2; - } else { - rightJoystickX = 0; - } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { - RightJoystick_Theta = rightJoystick.getAxis(InputConstants.AXIS_TWIST); - } else { - RightJoystick_Theta = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { - leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - } else { - leftJoystickY = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); - } else { - leftJoystickX = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { - LeftJoystick_Theta = leftJoystick.getAxis(InputConstants.AXIS_TWIST); - } else { - LeftJoystick_Theta = 0; - } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + // RightJoystick_Y = rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + // } else { + // RightJoystick_Y = 0; + // } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + // rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)/2; + // } else { + // rightJoystickX = 0; + // } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { + // RightJoystick_Theta = rightJoystick.getAxis(InputConstants.AXIS_TWIST); + // } else { + // RightJoystick_Theta = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + // leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + // } else { + // leftJoystickY = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + // leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + // } else { + // leftJoystickX = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { + // LeftJoystick_Theta = leftJoystick.getAxis(InputConstants.AXIS_TWIST); + // } else { + // LeftJoystick_Theta = 0; + // } // Sets intake state based on button pressed if (controlPanel.getButtonPressed(InputConstants.INTAKE)){ @@ -170,7 +178,9 @@ public void run(Context context) { Robot.storage.beltIdle(); intakeState = IntakeState.IDLE; } - } + } */ + + /* if (controlPanel.getButtonPressed(InputConstants.OUTTAKE)){ if (intakeState == IntakeState.IDLE){ Robot.intake.reverseIntake(); @@ -181,89 +191,141 @@ public void run(Context context) { Robot.storage.beltIdle(); intakeState = IntakeState.IDLE; } - } + } */ - // Sets the wheels to the cross position if the cross button is pressed - if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { - if (!isCross) { - context.startAsync(new setCross()); - } - isCross = !isCross; - } + // // Sets the wheels to the cross position if the cross button is pressed + // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + // if (!isCross) { + // context.startAsync(new setCross()); + // } + // isCross = !isCross; + // } - SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + // SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); - // Moves the robot if there are joystick inputs - if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { - context.takeOwnership(Robot.drive); - // If a button is pressed, drive is just fine adjustment - if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { - Robot.drive.swerveDrive((leftJoystickX * FINE_DRIVING_COEFFICIENT), (-leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); - } else { - Robot.drive.swerveDrive((leftJoystickX), (-leftJoystickY), (rightJoystickX)); - } - } else if (!isCross) { - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - } + // // Moves the robot if there are joystick inputs + // if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { + // context.takeOwnership(Robot.drive); + // // If a button is pressed, drive is just fine adjustment + // if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + // Robot.drive.swerveDrive((leftJoystickX * FINE_DRIVING_COEFFICIENT), (-leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); + // } else { + // Robot.drive.swerveDrive((leftJoystickX), (-leftJoystickY), (rightJoystickX)); + // } + // } else if (!isCross) { + // Robot.drive.stopDriveMotors(); + // Robot.drive.stopSteerMotors(); + // } + + // // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + // // context.startAsync(new setCross()); + // // } + + // if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { + // Robot.arms.pidForArmOne(-17.379); + // Robot.arms.pidForArmTwo(-66.61); + // } + // if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { + // Robot.arms.pidForArmOne(3.7765); + // Robot.arms.pidForArmTwo(-97.703); + // } + // if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-90); + // } + // if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-76.964); + // } + // if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-152.387); + // } + // /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-73.664); + // } */ + // if(controlPanel.getButton(InputConstants.NUDGE_UP)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); + // } + + // if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); + // } + // Robot.arms.logs(); + + // if(controlPanel.getButton(InputConstants.GRAB_IN)){ + // Robot.grabber.grabberPickUp(); + // } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ + // Robot.grabber.grabberLetGo(); + // } else { + // Robot.grabber.grabberStop(); + // } + + // if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { + // Robot.arms.brake(); + // } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { + // Robot.arms.coast(); + // } + + // if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ + // Robot.arms.armStop(); + // } // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { // context.startAsync(new setCross()); // } - if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { - Robot.arms.pidForArmOne(-17.379); - Robot.arms.pidForArmTwo(-66.61); - } - if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { - Robot.arms.pidForArmOne(3.7765); - Robot.arms.pidForArmTwo(-97.703); - } - if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { - Robot.arms.pidForArmOne(17.269); - Robot.arms.pidForArmTwo(-90); - } - if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { - Robot.arms.pidForArmOne(22.73); - Robot.arms.pidForArmTwo(-76.964); - } - if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { - Robot.arms.pidForArmOne(17.269); - Robot.arms.pidForArmTwo(-152.387); - } - /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ - Robot.arms.pidForArmOne(22.73); - Robot.arms.pidForArmTwo(-73.664); - } - if(controlPanel.getButton(InputConstants.NUDGE_UP)){ - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); - } + // if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { + // Robot.arms.pidForArmOne(-17.379); + // Robot.arms.pidForArmTwo(-66.61); + // } + // if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { + // Robot.arms.pidForArmOne(3.7765); + // Robot.arms.pidForArmTwo(-97.703); + // } + // if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-90); + // } + // if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-76.964); + // } + // if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-152.387); + // } + // /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-73.664); + // } */ + // if(controlPanel.getButton(InputConstants.NUDGE_UP)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); + // } - if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); - } - Robot.arms.logs(); + // if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); + // } + // Robot.arms.logs(); - if(controlPanel.getButton(InputConstants.GRAB_IN)){ - Robot.grabber.grabberPickUp(); - } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ - Robot.grabber.grabberLetGo(); - } else { - Robot.grabber.grabberStop(); - } + // if(controlPanel.getButton(InputConstants.GRAB_IN)){ + // Robot.grabber.grabberPickUp(); + // } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ + // Robot.grabber.grabberLetGo(); + // } else { + // Robot.grabber.grabberStop(); + // } - if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { - Robot.arms.brake(); - } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { - Robot.arms.coast(); - } + // if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { + // Robot.arms.brake(); + // } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { + // Robot.arms.coast(); + // } - if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ - Robot.arms.armStop(); - } + // if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ + // Robot.arms.armStop(); + // } } - }*/ - -} diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 06ef61b7..02821c09 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -19,8 +19,6 @@ public static void robotInit() { drive = new Drive(); // arms = new Arms(); gyro = new Gyro(); - // grabber = new Grabber(); - JanuaryTag = new januaryTag(); - //jFrame JFrame = new jFrame(); + //grabber = new Grabber(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index c3d52ef6..31c362ec 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -390,7 +390,7 @@ public void PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBe pidY.calculate(curY); double outX = pidX.getOutput(); - double outY = pidX.getOutput(); + double outY = pidY.getOutput(); if(outY == 0){ yDone = true; diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java index 0d1b6216..f423585d 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java @@ -5,3 +5,9 @@ public januaryTagException (String errorMessage) { super(errorMessage); } } + +public class januaryTagException extends RuntimeException { + public januaryTagException (String errorMessage) { + super(errorMessage); + } +} \ No newline at end of file From d091c3eca148d1f16f985506215f81123a5da490 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 12 Aug 2023 21:27:16 -0700 Subject: [PATCH 26/29] Update Drive.java from unmerged code on main --- .../com/team766/robot/mechanisms/Drive.java | 89 ++++++++++++------- 1 file changed, 58 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 31c362ec..14ba655c 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -58,6 +58,7 @@ public class Drive extends Mechanism { private PIDController pidX; private PIDController pidY; + private PIDController pidPythag; private boolean yDone; @@ -67,8 +68,10 @@ public Drive() { yDone = false; - pidX = new PIDController(0, 0, 0, 0, -0.2, 0.2, 0.5); - pidY = new PIDController(0, 0, 0, 0, -0.2, 0.2, 0.5); + pidX = new PIDController(0.1, 0, 0, 0, -0.2, 0.2, 0.5); + pidY = new PIDController(0.5, 0, 0, 0, -0.2, 0.2, 0.5); + pidPythag = new PIDController(0.2, 0, 0, 0, 0.2, -0.2, 0.1); + // Initializations of motors // Initialize the drive motors m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); @@ -179,7 +182,7 @@ public double getAngle(double LR, double FB) { */ public boolean withinHalfACircle(double angle1, double angle2) { angle1 = mod(angle1, 360); - angle2 = mod(angle2, 360); + angle2 = mod(angle2, 360); if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) { angle2 += 360; } @@ -377,51 +380,74 @@ public void stopSteerMotors() { m_SteerBackLeft.stopMotor(); } - public void PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBestTarget){ + public int PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBestTarget){ double curX = bestTrackedTarget.getX(); double curY = bestTrackedTarget.getY(); - + log("cur: " + curX + " e: " + curY); double curAng = notBestTarget.getSkew(); + + double pythagorean = curX * curX + curY * curY; + double sqrtPythag = Math.sqrt(pythagorean); + double angleB = Math.asin(1/sqrtPythag*curX); pidX.setSetpoint(0); pidY.setSetpoint(0); + pidPythag.setSetpoint(0); pidX.calculate(curX); pidY.calculate(curY); + pidPythag.calculate(sqrtPythag); - double outX = pidX.getOutput(); + double outX = -1 * pidX.getOutput(); double outY = pidY.getOutput(); + double outPythag = -1 * pidPythag.getOutput(); - if(outY == 0){ + log("X: " + outX); + log("Y: " + outY); + + if(curY <=0.2 && curY >= -0.2){ yDone = true; }else{ yDone = false; } - - - if(yDone){ - setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED - m_DriveBackLeft.set(outX); - m_DriveBackRight.set(outX); - m_DriveFrontLeft.set(outX); - m_DriveFrontRight.set(outX); + setAllAngles(angleB); + + m_DriveBackLeft.set(outPythag); + m_DriveBackRight.set(outPythag); + m_DriveFrontLeft.set(outPythag); + m_DriveFrontRight.set(outPythag); + + // if(yDone){ + // setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + // m_DriveBackLeft.set(outX); + // m_DriveBackRight.set(outX); + // m_DriveFrontLeft.set(outX); + // m_DriveFrontRight.set(outX); + // }else{ + // setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + // m_DriveBackLeft.set(outY); + // m_DriveBackRight.set(outY); + // m_DriveFrontLeft.set(outY); + // m_DriveFrontRight.set(outY); + // } + // log("currAng: " + curAng); + // if(Math.abs(curAng) <= 2){ + // //do nothing + // }else if(outX == 0 && outY == 0){ + // if(curAng > 0){ + // setAllAngles(curAng - 0.15); + // }else{ + // setAllAngles(curAng + 0.15); + // } + // } + + if(curY <=0.2 && curY >= -0.2 && curX <= 0.2 && curX >= -0.2){ + log("DONE"); + return 1; }else{ - setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED - m_DriveBackLeft.set(outY); - m_DriveBackRight.set(outY); - m_DriveFrontLeft.set(outY); - m_DriveFrontRight.set(outY); - } - - if(Math.abs(curAng) <= 2){ - //do nothing - }else if(outX == 0 && outY == 0){ - if(curAng > 0){ - setAllAngles(curAng - 0.15); - }else{ - setAllAngles(curAng + 0.15); - } + log("Q: " + curY + " : " + curX); + return 0; } } @@ -513,6 +539,7 @@ public void swerveDrive(PointDir joystick) { swerveDrive(-1 * joystick.getY(), -1 * joystick.getX(), joystick.getHeading()); } + /** * This method is used to simply turn the robot without moving it * @@ -747,4 +774,4 @@ public void run() { } } -// AS \ No newline at end of file +// AS From d3b5144fdc07392c6aa80d52d2014df5734bb17d Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 12 Aug 2023 21:27:51 -0700 Subject: [PATCH 27/29] Update januaryTag.java from correct unmerged code on main --- src/main/java/com/team766/robot/mechanisms/januaryTag.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java index 8164bb35..1c8d5c73 100644 --- a/src/main/java/com/team766/robot/mechanisms/januaryTag.java +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -89,6 +89,11 @@ public void manual(double left, double right){ rightMotor.set(right); } + public Transform3d getBestTag(){ + return getBestCameraToTarget(getBestTrackedTarget()); + } + + public void swerveCalculate(){ Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); From c7d4364dfb9e922f3f08694a6be4ee39351be639 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 13 Aug 2023 07:22:41 -0700 Subject: [PATCH 28/29] Update OI.java --- src/main/java/com/team766/robot/OI.java | 29 +++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 285c9970..98fb8bcd 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -1,6 +1,7 @@ package com.team766.robot; import com.team766.framework.Procedure; +import org.photonvision.targeting.PhotonTrackedTarget; import com.team766.framework.Context; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; @@ -11,6 +12,7 @@ import com.team766.robot.mechanisms.*; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -27,12 +29,31 @@ public OI(){ public void run(Context context){ context.takeOwnership(Robot.JanuaryTag); - Robot.JanuaryTag.setDeadzones(2, 2); + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.intake); + + Transform3d btt = Robot.JanuaryTag.getBestTag(); + PhotonTrackedTarget t = Robot.JanuaryTag.getBestTrackedTarget(); + if(joystickOne.getButton(1)){ - Robot.JanuaryTag.go(); - } else{ - Robot.JanuaryTag.manual(joystickOne.getAxis(1), joystickOne.getAxis(1)); + int output = Robot.drive.PhotonDrive(btt, t); + + if(output == 1){ + Robot.intake.reverseIntake(); + Robot.storage.beltOut(); + }else{ + Robot.intake.stopIntake(); + Robot.storage.beltIdle(); + } + + } + + // if(joystickOne.getButtonReleased(1)){ + // Robot.drive.drive2D(0,0); + // } + + } /* private JoystickReader leftJoystick; From ae132a316f256588b903d5ecc33ddaab2b6e37e0 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Wed, 23 Aug 2023 18:09:33 -0700 Subject: [PATCH 29/29] Update Drive.java RELIABLE ish januarytag code --- .../com/team766/robot/mechanisms/Drive.java | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 14ba655c..33b1f0e4 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -411,26 +411,26 @@ public int PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBes yDone = false; } - setAllAngles(angleB); + // setAllAngles(angleB); - m_DriveBackLeft.set(outPythag); - m_DriveBackRight.set(outPythag); - m_DriveFrontLeft.set(outPythag); - m_DriveFrontRight.set(outPythag); + // m_DriveBackLeft.set(outPythag); + // m_DriveBackRight.set(outPythag); + // m_DriveFrontLeft.set(outPythag); + // m_DriveFrontRight.set(outPythag); - // if(yDone){ - // setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED - // m_DriveBackLeft.set(outX); - // m_DriveBackRight.set(outX); - // m_DriveFrontLeft.set(outX); - // m_DriveFrontRight.set(outX); - // }else{ - // setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED - // m_DriveBackLeft.set(outY); - // m_DriveBackRight.set(outY); - // m_DriveFrontLeft.set(outY); - // m_DriveFrontRight.set(outY); - // } + if(yDone){ + setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outX); + m_DriveBackRight.set(outX); + m_DriveFrontLeft.set(outX); + m_DriveFrontRight.set(outX); + }else{ + setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outY); + m_DriveBackRight.set(outY); + m_DriveFrontLeft.set(outY); + m_DriveFrontRight.set(outY); + } // log("currAng: " + curAng); // if(Math.abs(curAng) <= 2){ // //do nothing