diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshBot.java new file mode 100644 index 000000000000..26023379eaea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshBot.java @@ -0,0 +1,69 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.drivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.drivetrain.MecanumDrive; +@Config +@TeleOp +public class FreshBot extends LinearOpMode { + public static double P=0, I=0, D=0; + ElapsedTime headingTimer = new ElapsedTime(); + + // add extends LinearOpMode to inherit object Linear OpMode + public void runOpMode() throws InterruptedException { // throws error if something is wrong + Drivetrain drivetrain = new MecanumDrive(hardwareMap); // creates new instance of the object Drivetrain; includes hardware map + Imu imu = new Imu(hardwareMap); + PIDController headingControl = new PIDController(P, I, D); + double targetheading = 0; + double turnpower = 0; + boolean holdHeading = true; + + waitForStart(); // initialising code will be entered before; if nothing then have to start to start code + while(opModeIsActive()){ + + double driveTurn = Math.pow(-gamepad1.right_stick_x, 1); // adds game pad controls to + double driveY = -Math.pow(-gamepad1.left_stick_x, 1); + double driveX = -Math.pow(-gamepad1.left_stick_y, 1); + double magnitude = Math.hypot(driveX, driveY); + double theta = Math.toDegrees(Math.atan2(driveY, driveX)); + theta = theta - imu.getHeading(); + headingControl.setPID(P,I,D); + + if(driveTurn == 0){ + if (headingTimer.seconds()>1){ + double currentHeading = imu.getHeading(); + double error = targetheading - currentHeading; + turnpower = headingControl.calculate(0, error); + if(Math.abs(error)<2){ + turnpower = 0; + } + driveTurn = turnpower; + } + else{ + targetheading = imu.getHeading(); + } + } + else{ + headingTimer.reset(); + targetheading = imu.getHeading(); + + } + + drivetrain.drive(magnitude, theta, driveTurn, 0.8); + + + telemetry.addData("heading: ", imu.getHeading()); + telemetry.addData("turnPower: ", turnpower); + telemetry.addData("targetHeading: ", targetheading); + telemetry.addData("driveTurn: ", driveTurn); + telemetry.update(); + + } + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshClaw.java new file mode 100644 index 000000000000..b4b7c16e3845 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshClaw.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.qualcomm.robotcore.hardware.Servo; + +public class FreshClaw { + + Servo Claw; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshSlides.java new file mode 100644 index 000000000000..508dd17d6b9d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/FreshSlides.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class FreshSlides { + DcMotorEx Slides1; + DcMotorEx Slides2; + + double slidePower; + + // constructor + public FreshSlides(HardwareMap hwmap){ + Slides1 = hwmap.get(DcMotorEx.class, "Slide1"); + Slides2 = hwmap.get(DcMotorEx.class, "Slide2"); + Slides1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + Slides2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + slidePower = 1; + } + + public void goUp(){ + Slides1.setPower(slidePower); + Slides2.setPower(slidePower); + } + + public void goDown(){ + Slides1.setPower(-slidePower); + Slides2.setPower(-slidePower); + } + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Imu.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Imu.java new file mode 100644 index 000000000000..887ccca480b7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Imu.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class Imu { + private IMU imu; + public Imu(HardwareMap Hardwaremap){ + imu = Hardwaremap.get(IMU.class,"imu"); + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.RIGHT))); + imu.resetYaw(); + } + + public double getHeading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + } + +} diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fceae6e..8049c684f04f 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.5-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/testFile b/testFile new file mode 100644 index 000000000000..e69de29bb2d1