diff --git a/TeamCode/src/main/java/Testing_Code.java b/TeamCode/src/main/java/Testing_Code.java new file mode 100644 index 0000000..5dd56ff --- /dev/null +++ b/TeamCode/src/main/java/Testing_Code.java @@ -0,0 +1,30 @@ +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@TeleOp +public class Testing_Code extends OpMode { + + private DcMotor leftMotor; + private DcMotor rightMotor; + + @Override + public void init() { + leftMotor = hardwareMap.get(DcMotor.class,"left"); + rightMotor = hardwareMap.get(DcMotor.class,"right"); + leftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + + } + + @Override + public void loop() { + double leftPower = -gamepad1.left_stick_y; + double rightPower = -gamepad1.left_stick_y; + + leftMotor.setPower(leftPower); + rightMotor.setPower(rightPower); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java new file mode 100644 index 0000000..02b5ffd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -0,0 +1,336 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +//@Autonomous(name = "CompBasket") +public class CompBasket extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 400.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 1.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 500.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 400.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 15.0; + this.wayPoints[4].y = 700.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + dashboardtelemtry.addData("made to else", ""); + dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java new file mode 100644 index 0000000..9d1862e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java @@ -0,0 +1,364 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "CompBasket& Park") +public class CompBasket_andPark extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + //Waypoint setup + // Add waypoints for parking + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 400.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 1.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 425.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 400.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint();// go and raise elevator + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint();//open claw at basket + this.wayPoints[4].x = 25.0; + this.wayPoints[4].y = 650.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint();// small backup and drop elevator + this.wayPoints[5].x = 75.0; + this.wayPoints[5].y = 650.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5- deposit and elevator up + //Waypoint to get away from basket and drop elevator + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 562.0; + this.wayPoints[6].y = 478.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.NORTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + //dashboardtelemtry.addData("made to else", ""); + //dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar + case 5: + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("drop elevator??", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); + if (this.elbow.getElbowState() <= 0.6) + { + this.elbow.setElbowState(3); + } + else { + this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + } + break; +// case 7: +// this.elbow.setElbowState(0); + // elevator.setElevatorState(3); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + //elevator.setElevatorState(3); + //driveTrain.setDirection(Constants.NORTH); + break; + } + // Done Processing Waypoints + // Process loop + driveTrain.loop(); + this.elevator.loop(); + + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java new file mode 100644 index 0000000..5aea354 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -0,0 +1,636 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "CompBasket2High") +public class Compbasket_2High extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 25; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + private boolean settime = false; + private double startTime; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + //Waypoint setup + // Add waypoints for parking + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 400.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 1.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 425.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 400.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint();// go and raise elevator + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint();//open claw at basket + this.wayPoints[4].x = 25.0; + this.wayPoints[4].y = 650.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint();// small backup and drop elevator + this.wayPoints[5].x = 75.0; + this.wayPoints[5].y = 650.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5- deposit and elevator up + //Waypoint to get away from basket and drop elevator + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 542.0;//was 562 + this.wayPoints[6].y = 478.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.NORTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 110.0; + this.wayPoints[9].y = 90.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[10] = new WayPoint();//open claw at basket + this.wayPoints[10].x = 25.0; + this.wayPoints[10].y = 650.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.SOUTH_WEST; + //Done Waypoint 10 + //Waypoint 11 + this.wayPoints[11] = new WayPoint();// small backup and drop elevator + this.wayPoints[11].x = 75.0; + this.wayPoints[11].y = 650.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Done waypoint 11 + //Waypoint to get away from basket and drop elevator + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = 479.0;//was 562 + this.wayPoints[12].y = 752.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Done waypoint 12 + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = 270.0; + this.wayPoints[13].y = 250.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = 270.0; + this.wayPoints[14].y = 530.0; + this.wayPoints[14].y_speed = 0.3; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + // Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = 110.0; + this.wayPoints[15].y = 90.0; + this.wayPoints[15].y_speed = 0.3; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[16] = new WayPoint();//open claw at basket + this.wayPoints[16].x = 25.0; + this.wayPoints[16].y = 650.0; + this.wayPoints[16].y_speed = 0.3; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.SOUTH_WEST; + + this.wayPoints[17] = new WayPoint();// small backup and drop elevator + this.wayPoints[17].x = 75.0; + this.wayPoints[17].y = 650.0; + this.wayPoints[17].y_speed = 0.3; + this.wayPoints[17].x_speed = 0.3; + this.wayPoints[17].facing = Constants.NORTH; + + this.wayPoints[18] = new WayPoint();// line up with submersible + this.wayPoints[18].x = 1368.0; + this.wayPoints[18].y = 649.6; + this.wayPoints[18].y_speed = 0.3; + this.wayPoints[18].x_speed = 0.3; + this.wayPoints[18].facing = Constants.NORTH; + + this.wayPoints[19] = new WayPoint();// line up with submersible + this.wayPoints[19].x = 1368.0; + this.wayPoints[19].y = -37; + this.wayPoints[19].y_speed = 0.3; + this.wayPoints[19].x_speed = 0.3; + this.wayPoints[19].facing = Constants.EAST; + + this.wayPoints[20] = new WayPoint();// line up with submersible + this.wayPoints[20].x = 1368.0; + this.wayPoints[20].y = -87; + this.wayPoints[20].y_speed = 0.3; + this.wayPoints[20].x_speed = 0.3; + this.wayPoints[20].facing = Constants.EAST; + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + //dashboardtelemtry.addData("made to else", ""); + //dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basket + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar + case 5: + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("drop elevator??", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); +// if (this.elbow.getElbowState() <= 0.6) +// { +// this.elbow.setElbowState(3); +// } +// else { + //this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + // resetRuntime(); + break; + case 7: + this.elbow.setElbowState(3); + elevator.setElevatorState(0); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); + this.current_step++; +// } + break; + case 8: + dashboardtelemtry.addData("Claw State:", claw.getClawState()); + if (this.claw.getClawState()==Constants.CLAW_OPEN) { + this.claw.closeClaw(); + startTime = getRuntime(); + settime = true; + } + else { + if (getRuntime() - startTime >= 0.6) + { + this.current_step++; + settime = false; + } + } + break; + case 9: + this.elbow.setElbowState(0); + //turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 10: + // drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("in else 11", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + + case 12: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); +// if (this.elbow.getElbowState() <= 0.6) +// { +// this.elbow.setElbowState(3); +// } +// else { + //this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + // resetRuntime(); + break; + + case 13: + this.elbow.setElbowState(3); + elevator.setElevatorState(0); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); + this.current_step++; +// } + break; + case 14: + dashboardtelemtry.addData("Claw State:", claw.getClawState()); + if (this.claw.getClawState()==Constants.CLAW_OPEN) { + this.claw.closeClaw(); + startTime = getRuntime(); + settime = true; + } + else { + if (getRuntime() - startTime >= 0.6) + { + this.current_step++; + settime = false; + } + } + break; + case 15: + this.elbow.setElbowState(0); + //turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 16: + // drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + case 17: + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("in else 17", elevator.getState()); + driveTrain.drive(0.0,0.0); + claw.closeClaw(); + this.current_step++; + } + break; + + case 18: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 19: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + + case 20: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + elevator.setElevatorState(3); + } + else + { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + +// + + +// if (!this.at_xy(this.wayPoints[current_step])) +// { +// this.goto_xy(this.wayPoints[current_step]); +// } +// else { +// +// elevator.setElevatorState(0); +// dashboardtelemtry.addData("drop elevator??", elevator.getState()); +// driveTrain.drive(0.0, 0.0); +// this.current_step++; +// } +// break; +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + //elevator.setElevatorState(3); + //driveTrain.setDirection(Constants.NORTH); + break; + } + // Done Processing Waypoints + // Process loop + driveTrain.loop(); + this.elevator.loop(); + + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java new file mode 100644 index 0000000..31f38cb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java @@ -0,0 +1,169 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "DoNothing") +public class Do_Nothing extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); +// String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); +// telemetry.addData("Position", data); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 25.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.0; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 25.0; + this.wayPoints[2].x_speed = 0.0; + this.wayPoints[2].y = -1108.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + double local_heading = pos.getHeading(AngleUnit.DEGREES); +// String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); +// telemetry.addData("Position", data); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + + telemetry.addData("Elevator State:",elevator.getState()); + telemetry.addData("heading:",local_heading); + } + //end of loop + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + public boolean at_x(double previousX, double currentX) + { + double error; + error = (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); + telemetry.addData("X Error:",error); + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + double error; + error = (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); + telemetry.addData("Y Error:",error); + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java new file mode 100644 index 0000000..c300dc7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -0,0 +1,212 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "NewPark_Right") +public class New_Park extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private boolean autodone = false; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized:","Done"); + } + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 25.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.0; + this.wayPoints[1].facing = Constants.NORTH; + // Done Waypoint 1 + // WayPoint 2 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 25.0; + this.wayPoints[2].x_speed = 0.0; + this.wayPoints[2].y = -1058.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("heading error:","%.2f",driveTrain.headingError); + telemetry.addData("heading correction:","%.2f",driveTrain.headingCorrection); + telemetry.addData("Elevator State:",elevator.getState()); + //starting waypoint navigation + switch (this.current_step) { + case 1: // Move an inch from the wall + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: // Move over to Park + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + // this.current_step++; + break; + default: + driveTrain.setFacing(Constants.NORTH); + driveTrain.drive(0.0,0.0); + autodone = true; + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + stop();// put here for testing bw + if (!autodone) { + driveTrain.loop(); + this.elevator.loop(); + } + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + public boolean at_xy(WayPoint currentWaypoint) + { + return this.at_x(currentWaypoint.x) && this.at_y(currentWaypoint.y); + } + public boolean at_x(double targetX) + { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) + { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + // New Way to get there + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); + + // New Way End + // Old Way Start + // Check to see if you are at y. If yes, then process x; + // Process y direction +// if (!this.at_y(targetWaypoint.y)) { +// if (targetWaypoint.y >= driveTrain.getYPosition()) { +// this.driveTrain.drive(0.0, targetWaypoint.y_speed); +// } +// else { +// this.driveTrain.drive(0.0,-targetWaypoint.y_speed); +// } +// return false; +// } +// // Process x direction +// if (!this.at_x(targetWaypoint.x)) { +// if (targetWaypoint.x >= driveTrain.getXPosition()) { +// this.driveTrain.drive(targetWaypoint.x_speed, 0.0); +// } +// else { +// this.driveTrain.drive(-targetWaypoint.x_speed, 0.0); +// } +// return false; +// } +// else +// { +// this.driveTrain.drive(0.0,0.0); +// } +// return true; + // End of Old Way + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java deleted file mode 100644 index 52c64d8..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -//import com.qualcomm.robotcore.eventloop.opmode.teleop; - -import org.firstinspires.ftc.teamcode.Mechanisms.Constants; - -@Autonomous (name = "AutoRedSimple") -public class RedSimple extends BaseAutoPractice{ - - public void init(){ - - - }//inti - }//everything - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index bb017eb..a23db3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -55,7 +55,8 @@ @Config // need to use dashboard to change PID gains; comment out for competition @TeleOp(name = "CompBot") -public class CompBot extends OpMode { +public class +CompBot extends OpMode { private final ElapsedTime runtime = new ElapsedTime(); @@ -87,7 +88,7 @@ public class CompBot extends OpMode { @Override public void init() { telemetry = new CAITelemetry(telemetry); - ((CAITelemetry) telemetry).setDashboardEnabled(true); + ((CAITelemetry) telemetry).setDashboardEnabled(false); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new DriveTrain2025(hardwareMap); @@ -96,12 +97,12 @@ public void init() { boolean manualElevator = false; elevator = new Elevator(hardwareMap);// already in DriveTrain2025 - driveTrain.init(); + driveTrain.init(); // commented out ,done in Auto elbow.init(); elevator.init(); this.elbowPosition = 0; // comment this out for competition and ensure it happens in auto code - driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.resetPosAndIMU(); // comment out with Auto telemetry.addData("Status", "Initialized"); @@ -198,7 +199,11 @@ public void loop() { { claw.switchClaw(); } - + // Emergency Reset + if((driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.3) && (driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.3)) + { + driveTrain.odometer.resetPosAndIMU(); + } // Telemetry Output telemetry.addData("Direction Stick X:","%5.2f",driver.getRightX()); telemetry.addData("Direction Stick Y:","%5.2f",driver.getRightY()); @@ -207,6 +212,7 @@ public void loop() { telemetry.addData("Elevator State:","%d",elevatorPosition); telemetry.addData("Claw State:","%s",claw.printClaw()); telemetry.addData("Elbow Position:","%s",this.elbowPositionDescriptions[elbowPosition]); + telemetry.addData("Heading Correction",driveTrain.headingCorrection); // telemetry.addData("Claw Position",(); telemetry.addData("Elbow Position:","%2.4f",elbow.getElbowState()); //telemetry.addData("elevator manual/auto","%2.4f",elevator.manualElevator); @@ -214,7 +220,7 @@ public void loop() { Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); -elevator.printTelemetry(telemetry); + elevator.printTelemetry(telemetry); driveTrain.printTelemetry(telemetry);// correct usage?? } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java index f67e536..9710c1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java @@ -18,17 +18,17 @@ import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; //uncomment below to add back to opmode list -//@TeleOp(name = "testElevator") +@TeleOp(name = "testElevator") // this is just used to test elevator and get encoder values for pre-sets public class ElevatorTesting extends OpMode { static Motor elevatorMotor = null; public static double height; - private HardwareMap hwMap; + //private HardwareMap hwMap; private Elevator elevator; public double elevatorHeight; public GamepadEx operator = null; - private PracticeDriveTrain2025 driveTrain; + private DriveTrain2025 driveTrain; private Elbow elbow; private double elevatorSpeed; @@ -36,23 +36,24 @@ public class ElevatorTesting extends OpMode { public void init() { telemetry = new CAITelemetry(telemetry); ((CAITelemetry) telemetry).setDashboardEnabled(true); - driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain = new DriveTrain2025(hardwareMap); elevator = new Elevator(hardwareMap); driveTrain.init(); + elevator.init(); elbow = new Elbow(hardwareMap); // this.setHeight(Constants.ELEVATOR_START); - this.hwMap = hwMap; + } @Override public void start() { - operator = new GamepadEx(gamepad2); + // operator = new GamepadEx(gamepad2); //driveTrain.start(); //reset encoders operator = new GamepadEx(gamepad2); // This controls the movement of items on the robot elbow.setElbowState(0);// start position //driveTrain.resetIMU(); - //this.hwMap = hwMap; - elevatorMotor = new Motor(hwMap, "elevator");// expansion hub port 0 + // this.hwMap = hwMap; + elevatorMotor = new Motor(hardwareMap, "elevator");// expansion hub port 0 elevatorMotor.setRunMode(Motor.RunMode.RawPower); elevatorMotor.resetEncoder(); } @@ -62,6 +63,8 @@ public void loop(){ operator.readButtons(); driveTrain.loop(); elevator.move(operator.getLeftY()); + elevator.printTelemetry(telemetry); + } public double getHeight(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java index 4e3a846..1dbde0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java @@ -5,6 +5,7 @@ //import com.qualcomm.robotcore.hardware.Servo; public class + Claw { private SimpleServo servo; // private HardwareMap hwMap; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index dd57eb3..62b382b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -12,15 +12,14 @@ public static final double SPEED_RATIO = 1.0; // Use this to slow down robot public static final double TURN_RATIO = 1.0; // use this to slow turn rate - public static final double DRIVE_PID_ERROR = 1.5; // inches public static final double AUTO_DRIVE_SPEED = 0.3; public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR = 5; // Degrees... this now matches PID tolerance - // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode + // + // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode public static final double FORWARD = 0; // north public static final double BACK = 180; // south public static final double RIGHT = -90; // east @@ -35,48 +34,52 @@ public static final double NORTH_WEST = 45; // North West // SERVO CONSTANTS // finger servo closed position - public static final double STEP = 0.05; // used for manual increment of wrist servo - /*public static final double LF_CLOSED = 0.3 ;// finger servo closed position + // public static final double STEP = 0.05; // used for manual increment of wrist servo + // public static final double LF_CLOSED = 0.3 ;// finger servo closed position public static final double LF_OPEN = 0.7 ; // open position public static final double RF_CLOSED = 0.6 ; public static final double RF_OPEN = 0.2 ; // open position*/ // for elevator moves public static final int ELEVATOR_START = 0; - public static final int ELEVATOR_GROUND = 100; - public static final int ELEVATOR_LOW_BASKET = 850; // nov 22 + public static final int ELEVATOR_PICKUP_SPECIMEN = 40; + public static final int ELEVATOR_HIGH_BAR = 1833; // nov 22 - public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - public static final int ELEVATOR_HIGH_BAR = 660; + // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - public static final int ELEVATOR_HIGH_BASKET = 1300; // or 1260? - public static final int ELEVATOR_MAX = 1220; + public static final int ELEVATOR_HIGH_BASKET = 3578; + public static final int ELEVATOR_MAX = 4337; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator - public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; - public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; - public static final int ELEVATOR_STUCK_COUNT = 5; - //Claw constants +// public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; +// public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; +// public static final int ELEVATOR_STUCK_COUNT = 5; + // Claw constants + //robot 2 need to be fixed, values were revered public static boolean CLAW_OPEN = true; public static boolean CLAW_CLOSED = false; public static final double SERVO_CLAW_OPEN = 0.2 ; public static final double SERVO_CLAW_CLOSED = 0.8 ; // =========== for elbow movements ====================== - public static final int ELBOW_START = 0; - public static final double ELBOW_BOTTOM = 0.3; +//robot 2 need to be fixed, values were revered + // public static final double ELBOW_BOTTOM = 0.3; public static final double ELBOW_START_POSITION = 0.0; // public static final double ELBOW_PICKUP_SAMPLE = 1.0; - public static final double ELBOW_PICKUP_SPECIMEN = 0.3; - public static final double ELBOW_DEPOSIT_SPECIMEN_LOW_BAR = 0.4; -// public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; - public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; - public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.75; + public static final double ELBOW_PICKUP_SPECIMEN = 0.4; + //Pickup specimen from the wall for CompBot (Tortuga) + public static final double ELBOW_PICKUP_SPECIMEN_WALL = 0.5; + public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.67; + public static int ELBOW_MAX = 3; public static int ELBOW_MIN = 0; // // Tolerance Section public static final double JOYSTICK_TOLERANCE = 0.5; - + public static final double AT_XY_TOLERANCE = 25.0; + public static double AUTO_Y_DISTANCE_ERROR = 15.0; + public static double AUTO_X_DISTANCE_ERROR = 15.0; + public static final double DRIVE_PID_ERROR = 1.5; // inches?? Only used in PractiseDriveTrain?? + public static double HEADING_ERROR_Tolerance = 5.0; // Degrees... this now matches PID tolerance // Bruce:sept30: change these for use in elevator?kk // public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; // public static final double ARM_TICKS_PER_90DEG = 113; // CONFIRM #TICKS PER 90. @@ -84,6 +87,13 @@ //odometer constants public static int ODOMETER_PRACTICE_MODE = 1; public static int ODOMETER_COMPETITION_MODE = 8; - public static double ODOMETER_X_OFFSET = -145.0; - public static double ODOMETER_Y_OFFSET = 270.0; + public static double ODOMETER_X_OFFSET = 0;// left offset is + uppdated after contact with gobilda support + public static double ODOMETER_Y_OFFSET = 24.0; + // auto constants + public static double XPID_Kp = 0.018;// was 0.14 + public static double YPID_Kp = 0.018; + public static double HEADING_Kp = 0.015; // was 0.01 + + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index cd08440..d86be2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -1,21 +1,21 @@ package org.firstinspires.ftc.teamcode.Mechanisms; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDCoefficients; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -public class - -DriveTrain2025 { +@Config +public class DriveTrain2025 { // Driving Motors private final Motor leftFrontDrive; @@ -24,16 +24,18 @@ private final Motor rightBackDrive; - //servos private final SimpleServo claw; private final SimpleServo elbow; MecanumDrive driveBase; public GoBildaPinpointDriver odometer; - private PIDController headingControl = null; - private PIDController xControl = null; - private PIDController yControl = null; + public PIDController headingControl = null; + public static PIDCoefficients headingpid = new PIDCoefficients(Constants.HEADING_Kp, 0.001, 0.000); + public static PIDCoefficients xpid = new PIDCoefficients(Constants.XPID_Kp, 0.0, 0.00); + public static PIDCoefficients ypid = new PIDCoefficients(Constants.YPID_Kp, 0.0, 0.00); + public PIDController xControl = null; + public PIDController yControl = null; public double headingCorrection = 0; @@ -48,9 +50,9 @@ private double currentYTarget = 0; double xSpeed = 0; double ySpeed = 0; + public double headingError; - public DriveTrain2025(HardwareMap hwMap) - { + public DriveTrain2025(HardwareMap hwMap) { this.hwMap = hwMap; // Define and Initialize Motors (note: need to use reference to actual OpMode). @@ -60,26 +62,26 @@ public DriveTrain2025(HardwareMap hwMap) leftBackDrive = new Motor(hwMap, "left_back_drive"); // 2 rightBackDrive = new Motor(hwMap, "right_back_drive"); // 3 //elevator = new Motor(hwMap,"elevator"); - claw = new SimpleServo(hwMap,"claw",0,1); // expansion hub port 0 - elbow = new SimpleServo(hwMap,"elbow",0,1); // expansion hub port 1 + claw = new SimpleServo(hwMap, "claw", 0, 1); // expansion hub port 0 + elbow = new SimpleServo(hwMap, "elbow", 0, 1); // expansion hub port 1 //elevator.resetEncoder(); - // xPod = new Motor(hwMap, "x-pod"); // 3 - // yPod = new Motor(hwMap, "y-pod"); // 2 + // xPod = new Motor(hwMap, "x-pod"); // 3 + // yPod = new Motor(hwMap, "y-pod"); // 2 - - driveBase = new MecanumDrive(leftFrontDrive,rightFrontDrive,leftBackDrive,rightBackDrive); + driveBase = new MecanumDrive(leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive); } + public void init() { - headingControl = new PIDController(0.02, 0.002, 0.000 ); - headingControl.setTolerance(10);// was 3 increased to see if affects spinnning ..cbw - xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? - yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO + headingControl = new PIDController(headingpid.p, headingpid.i, headingpid.d); + headingControl.setTolerance(Constants.HEADING_ERROR_Tolerance);// was 3 increased to see if affects spinnning ..cbw + xControl = new PIDController(xpid.p, xpid.i, xpid.d);//FOR AUTO + yControl = new PIDController(ypid.p, ypid.i, ypid.d);//For AUTO // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); @@ -92,8 +94,8 @@ public void init() { leftFrontDrive.setInverted(true); leftBackDrive.setInverted(true); // odometer initializing -- this should go into PracticeDriveTrain2025?? - odometer = hwMap.get(GoBildaPinpointDriver.class,"xy-cord"); - odometer.setOffsets(Constants.ODOMETER_X_OFFSET,Constants.ODOMETER_Y_OFFSET); + odometer = hwMap.get(GoBildaPinpointDriver.class, "xy-cord"); + odometer.setOffsets(Constants.ODOMETER_X_OFFSET, Constants.ODOMETER_Y_OFFSET); odometer.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD); odometer.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); @@ -103,20 +105,20 @@ public void init() { public void start() { } - public void loop(){ + + public void loop() { //heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); odometer.update(); // needs to be called once per loop Pose2D pos = odometer.getPosition(); heading = pos.getHeading(AngleUnit.DEGREES); // Because it's a double, can't check for exactly 180, so we check if it's almost 180 in either direction. - if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR) { + if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR_Tolerance) { // "south" is special because it's around the 180/-180 toggle-point // Change set-point between 180/-180 depending on which is closer. if (heading < 0.0) { headingSetPoint = -180; - } - else { + } else { headingSetPoint = 180; } } @@ -124,14 +126,14 @@ public void loop(){ // PID controller for heading headingControl.setSetPoint(headingSetPoint); headingCorrection = -headingControl.calculate(heading);// confirm if (-) is needed. -// test this code in Practice bot first - /*if(autoEnabled && !atTarget()) { - double xTargetSpeed = -xControl.calculate(getXPosition()); - double yTargetSpeed = -yControl.calculate(getYPosition()); - double targetSpeed = currentSpeed;// Math.sqrt(yTargetSpeed * yTargetSpeed + xTargetSpeed * xTargetSpeed); - xSpeed = xTargetSpeed * targetSpeed; - ySpeed = yTargetSpeed * targetSpeed; - }*/ + headingError = Math.abs(headingSetPoint - heading); +//temporary test code + if (headingError <= Constants.HEADING_ERROR_Tolerance) { + headingCorrection = 0; + } + + // need to send 0 correction if 'Happy" + driveBase.driveFieldCentric( xSpeed,// strafe @@ -139,21 +141,22 @@ public void loop(){ headingCorrection,// turn heading,// heading false); - } + }// end of loop() - /* public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. - double xError = getXPosition() - currentXTarget; - double yError = getYPosition() - currentYTarget; - return !autoEnabled || Math.sqrt(xError * xError + yError * yError) < Constants.DRIVE_PID_ERROR; - }*/ +// public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. +// double xError = getXPosition() - currentXTarget; +// double yError = getYPosition() - currentYTarget; +// return !autoEnabled || Math.sqrt(xError * xError + yError * yError) < Constants.DRIVE_PID_ERROR; +// } //============== Move in new Direction ========== public void setDirection(double newHeading) { headingSetPoint = newHeading; } - + public void setFacing(double newHeading) { headingSetPoint = newHeading; } public boolean onHeading() { - return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR; + return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR_Tolerance; + } public void driveTo(double speed, double xDist, double yDist, double facing) { autoEnabled = true; @@ -164,12 +167,13 @@ public void driveTo(double speed, double xDist, double yDist, double facing) { xControl.setSetPoint(currentXTarget); yControl.setSetPoint(currentYTarget); + } public void drive(double forwardSpeed, double strafeSpeed) { // tell ftclib its inputs strafeSpeed,forwardSpeed,turn,heading // turn and heading are managed in loop with the heading control PID - // inverting strafe speed to correct directions + // inverting strafe speed to correct directions, inverting no longer required for telemtry pdos have been turned to right position xSpeed = -strafeSpeed; ySpeed = forwardSpeed; } @@ -178,13 +182,15 @@ public double getXPosition() { // Convert xPod into actual direction based on cu double xPos = 0.0; Pose2D pos = odometer.getPosition(); xPos = pos.getX(DistanceUnit.MM); - return xPos; + return -xPos;// pod mounted backwards } public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; Pose2D pos = odometer.getPosition(); + // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); - return yPos; + + return -yPos;// pod mounted backwards } public void stop() { @@ -193,6 +199,7 @@ public void stop() { driveBase.stop(); autoEnabled = false; } + public void printTelemetry(Telemetry telemetry) { //telemetry.addData("actual heading:", "%5.2f", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); telemetry.addData("saved heading:", "%5.2f", heading); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index 050db73..cfd956e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -5,14 +5,12 @@ //import com.qualcomm.robotcore.hardware.Servo; -public class - -Elbow { +public class Elbow { private SimpleServo armServo; private HardwareMap hwMap; public void init() { - this.setElbowState(Constants.ELBOW_START); + this.armServo.setPosition(Constants.ELBOW_START_POSITION); } public Elbow(HardwareMap hwMap) { @@ -35,16 +33,19 @@ public void setElbowState(int position) { // case 0: //Case for pickup specimen // armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN); // break; - case 1: //Case for deposit specimen low bar - armServo.setPosition(Constants.ELBOW_DEPOSIT_SPECIMEN_LOW_BAR); + case 1: //Case for pickup specimen for practice bot + armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN); break; + case 2: //Case for pickup specimen for comp bot + armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN_WALL); + break; // case 4: //Case for deposit specimen high bar // armServo.setPosition(Constants.ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR); // break; - case 2: //Case for deposit sample on low bucket - armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); - break; - case 3: //Case for deposit sample on high bucket + // case 2: //Case for deposit sample on low bucket + // armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); + //break; + case 3: //Case for deposit sample on high bucket??? armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET); break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index 598b2b3..9a12482 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -4,6 +4,7 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDCoefficients; @@ -11,7 +12,7 @@ @Config public class Elevator { static Motor elevatorMotor = null; - public static int target_height; + int target_height; private HardwareMap hwMap; //public boolean manualElevator = false; private int elevatorPosition; @@ -26,10 +27,12 @@ public class Elevator { public void init() { elevatorController = new PIDController(pid.p, pid.i, pid.d); elevatorMotor.setRunMode(Motor.RunMode.RawPower); + // added reset encoder + elevatorMotor.resetEncoder(); } public void start() { - elevatorMotor.resetEncoder(); + // elevatorMotor.resetEncoder(); } public void loop() { @@ -56,35 +59,25 @@ public Elevator(HardwareMap hwMap) { //====================== METHODS =================== // Set the elevator state - public void setElevatorState(int position) { + public int setElevatorState(int position) { switch (position) { - case 0: //Case for start position + case 0: //Case for start position/pickup sample this.gotoPreSet(Constants.ELEVATOR_START); setpoint = Constants.ELEVATOR_START; //setpoint = target_height; // DEBUG: get from FTCDashboard break; - case 1: //Case for pickup sample - this.gotoPreSet(Constants.ELEVATOR_GROUND); - setpoint = Constants.ELEVATOR_GROUND; - break; - - case 2: //Case for pickup specimen - this.gotoPreSet(Constants.ELEVATOR_LOW_BAR); - setpoint = Constants.ELEVATOR_LOW_BAR; + case 1: //Case for pickup specimen + this.gotoPreSet(Constants.ELEVATOR_PICKUP_SPECIMEN); + setpoint = Constants.ELEVATOR_PICKUP_SPECIMEN; break; - case 3: //Case for deposit specimen high bar + case 2: //Case for deposit specimen high bar this.gotoPreSet(Constants.ELEVATOR_HIGH_BAR); setpoint = Constants.ELEVATOR_HIGH_BAR; break; - case 4: //Case for deposit specimen low bar - this.gotoPreSet(Constants.ELEVATOR_LOW_BASKET); - setpoint = Constants.ELEVATOR_LOW_BASKET; - break; - - case 5: //Case for deposit sample in low bucket + case 3: //Case for deposit sample in high basket this.gotoPreSet(Constants.ELEVATOR_HIGH_BASKET); setpoint = Constants.ELEVATOR_HIGH_BASKET; break; @@ -93,15 +86,17 @@ public void setElevatorState(int position) { // elevatorMotor.stopMotor(); } this.elevatorPosition = position; + return position; } public void move(double speed) {// this method is for manual elevator moves double elevatorSpeed = 0.0; - elevatorMotor.setRunMode(Motor.RunMode.RawPower); + //elevatorMotor.setRunMode(Motor.RunMode.RawPower); elevatorSpeed = speed * Constants.ELEVATOR_SPEED_FACTOR; elevatorMotor.set(elevatorSpeed); + // if (getHeight() >= Constants.ELEVATOR_MIN & getHeight() <= Constants.ELEVATOR_MAX) { // elevatorMotor.set(elevatorSpeed); // } else if (getHeight() >= Constants.ELEVATOR_MAX & speed < 0.0) { @@ -132,7 +127,7 @@ public void gotoPreSet (int setpoint) { } public int getState() { - return this.elevatorPosition; + return setpoint; } public void printTelemetry(Telemetry telem){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java new file mode 100644 index 0000000..7bc8bc4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.Mechanisms; + +import android.os.Environment; +import java.io.FileNotFoundException; +import java.io.PrintStream; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; + +///////////////////////////////////////////////// +// This is the logging module. +// +// Written By: George Nazarey +// Date: 3/5/2025 +// Email: george@nazarey.ca +// +public class Logger { + + //private final String fileName = "/sdcard/FIRST/data/test.log"; + private final String fileName; + private PrintStream logWriter; + private Boolean logging = true; + private final SimpleDateFormat dateFormat; + private final Boolean addTimeStamp; + + public Logger(String filename, Boolean addTimeStamp){ + this.addTimeStamp = addTimeStamp; + this.fileName = String.format("%s/FIRST/data/%s",Environment.getExternalStorageDirectory().getPath(), filename); + this.dateFormat = new SimpleDateFormat("yyyy-MM-dd@HH-mm-ss", Locale.US); + this.init(); + } + public Logger(String filename){ + this(filename,false); + } + + private void init(){ + try{ + this.logWriter = new PrintStream(this.fileName); + } + catch (FileNotFoundException e) { + this.logging = false; + } + } + + public boolean getLogging(){ + return this.logging; + } + + public String getTimeStamp(){ + return this.dateFormat.format(new Date()); + } + + public void writeLog(String logLine) + { + if (this.logging) { + if (addTimeStamp) { + this.logWriter.printf(Locale.ENGLISH, "%s %s%n", this.getTimeStamp(), logLine); + } else { + this.logWriter.println(logLine); + } + } + } + + public void stopLog() { + if (this.logging) { + this.logWriter.flush(); + this.logWriter.close(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 53e4df7..61aa8d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -4,26 +4,13 @@ import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import com.arcrobotics.ftclib.controller.PIDController; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; -import com.arcrobotics.ftclib.hardware.SimpleServo; -import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.PracticeBot; import java.util.Locale; @@ -37,6 +24,9 @@ public class PracticeDriveTrain2025 private final Motor rightBackDrive; MecanumDrive driveBase; + //servos + private final SimpleServo claw; + private final SimpleServo elbow; public GoBildaPinpointDriver odometer; private PIDController headingControl = null; @@ -50,6 +40,8 @@ public class PracticeDriveTrain2025 private boolean autoEnabled = false; + + HardwareMap hwMap; private double currentSpeed = 0; private double currentXTarget = 0; @@ -68,8 +60,16 @@ public PracticeDriveTrain2025(HardwareMap hwMap) leftBackDrive = new Motor(hwMap, "left_back_drive"); // 2 rightBackDrive = new Motor(hwMap, "right_back_drive"); // 3 + //elevator = new Motor(hwMap,"elevator"); + claw = new SimpleServo(hwMap,"claw",0,1); // expansion hub port 0 + elbow = new SimpleServo(hwMap,"elbow",0,1); // expansion hub port 1 + + //elevator.resetEncoder(); + + driveBase = new MecanumDrive(leftFrontDrive,rightFrontDrive,leftBackDrive,rightBackDrive); } + public void init() { headingControl = new PIDController(0.02, 0.002, 0.000 ); headingControl.setTolerance(5);// was 1 .. @@ -77,6 +77,7 @@ public void init() { yControl = new PIDController(0.17, 0.0, 0.00); + // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); rightBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); @@ -88,7 +89,7 @@ public void init() { leftFrontDrive.setInverted(true); leftBackDrive.setInverted(true); - // odometer initializing -- this should go into PracticeDriveTrain2025?? + // odometer initializing odometer = hwMap.get(GoBildaPinpointDriver.class,"xy-cord"); odometer.setOffsets(Constants.ODOMETER_X_OFFSET,Constants.ODOMETER_Y_OFFSET); odometer.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD); @@ -108,7 +109,7 @@ public void loop(){ //rightFrontDrive.setTargetDistance(1.0); // what is this doing?? and why?? // Because it's a double, can't check for exactly 180, so we check if it's almost 180 in either direction. - if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR) { + if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR_Tolerance) { // "south" is special because it's around the 180/-180 toggle-point // Change set-point between 180/-180 depending on which is closer. @@ -138,6 +139,7 @@ public void loop(){ headingCorrection,// turn heading,// current heading in degrees false); + } public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. @@ -152,7 +154,7 @@ public void setDirection(double newHeading) { } public boolean onHeading() { - return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR; + return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR_Tolerance; } public void driveTo(double speed, double xDist, double yDist,double facing) { autoEnabled = true; @@ -162,6 +164,7 @@ public void driveTo(double speed, double xDist, double yDist,double facing) { xControl.setSetPoint(currentXTarget); yControl.setSetPoint(currentYTarget); + } public void drive(double forwardSpeed, double strafeSpeed) { @@ -169,19 +172,52 @@ public void drive(double forwardSpeed, double strafeSpeed) { // turn and heading are managed in loop with the heading control PID // inverting strafe speed to correct directions xSpeed = strafeSpeed; - ySpeed = -forwardSpeed; + ySpeed = forwardSpeed; + } + + // These functions will control where we are going + public void auto_goto_xy(double speed, double x_distance, double y_distance) + { + Pose2D pos = this.odometer.getPosition(); + this.auto_drive_x(speed,x_distance); + // this.auto_drive_y(speed,y_distance); + } + public void auto_drive_x(double speed,double distance) { + double current_position; + double final_position; + + current_position = this.getXPosition(); + final_position = current_position + distance; + // calculate direction + // positive is right + // negative is left + if (distance < 0.0) { + speed = -speed; + } + while (Math.abs(final_position - current_position) <= Constants.AUTO_X_DISTANCE_ERROR) { + current_position = this.getXPosition(); + xSpeed = speed; + } + } + public void auto_drive_y(double speed,double distance) + { + } public double getXPosition() { // Convert xPod into actual direction based on current heading - double xPos=0.0; + double xPos = 0.0; + //odometer.update(); Pose2D pos = odometer.getPosition(); xPos = pos.getX(DistanceUnit.MM); return xPos; } public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; + //odometer.update(); Pose2D pos = odometer.getPosition(); + // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); + // Added negative sign to match drive control return yPos; } @@ -200,6 +236,11 @@ public void printTelemetry(Telemetry telemetry) { telemetry.addData("heading correction:", headingCorrection); telemetry.addData("X Distance inches:", "%5.2f", getXPosition()); telemetry.addData("Y Distance inches:", "%5.2f", getYPosition()); + + + odometer.update(); + odometer.update(); + telemetry.addData("Auto DriveTo Enabled", autoEnabled); // Odometer Output odometer.update(); @@ -208,6 +249,7 @@ public void printTelemetry(Telemetry telemetry) { String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); + //telemetry.addData("elevator","%5.2f",elevator.getCurrentPosition()); if(autoEnabled) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java index 7f49357..1fefa21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java @@ -57,7 +57,7 @@ -Ethan Doak */ -@TeleOp(name="PinPoint Odometry Reading", group="Linear OpMode") +//@TeleOp(name="PinPoint Odometry Reading", group="Linear OpMode") //@Disabled public class diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java similarity index 87% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java index 24256f5..d39c480 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -20,7 +20,9 @@ public class BaseAutoComp extends OpMode{ public class WayPoint { public double x; + public double x_speed = 0.2; public double y; + public double y_speed = 0.2; public double facing; } @Override @@ -30,7 +32,7 @@ public void start() { } public void init(){ telemetry = new CAITelemetry(telemetry); - ((CAITelemetry)telemetry).setDashboardEnabled(true); + ((CAITelemetry)telemetry).setDashboardEnabled(false); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new DriveTrain2025(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java similarity index 63% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java index bd98b20..55f0306 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java @@ -1,37 +1,40 @@ -package org.firstinspires.ftc.teamcode.Auto; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; -import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; //@Autonomous -public class BaseAutoPractice extends OpMode{ +public class BaseAutoPractice extends OpMode { String state = "START"; public PracticeDriveTrain2025 driveTrain; -// public Elevator elevator; -// public Claw claw; -// public Elbow elbow; + // public DriveTrain2025 PracticeDriveTrain2025; + public Elevator elevator; + public Claw claw; + public Elbow elbow; public class WayPoint { public double x; + public double x_speed = 0.2; public double y; + public double y_speed = 0.2; public double facing; } - @Override + @Override public void start() { state = "START"; - } - public void init(){ + + public void init() { telemetry = new CAITelemetry(telemetry); - ((CAITelemetry)telemetry).setDashboardEnabled(true); + ((CAITelemetry) telemetry).setDashboardEnabled(Constants.DASHBOARD_ENABLED); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new PracticeDriveTrain2025(hardwareMap); @@ -39,19 +42,11 @@ public void init(){ driveTrain.init(); telemetry.addData("Status", "Initialized"); telemetry.update(); - -// elevator = new Elevator(hardwareMap); -// claw = new Claw(hardwareMap); -// elbow = new Elbow(hardwareMap); -// elevator.init();// delete if init in elevator can be private - - // this.elbowPosition = 0; - } @Override public void loop() { telemetry.addData("State", state); - - } } +} // End of Class + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java new file mode 100644 index 0000000..e4a876e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java @@ -0,0 +1,364 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import java.util.Locale; + +//@Autonomous(name = "CompBasket_cbw") +public class CompBasket_CBW extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + TelemetryPacket pack; + CAITelemetry dashboardtelemetry; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + // Dashboard Telemetry + // ftc-dashboard telemetry + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemetry.update(); + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // Dashboard Setup +// pack = new TelemetryPacket(); + dashboardtelemetry = new CAITelemetry(telemetry); + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 640.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 640.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 100.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 640.0; + this.wayPoints[3].y = 100.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 100.0; + this.wayPoints[4].y = 450.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + dashboardtelemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + dashboardtelemetry.addData("Facing",this.wayPoints[this.current_step].facing); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + // ftc-dashboard telemetry + dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + dashboardtelemetry.addData("Facing",this.wayPoints[this.current_step-1].facing); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + // ftc-dashboard telemetry +// pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); +// pack.put("X distance:", driveTrain.getXPosition()); +// pack.put("Y distance:", driveTrain.getYPosition()); + // dashboard info +// dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); +// dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); +// dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemetry.update(); + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + // this.claw.openClaw(); + this.current_step++; + } + // this.current_step++; + break; + case 3://turn to SW + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else { + driveTrain.drive(0.0, 0.0); +// this.current_step++; + } + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + +// @Override +// public void stop() { +// elevator.setElevatorState(0); +// super.stop(); +// } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + return Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + return Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java new file mode 100644 index 0000000..872c196 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java @@ -0,0 +1,176 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; + +import java.util.Locale; + +//@Autonomous(name = "CompParkRed") +public class CompPark_Red extends BaseAutoComp { + + private DriveTrain2025 driveTrain; + private double speed; + private int counter = 0; + private double current_x, current_y; + private double x_speed,y_speed; + private int total_waypoints = 20; + WayPoint[] wayPoints; + public int step = 1; + private int current_step; + + // auto driving data + private double start_x, start_y; + private double dest_x, dest_y, set_dest; + private boolean at_x, at_y, set_x, set_y; + private boolean at_xy; + + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + elbow.init(); + elbow.setElbowState(0); + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 165.0; + this.wayPoints[1].x_speed = 0.50; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.50; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 165.0; + this.wayPoints[2].x_speed = 0.5; + this.wayPoints[2].y =-1208.0; + this.wayPoints[2].y_speed = 0.5; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + + + }//init + + @Override + public void loop() { + + driveTrain.odometer.update(); + telemetry.addData("Sequence Step", counter); + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + driveTrain.odometer.update(); + if (counter <= 150){ + driveTrain.drive(0.0,0.3); + }else { + driveTrain.drive(0.0,0.0); + } + counter = counter + 1; + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); + String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Speed x","%.2f",this.x_speed); + telemetry.addData("Speed y","%.2f",this.y_speed); + telemetry.addData("heading:","%.2f",driveTrain.heading); +// telemetry.addData("Dest x:","%.2f",this.dest_x); +// telemetry.addData("Dest y:","%.2f",this.dest_y);t + //telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); + //telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + if (this.wayPoints[this.current_step] != null){ + //telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + //telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + //telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + +//cases + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + default: + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + + + } + } + + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java new file mode 100644 index 0000000..2be52c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java @@ -0,0 +1,418 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +//@Autonomous(name = "CompSpecimen") +public class CompSpecimen extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + double startTime; + boolean timeSet; + + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + elbow.init(); + elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = -400.0; + this.wayPoints[1].y_speed = 0.2; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = -737.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].y = -560.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].facing = Constants.NORTH; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = -870.0; + this.wayPoints[4].y = -560.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = -1118.0; + this.wayPoints[5].y = -560.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.SOUTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = -1118.0; + this.wayPoints[6].y = 200.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = -1118.0; + this.wayPoints[7].y = 200.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.NORTH; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 0.0; + this.wayPoints[8].y = 0.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 0.0; + this.wayPoints[9].y = -582.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + //Waypoint + this.wayPoints[11] = new WayPoint(); + this.wayPoints[11].x = 0.0; + this.wayPoints[11].y = -540.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Waypoint + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = -640.0; + this.wayPoints[12].y = -540.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Waypoint + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = -640.0; + this.wayPoints[13].y = -1350.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.NORTH; + //Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = -990.0; + this.wayPoints[14].y = -200.0; + this.wayPoints[14].y_speed = 0.4; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + //Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = -990.0; + this.wayPoints[15].y = -1350.0; + this.wayPoints[15].y_speed = 0.4; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.NORTH; + //Waypoint + this.wayPoints[16] = new WayPoint(); + this.wayPoints[16].x = -1220.0; + this.wayPoints[16].y = -200.0; + this.wayPoints[16].y_speed = 0.4; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.NORTH; + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + + this.current_step++; + } + break; + case 2: + elevator.setElevatorState(2); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 5: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 6: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 7: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 8: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 12: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 13: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 14: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 15: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 16: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() <= currentX; + } + else { + return driveTrain.getXPosition() >= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, -currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + + public void set_time() + { + this.startTime = time; + this.timeSet = true; + } +}//everything + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java new file mode 100644 index 0000000..f1cd8f9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java @@ -0,0 +1,289 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +//@Autonomous(name = "GeorgeTest") +public class GeorgeTest extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + enum WorkingStep{ + X, + Y + } + + BaseAutoComp.WayPoint[] wayPoints; + private int current_step; + private int total_waypoints = 10; + CAITelemetry dashboardtelemetry; + + public void init_loop() { + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("heading:","%.2f",driveTrain.heading); + + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.current_step = 1; + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + elevator.init(); + elevator.setElevatorState(0); + dashboardtelemetry = new CAITelemetry(telemetry); + // Display Settings + + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("heading:","%.2f",driveTrain.heading); + dashboardtelemetry.setDashboardEnabled(true); + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); + + + // Waypoint Setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 100.0; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 40.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = -200.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = 0.0; + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].y = -400.0; + this.wayPoints[3].y_speed = 0.5; + this.wayPoints[3].facing = 0.0; + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 0.0; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].y = -400.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].facing = 0.0; + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 0.0; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].y = -500.0; + this.wayPoints[5].y_speed = 0.4; + this.wayPoints[5].facing = 0.0; + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 0.0; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].y = -400.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].facing = 0.0; + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 0.0; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].y = 0.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].facing = 0.0; + } // init + + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + // dashboard info + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; +// case 2: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 3: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + case 2: + driveTrain.setDirection(Constants.WEST); + this.claw.openClaw(); + elevator.setElevatorState(3); + this.current_step++; + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(Constants.EAST); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 7: +// driveTrain.setDirection(Constants.NORTH); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + elevator.loop(); + } + + public boolean at_xy(BaseAutoComp.WayPoint previousWaypoint, BaseAutoComp.WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() <= currentX; + } + else { + return driveTrain.getXPosition() >= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java index 1e94d86..4201b60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java @@ -1,11 +1,9 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -@Autonomous(name = "AutoRedPark_Test") -public class RedPark extends BaseAutoPractice { +//@Autonomous(name = "AutoPark_Test") +public class Park extends BaseAutoPractice { private int currentStep; //Location location = location.DEPOSIT_SPECIMEN; //waypoint definition diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java new file mode 100644 index 0000000..85986c4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java @@ -0,0 +1,295 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +//@Autonomous(name = "PracticeBasket") +public class PracticeBasket extends BaseAutoPractice { + private PracticeDriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + BaseAutoPractice.WayPoint[] wayPoints; + + public void init(){ + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + //Waypoint setup + //Practice bot setup: (Based on heading facing North/Forward) + // +Y = +North + // -X = +West + // -Y = +South + // +X = +East + this.wayPoints = new BaseAutoPractice.WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new BaseAutoPractice.WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new BaseAutoPractice.WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = 525.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new BaseAutoPractice.WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = 525.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new BaseAutoPractice.WayPoint(); + this.wayPoints[3].x = 43.0; + this.wayPoints[3].y = 367.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new BaseAutoPractice.WayPoint(); + this.wayPoints[4].x = -270.0; + this.wayPoints[4].y = -250.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new BaseAutoPractice.WayPoint(); + this.wayPoints[5].x = -445.0; + this.wayPoints[5].y = -530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new BaseAutoPractice.WayPoint(); + this.wayPoints[6].x = -445.0; + this.wayPoints[6].y = -530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new BaseAutoPractice.WayPoint(); + this.wayPoints[7].x = -270.0; + this.wayPoints[7].y = -250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new BaseAutoPractice.WayPoint(); + this.wayPoints[8].x = -270.0; + this.wayPoints[8].y = -530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new BaseAutoPractice.WayPoint(); + this.wayPoints[9].x = -695.0; + this.wayPoints[9].y = -530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new BaseAutoPractice.WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 3: + elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 5: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 7: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 8: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(BaseAutoPractice.WayPoint previousWaypoint, BaseAutoPractice.WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(BaseAutoPractice.WayPoint previousWaypoint, BaseAutoPractice.WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0,currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java new file mode 100644 index 0000000..33ccf2e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java @@ -0,0 +1,216 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +//@Autonomous (name = "PracticePark")// +public class PracticePark extends BaseAutoPractice { + private PracticeDriveTrain2025 driveTrain; + public int step = -1; + private int counter = 0; + private int current_step; + BaseAutoPractice.WayPoint[] wayPoints; + // auto driving data + private double current_x, current_y; + private double speed; + private double start_x, start_y; + private double dest_x, dest_y, set_dest; + private double x_speed, y_speed; + private boolean at_x, at_y, set_x, set_y; + private boolean at_xy; + + enum WorkingStep { + X, + Y + } + + WorkingStep currentStep; + + public void init() { + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.speed = 0.0; + this.at_x = false; + this.at_y = false; + this.at_xy = false; + + this.step = 1; + this.currentStep = WorkingStep.X; + this.x_speed = 0.5; + this.y_speed = 0.5; + this.wayPoints = new WayPoint[4]; +// this.wayPoints[0] = new WayPoint(); +// this.wayPoints[0].x = 50.0; +// this.wayPoints[0].y = 50.0; +// this.wayPoints[0].facing = 0.0; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 50.0; + this.wayPoints[1].y = 50.0; + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = -50.0; + this.wayPoints[2].y = -50.0; + this.wayPoints[2].facing = 0.0; + + // this.dest_x = 50.0;//initializing + +// this.wayPoints = new WayPoint[12]; +// +// this.wayPoints[0].x = 10.0;//initializing +// this.wayPoints[0].y = 0.0; +// this.wayPoints[0].facing = Constants.NORTH; + }//init + + @Override + public void loop() { + telemetry.addData("Sequence Step", this.step); + // Update x y coords + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Speed x", "%.2f", this.x_speed); + telemetry.addData("Speed y", "%.2f", this.y_speed); + telemetry.addData("heading:", "%.2f", driveTrain.heading); +// telemetry.addData("Dest x:","%.2f",this.dest_x); +// telemetry.addData("Dest y:","%.2f",this.dest_y); + telemetry.addData("Distance to X", "%.2f", (Math.abs(this.wayPoints[this.step].x - this.current_x))); + telemetry.addData("Distance to Y", "%.2f", Math.abs(this.wayPoints[this.step].y - this.current_y)); + telemetry.addData("Function Position", data); + + + //cases + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + default: + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + + +// driveTrain.setDirection(Constants.SOUTH); +// switch (this.step){ +// case 1: +// switch (currentStep){ +// case X: +// if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) +// { +// driveTrain.drive(0,this.x_speed); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.Y; +// } +// break; +// case Y: +// if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) +// { +// driveTrain.drive(this.y_speed,0.0); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.X; +// this.step += 1; +// } +// } +// break; +// case 2: +// switch (currentStep){ +// case X: +// if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) +// { +// // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); +// driveTrain.drive(0,-this.x_speed); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.Y; +// } +// break; +// case Y: +// if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) +// { +// // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); +// driveTrain.drive(-this.y_speed,0.0); +// } +// else +// { +// this.at_y = true; +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.X; +// this.step += 1; +// } +// } +// default: +// break; +// } + // driveTrain.odometer.update(); + +// if (counter <= 150){ +// driveTrain.drive(0.0,0.3); +// speed = 0.1; +// }else { +// driveTrain.drive(0.0,0.0); +// speed = 0.0; +// } +// //driveTrain.drive(0.0,0.1); +// counter = counter + 1; +// driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); + } // loop + }//everything + + private void goto_xy(WayPoint wayPoint, WayPoint wayPoint1) { + } + + private boolean at_xy(WayPoint wayPoint, WayPoint wayPoint1) { + return false; + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java new file mode 100644 index 0000000..e538131 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java @@ -0,0 +1,399 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +//@Autonomous(name = "PracticeSpecimen") +public class PracticeSpecimen extends BaseAutoPractice { + private PracticeDriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + BaseAutoPractice.WayPoint[] wayPoints; + + public void init(){ + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.current_step = 1; + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = -582.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = -746.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].y = -560.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].facing = Constants.NORTH; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 1118.0; + this.wayPoints[4].y = -560.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 1118.0; + this.wayPoints[5].y = -560.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.SOUTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 1118.0; + this.wayPoints[6].y = 200.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 1118.0; + this.wayPoints[7].y = 200.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.NORTH; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 0.0; + this.wayPoints[8].y = 0.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 0.0; + this.wayPoints[9].y = -582.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + //Waypoint + this.wayPoints[11] = new WayPoint(); + this.wayPoints[11].x = 0.0; + this.wayPoints[11].y = -540.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Waypoint + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = 640.0; + this.wayPoints[12].y = -540.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Waypoint + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = 640.0; + this.wayPoints[13].y = -1350.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.NORTH; + //Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = 990.0; + this.wayPoints[14].y = -200.0; + this.wayPoints[14].y_speed = 0.4; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + //Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = 990.0; + this.wayPoints[15].y = -1350.0; + this.wayPoints[15].y_speed = 0.4; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.NORTH; + //Waypoint + this.wayPoints[16] = new WayPoint(); + this.wayPoints[16].x = 1220.0; + this.wayPoints[16].y = -200.0; + this.wayPoints[16].y_speed = 0.4; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.NORTH; + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 5: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 6: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 7: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 8: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 12: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 13: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 14: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 15: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 16: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +}//everything + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java index bd4cd63..b96b601 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java @@ -1,10 +1,8 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -@Autonomous(name = "AutoRedParkShort_Test") +//@Autonomous(name = "AutoRedParkShort_Test") public class RedShort extends BaseAutoPractice { private int currentStep; //Location location = location.DEPOSIT_SPECIMEN; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java new file mode 100644 index 0000000..7679ec6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java @@ -0,0 +1,389 @@ +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +//@Autonomous(name = "testGOTO") +public class gotoXY_test extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + TelemetryPacket pack; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + // Dashboard Telemetry + // ftc-dashboard telemetry + + dashboardtelemtry.addData("X",driveTrain.getXPosition()); + dashboardtelemtry.addData("Y",driveTrain.getYPosition()); + dashboardtelemtry.update(); + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // Dashboard Setup + pack = new TelemetryPacket(); + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 711; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 494.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 293.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 180.0; + this.wayPoints[3].y = 80.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 295.0; + this.wayPoints[4].y = 506.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + // driveTrain.loop(); + driveTrain.odometer.update(); +dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y)); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + // ftc-dashboard telemetry + pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); + pack.put("X distance:", driveTrain.getXPosition()); + pack.put("Y distance:", driveTrain.getYPosition()); + + dashboardtelemtry.addData("headingError", driveTrain.headingError); + dashboardtelemtry.update(); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + driveTrain.setFacing(Constants.EAST); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + dashboardtelemtry.addData("made to else", ""); + dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + // this.current_step++; + } + +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// //dashboardtelemtry.addData("in CASE 1!!!!!",""); +// // dashboardtelemtry.update(); +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// dashboardtelemtry.update(); +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } + + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + + // elevator.setElevatorState(3); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// // this.claw.openClaw(); +// this.current_step++; + break; + + // this.current_step++; + +// case 3://turn to SW +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// elevator.setElevatorState(3); +// this.current_step++; +// } +// break; +// case 4:// drive to basket at 135 heading +// stop(); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else { +// driveTrain.drive(0.0, 0.0); +// +// this.current_step++; +// } +// break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) + { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + double totalError = Math.sqrt(errorX*errorX + errorY*errorY); + dashboardtelemtry.addData("Error X",errorX); + dashboardtelemtry.addData("Error Y",errorY); + dashboardtelemtry.addData("heading:",driveTrain.heading); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) + { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) + { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.addData("Step", this.current_step); + dashboardtelemtry.addData("headingError", driveTrain.headingError); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java index 9cfb57c..2ab113e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java @@ -18,7 +18,7 @@ -@TeleOp(name = "PracticeBot") +//@TeleOp(name = "PracticeBot") public class PracticeBot extends OpMode { private PracticeDriveTrain2025 driveTrain; @@ -39,7 +39,7 @@ public void init() { // Drive Train.init(); driveTrain.init(); - odometer.resetPosAndIMU(); + driveTrain.odometer.resetPosAndIMU(); telemetry.addData("Status", "Initialized"); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java index 8f43e35..4b488d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java @@ -28,6 +28,7 @@ public class PracticeBotTesting extends OpMode { GoBildaPinpointDriver odometer; @Override + //just testing team laptop public void init() { telemetry = new CAITelemetry(telemetry); ((CAITelemetry)telemetry).setDashboardEnabled(true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java new file mode 100644 index 0000000..2100160 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java @@ -0,0 +1,349 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "testGOTO_old") +public class Test_GoTo_bruce extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + private PIDController headingControl = null; + private PIDController xControl = null; + private PIDController yControl = null; + WayPoint[] wayPoints; + + + public void init_loop() { + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized", "Done"); + } + } + + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + + xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? + yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 640.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 640.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 100.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 640.0; + this.wayPoints[3].y = 100.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 295.0; + this.wayPoints[4].y = 506.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + TelemetryPacket pack = new TelemetryPacket(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y, this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 2], this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 2].x, this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 2].y, this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + + // data to dashboard--------------------------------------COMMENT OUT FOR COMPETITION + pack.put("heading",driveTrain.heading); + pack.put ("X",driveTrain.getXPosition()); + pack.put ("y:",driveTrain.getYPosition()); + + FtcDashboard.getInstance().sendTelemetryPacket(pack); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } + if (!at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]))// if not at waypoint continue + { + goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + } + break; + case 2: + // strafe to line up with basketr + + // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + // this.current_step++; + break; + case 3://turn to SW + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + + this.current_step++; + } + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + double locationX = driveTrain.getXPosition(); + double locationY = driveTrain.getYPosition(); + double errorX = Math.abs(locationX - currentWaypoint.x); + double errorY = Math.abs(locationY - currentWaypoint.y); + // pack.put("Xerror:",errorX); + // pack.put("Yerror:",errorY); + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + return Math.sqrt((errorX * errorX) + (errorY * errorY)) <= Constants.AT_XY_TOLERANCE; + + + } + + public boolean at_x(double previousX, double currentX) { + double error; + error = Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); + telemetry.addData("X Error:", error); + return Math.abs((Math.abs(currentX) - Math.abs(driveTrain.getXPosition()))) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) { + double error; + error = Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); + telemetry.addData("Y Error:", error); + return Math.abs((Math.abs(currentY) - Math.abs(driveTrain.getYPosition()))) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + + xControl.setSetPoint(currentWaypoint.x);// PID setpoint is new X + yControl.setSetPoint(currentWaypoint.y);// PID setpoint is new Y + // now move robot using PID for x & y position targets + driveTrain.drive(xControl.calculate(driveTrain.getXPosition()), yControl.calculate(driveTrain.getYPosition())); + + }// end of goto_xy +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java new file mode 100644 index 0000000..daf5d3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.Testing; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.Mechanisms.Logger; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "TurnTest") +public class AutoTurn extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step = 0; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + private Logger logger; + private boolean settime = false; + private double startTime; + + public void init_loop() { + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + } + + public void init() { + + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].facing = Constants.WEST; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].facing = Constants.SOUTH; + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].facing = Constants.EAST; + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + this.logger = new Logger("autoturn.log",true); + this.logger.writeLog("Auto Turning Starting"); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + dashboardtelemtry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Function Position", data); + } + + public void loop(){ + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Function Position", data); + dashboardtelemtry.update(); + if (!settime){ + startTime = getRuntime(); + settime = true; + } + else { + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (getRuntime() - startTime >= 0.2) + { + this.current_step++; + settime = false; + } + } + if (this.current_step > 3){ + this.current_step = 0; + } + this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + } + + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java new file mode 100644 index 0000000..707727f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java @@ -0,0 +1,145 @@ +package org.firstinspires.ftc.teamcode.Testing; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Logger; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Config +@Autonomous(name = "DriveTest") +public class WayPoints extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step = 0; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + private Logger logger; + private boolean settime = false; + private double startTime; + public static double pause_time = 0.2; + private int circle_count = 0; + + public void init_loop() { + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.update(); + } + + public void init() { + + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[1].x = 254.0; + this.wayPoints[1].y = 254.0; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].facing = Constants.NORTH; + this.wayPoints[2].x = 127.0; + this.wayPoints[2].y = 512.0; + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + this.logger = new Logger("autoturn.log",true); + this.logger.writeLog("Auto Turning Starting"); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + dashboardtelemtry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Function Position", data); + driveTrain.odometer.resetPosAndIMU(); + } + + public void loop(){ + Pose2D pos = driveTrain.odometer.getPosition(); + dashboardtelemtry.update(); + switch (this.current_step) { + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: +// driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + default: + break; + } + this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + driveTrain.loop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file