diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java index 03b110f..29ae33f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java @@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "BURNSVILLE blue audience") +@Autonomous(name = "old BURNSVILLE blue audience") public class Competition2BlueBackStage extends CyberarmEngine { @@ -43,9 +43,9 @@ public class Competition2BlueBackStage extends CyberarmEngine { addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1")); // drive to the left, center, or right spike mark - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0")); // place pixel addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1")); @@ -57,49 +57,49 @@ public class Competition2BlueBackStage extends CyberarmEngine { addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3")); // drive back and away from the spike mark (x,y) (1050, 1000) H = 0 - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0")); // drive to the middle truss (right version) (1130,980) H = -90 - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0")); // drive to hover mode to clear the middle truss addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1")); // drive under the middle truss across the field (right version) (1170,1080) H = -90 - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0")); // drive to deposit mode to place on back drop addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1")); // drive to back drop - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0")); // open claw to deposit gold addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1")); // drive back from backdrop - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0")); // close left claw addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1")); // drive to parking spot - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0")); -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0")); // arm to collect pos addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1")); 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 new file mode 100644 index 0000000..5774812 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java @@ -0,0 +1,65 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmControlTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Burnsville audience blue") + +public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Burnsville audience blue"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); +// addTask(new ClawArmControlTask(robot)); + + this.robot.setup(); + addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0")); + +// addState(new CameraVisionState(robot)); + + addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1")); + + // drive to the left, center, or right spike mark + addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-1")); +// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0")); + + // place pixel + addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1")); + + // drive to search pos + addState(new ClawArmState(robot,"Burnsville audience blue", "0-02-2")); + + // close right finger + addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-3")); + } + +} 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 new file mode 100644 index 0000000..ba4b265 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java @@ -0,0 +1,65 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Burnsville audience red") + +public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Burnsville audience red"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); +// addTask(new ClawArmControlTask(robot)); + + this.robot.setup(); + addState(new ClawArmState(robot,"Burnsville audience red", "0-01-0")); + +// addState(new CameraVisionState(robot)); + + addState(new ClawArmState(robot,"Burnsville audience red", "0-01-1")); + + // drive to the left, center, or right spike mark + addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-1")); +// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0")); + + // place pixel + addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-1")); + + // drive to search pos + addState(new ClawArmState(robot,"Burnsville audience red", "0-02-2")); + + // close right finger + addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-3")); + + } + +} 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 new file mode 100644 index 0000000..d149e47 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java @@ -0,0 +1,86 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Burnsville BackDrop blue") + +public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Burnsville BackDrop blue"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); +// addTask(new ClawArmControlTask(robot)); + + this.robot.setup(); + addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0")); + +// addState(new CameraVisionState(robot)); + + 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 ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4")); + + // place pixel + addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-1")); + + // drive to search pos + addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-2")); + + // close right finger + addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-3")); + + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0")); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1")); + +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0")); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1")); + + addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2")); + + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0")); + + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task")); + +// + } + +} 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 new file mode 100644 index 0000000..d72f0c0 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java @@ -0,0 +1,87 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Burnsville BackDrop red") + +public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Burnsville BackDrop red"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); +// addTask(new ClawArmControlTask(robot)); + + this.robot.setup(); + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-0")); + +// addState(new CameraVisionState(robot)); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-1")); + + // drive to the left, center, or right spike mark +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-02-0")); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-5")); + + + // place pixel + addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-1")); + + // drive to search pos + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-2")); + + // close right finger + addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-3")); + + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0")); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1")); + +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0")); +// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0")); + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1")); + + addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2")); + + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0")); + + + addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task")); + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmControlTask.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmControlTask.java new file mode 100644 index 0000000..33898e5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmControlTask.java @@ -0,0 +1,21 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.acmerobotics.dashboard.config.Config; + +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Config +public class ClawArmControlTask extends CyberarmState { + + CompetitionRobotV1 robot; + + public ClawArmControlTask(CompetitionRobotV1 robot) { + this.robot = robot; + } + + @Override + public void exec() {robot.clawArmControl();} +} 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 115aa74..d487da0 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 @@ -57,27 +57,29 @@ public class DriveToCoordinatesState extends CyberarmState { if (objectPos != robot.objectPos) { // enter the ending loop setHasFinished(true); + } else { + + if (armDrive) { + robot.clawArmControl(); + } + + if (Math.abs(robot.positionX - robot.xTarget) < 5 + && Math.abs(robot.positionY - robot.yTarget) < 5) { + setHasFinished(true); + } + } + } else { + if (armDrive) { + robot.clawArmControl(); + } + + if (Math.abs(robot.positionX - robot.xTarget) < 5 + && Math.abs(robot.positionY - robot.yTarget) < 5) { + setHasFinished(true); + } } } - if (armDrive) { - robot.clawArmControl(); - } - - robot.OdometryLocalizer(); - robot.XDrivePowerModifier(); - robot.YDrivePowerModifier(); - robot.DriveToCoordinates(); - robot.OdometryLocalizer(); - - if (Math.abs(robot.positionX - robot.xTarget) < 2 - && Math.abs(robot.positionY - robot.yTarget) < 2){ - setHasFinished(true); - } - } - - - @Override public void telemetry() { engine.telemetry.addData("x pos", robot.positionX); 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 new file mode 100644 index 0000000..ebbee2f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java @@ -0,0 +1,23 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import android.util.Log; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config +public class DriveToCoordinatesTask extends CyberarmState { + + CompetitionRobotV1 robot; + public DriveToCoordinatesTask(CompetitionRobotV1 robot) {this.robot = robot;} + @Override + public void exec() { + robot.XDrivePowerModifier(); + robot.YDrivePowerModifier(); + robot.DriveToCoordinates(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/OdometryLocalizerTask.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/OdometryLocalizerTask.java new file mode 100644 index 0000000..d439406 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/OdometryLocalizerTask.java @@ -0,0 +1,18 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.acmerobotics.dashboard.config.Config; + +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config +public class OdometryLocalizerTask extends CyberarmState { + + CompetitionRobotV1 robot; + public OdometryLocalizerTask(CompetitionRobotV1 robot) { + this.robot = robot; + } + @Override + public void exec() {robot.OdometryLocalizer();} +} 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 b8a4f37..9045baa 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -40,7 +40,7 @@ public class CompetitionRobotV1 extends Robot { // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: public OpenCvWebcam webcam1 = null; public WebcamName webCamName; - public DcMotor frontLeft, frontRight, backLeft, backRight, lift, /*clawArm,*/ chinUp; + public DcMotor frontLeft, frontRight, backLeft, backRight, lift, chinUp; public DcMotorEx clawArm; public IMU imu; @@ -208,6 +208,7 @@ public class CompetitionRobotV1 extends Robot { } + // -------------------------------------------------------------------------------------------------------------------------- Functions: @@ -258,7 +259,7 @@ public class CompetitionRobotV1 extends Robot { public double HeadingPIDControl(double reference, double current){ double error = angleWrap(current - reference); headingIntegralSum += error * headingTimer.seconds(); - double derivative = (headingLastError - error) / headingTimer.seconds(); + double derivative = (error - headingLastError) / headingTimer.seconds(); headingTimer.reset(); 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 ae9f972..1476885 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 @@ -268,7 +268,7 @@ public class CompetitionTeleOpState extends CyberarmState { } if (Objects.equals(armPos, "lift up")) { - robot.shoulder.setPosition(robot.shoulderDeposit); + robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowDeposit); target = 120; robot.chinUpServo.setPosition(chinUpServoUp); @@ -282,8 +282,7 @@ public class CompetitionTeleOpState extends CyberarmState { robot.lift.setPower(0); robot.chinUpServo.setPosition(chinUpServoDown); robot.shoulder.setPosition(robot.shoulderCollect); - robot.elbow.setPosition(robot.elbowCollect); - target = 850; + target = 120; } }