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 4276815..eb9a3d8 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 @@ -41,17 +41,17 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine { // addTask(new ClawArmControlTask(robot)); this.robot.setup(); - addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0")); +// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0")); - addState(new CameraVisionState(robot)); +// addState(new CameraVisionState(robot)); - addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1")); +// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1")); // drive to the left, center, or right spike mark addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0")); - addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1")); - addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0")); - addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0")); addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4")); 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 f84336a..5a9c504 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 @@ -12,11 +12,11 @@ import dev.cyberarm.engine.V2.CyberarmState; public class DriveToCoordinatesState extends CyberarmState { CompetitionRobotV1 robot; - public double xTarget; - public double yTarget; - public double hTarget; - public double maxVelocityX; - public double maxVelocityY; + public static double xTarget; + public static double yTarget; + public static double hTarget; + public static double maxVelocityX; + public static double maxVelocityY; public double maxVelocityH; public boolean posAchieved = false; public boolean armDrive; @@ -69,7 +69,7 @@ public class DriveToCoordinatesState extends CyberarmState { if (posSpecific) { if (objectPos != robot.objectPos) { // enter the ending loop - setHasFinished(true); +// setHasFinished(true); } else { if (armDrive) { @@ -78,7 +78,7 @@ public class DriveToCoordinatesState extends CyberarmState { if (Math.abs(robot.positionX - robot.xTarget) < 5 && Math.abs(robot.positionY - robot.yTarget) < 5) { - setHasFinished(true); +// setHasFinished(true); } } } else { @@ -88,7 +88,7 @@ public class DriveToCoordinatesState extends CyberarmState { if (Math.abs(robot.positionX - robot.xTarget) < 5 && Math.abs(robot.positionY - robot.yTarget) < 5) { - setHasFinished(true); +// setHasFinished(true); } } } 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 f468362..5a4c0fe 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -112,7 +112,7 @@ public class CompetitionRobotV1 extends Robot { public double xVelocity; public double yVelocity; - public double deltaTime; + public double deltaTime = 0; //-------------------------------------------------------------------------------------------------------------- arm sequence variables: @@ -299,27 +299,6 @@ public class CompetitionRobotV1 extends Robot { double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi); return output; } - public double XVeloPIDControl ( double reference, double current){ - double error = (reference - current); - xVeloIntegralSum += error * xVeloTimer.seconds(); - double derivative = (error - xVeloLastError) / xVeloTimer.seconds(); - - xTimer.reset(); - - double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi); - return output; - } - - public double YVeloPIDControl ( double reference, double current){ - double error = (reference - current); - yVeloIntegralSum += error * yVeloTimer.seconds(); - double derivative = (error - yVeloLastError) / xTimer.seconds(); - - xTimer.reset(); - - double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi); - return output; - } public double XPIDControl ( double reference, double current){ double error = (reference - current); @@ -376,10 +355,6 @@ public class CompetitionRobotV1 extends Robot { public void DriveToCoordinates () { // determine the powers needed for each direction // this uses PID to adjust needed Power for robot to move to target - targetVelocityX = XPIDControl(xTarget, positionX); - targetVelocityY = YPIDControl(yTarget, positionY); - XVeloPIDControl(targetVelocityX, xVelocity); - YVeloPIDControl(targetVelocityY, yVelocity); double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java index 32998df..a324562 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -339,6 +339,7 @@ public class CompetitionTeleOpState extends CyberarmState { robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); lastExecutedTime = System.currentTimeMillis(); + } @Override