Off season

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);
+
+    }
+}

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;
+    }
+}