diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java index 0349cca..f362565 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; @@ -15,7 +16,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; @Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp") - +@Disabled public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine { CompetitionRobotV1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java index c375ef1..6707c8d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; @@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; @Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp") +@Disabled public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java index eb9a3d8..eb17a25 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; @@ -15,6 +16,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; @Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp") +@Disabled public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java index fb49c6c..472b8e8 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; @@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; @Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp") +@Disabled public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java index 38cf137..ea902ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -20,6 +20,7 @@ public class DriveToCoordinatesState extends CyberarmState { public boolean posSpecific; public double maxXPower; public double maxYPower; + public double maxHPower; private String actionName; public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { @@ -31,6 +32,7 @@ public class DriveToCoordinatesState extends CyberarmState { this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value(); this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value(); + this.maxHPower = robot.configuration.variable(groupName, actionName, "maxHPower").value(); this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value(); this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value(); this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value(); @@ -45,22 +47,15 @@ public class DriveToCoordinatesState extends CyberarmState { robot.xTarget = xTarget; robot.yMaxPower = maxYPower; robot.xMaxPower = maxXPower; + robot.hMaxPower = maxHPower; } else { setHasFinished(true); } - Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget); - } @Override public void exec() { -// robot.yMaxPower = maxYPower; -// robot.xMaxPower = maxXPower; -// robot.hTarget = hTarget; -// robot.yTarget = yTarget; -// robot.xTarget = xTarget; if (posSpecific) { if (objectPos != robot.objectPos) { @@ -73,7 +68,8 @@ public class DriveToCoordinatesState extends CyberarmState { } if (Math.abs(robot.positionX - robot.xTarget) < 5 - && Math.abs(robot.positionY - robot.yTarget) < 5) { + && Math.abs(robot.positionY - robot.yTarget) < 5 + && Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) > 2) { setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java index ebbee2f..422c5fe 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java @@ -18,6 +18,7 @@ public class DriveToCoordinatesTask extends CyberarmState { public void exec() { robot.XDrivePowerModifier(); robot.YDrivePowerModifier(); + robot.HDrivePowerModifier(); robot.DriveToCoordinates(); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index bf5ce25..b98924e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -103,11 +103,13 @@ public class CompetitionRobotV1 extends Robot { public double yMaxPower = 1; public double xMaxPower = 1; + public double hMaxPower = 1; public double pidX; public double pidY; + public double pidH; public double rawPidX; public double rawPidY; - + public double rawPidH; public double xVelocity; public double yVelocity; public double deltaTime = 0; @@ -284,7 +286,7 @@ public class CompetitionRobotV1 extends Robot { * A Controller taking error of the heading position and converting to a power in the direction of a target. * @param reference reference is the target position * @param current current is the measured sensor value. - * @return A power to the target the position + * @return A power to the target position * */ public double HeadingPIDControl(double reference, double current){ @@ -349,15 +351,27 @@ public class CompetitionRobotV1 extends Robot { pidX = rawPidX; } } + public void HDrivePowerModifier () { + + rawPidH = HeadingPIDControl(Math.toRadians(hTarget), imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + if (Math.abs(rawPidH) > hMaxPower) { + if (rawPidH < 0) { + pidH = -hMaxPower; + } else { + pidH = hMaxPower; + } + } else { + pidH = rawPidH; + } + } public void DriveToCoordinates () { // determine the powers needed for each direction // this uses PID to adjust needed Power for robot to move to target double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); - double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); + double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(pidH), 1); // field oriented math, (rotating the global field to the relative field) double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading); @@ -365,10 +379,10 @@ public class CompetitionRobotV1 extends Robot { // finding approximate power for each wheel. - frontLeftPower = (rotY + rotX + rx) / denominator; - backLeftPower = (rotY - rotX + rx) / denominator; - frontRightPower = (rotY - rotX - rx) / denominator; - backRightPower = (rotY + rotX - rx) / denominator; + frontLeftPower = (rotY + rotX + pidH) / denominator; + backLeftPower = (rotY - rotX + pidH) / denominator; + frontRightPower = (rotY - rotX - pidH) / denominator; + backRightPower = (rotY + rotX - pidH) / denominator; // apply my powers frontLeft.setPower(frontLeftPower); @@ -412,12 +426,6 @@ public class CompetitionRobotV1 extends Robot { } -// pidController.setPID(p, i, d); -// int armPos = clawArm.getCurrentPosition(); -// double pid = pidController.calculate(armPos, target); -// -// power = pid; - clawArm.setTargetPosition(target); clawArm.setPower(0.4);