diff --git a/RIC.zip b/RIC.zip index 3251f28..050551c 100644 Binary files a/RIC.zip and b/RIC.zip differ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java index 1c05fad..4bf8abb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java @@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.utils.enums.State; import org.firstinspires.ftc.teamcode.utils.enums.TrajectoryType; import org.firstinspires.ftc.teamcode.utils.enums.driveDirection; -import org.jetbrains.annotations.Contract; import java.util.Arrays; import java.util.LinkedList; @@ -53,7 +52,7 @@ public static final class Params{ private final TelemetryPacket telemetryPacket; private final LinkedList poseHistory = new LinkedList<>(); - private Pose2d RobotPosition; + public Pose2d RobotPosition; private double BufPower=1f; private final Localizer localizer; @@ -83,7 +82,7 @@ public class DriveCommand { public abstract class commandRunningNode{ public void runCommand() {} } - + public commandRunningNode MEAN; private double BufPower; private Pose2d DeltaTrajectory; @@ -97,7 +96,7 @@ public void runCommand() {} this.BufPower=BufPower; this.pose=pose; } - + /** * 在该节点只修改电机BufPower，不会在定义时影响主程序 * @param power 目标设置的电机BufPower @@ -111,7 +110,7 @@ public void runCommand() { } }; } - + /** * 在该节点让机器旋转指定弧度 * @param radians 要转的弧度 @@ -125,7 +124,7 @@ public void runCommand() { }; DeltaTrajectory=new Pose2d(new Vector2d(0,0),radians); } - + /** * 在该节点让机器在指定角度行驶指定距离 * @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向） @@ -144,7 +143,7 @@ public void runCommand() { .toVector2d() ,radians); } - + /** * 在该节点让机器在不旋转的情况下平移 * @param pose 目标矢量点位 @@ -159,7 +158,7 @@ public void runCommand() { }; DeltaTrajectory=new Pose2d(cache.toVector2d(),this.pose.heading); } - + /** * 不要在自动程序中调用这个函数，否则你会后悔的 */ @@ -169,12 +168,11 @@ public void RUN(){ public Pose2d getDeltaTrajectory(){ return DeltaTrajectory; } - + /** * @return 该Command节点的目标点位 */ @NonNull - @Contract(" -> new") public Pose2d NEXT(){ return new Pose2d( pose.position.x+DeltaTrajectory.position.x, @@ -383,7 +381,6 @@ public void runDriveCommandPackage(@NonNull DriveCommandPackage driveCommandPack * @return 在目标进度下机器的理想位置 */ @NonNull - @Contract ("_, _, _ -> new") private Pose2d getAimPositionThroughTrajectory(@NonNull Pose2d from, @NonNull Pose2d end, double progress){ Complex cache=new Complex(new Vector2d( end.position.x-from.position.x, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java new file mode 100644 index 0000000..f2c64af --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.RIC_samples; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive.DriveCommandPackage; +import org.firstinspires.ftc.teamcode.utils.AutonomousProgramTemplate; +import org.firstinspires.ftc.teamcode.utils.enums.AutonomousLocation; + +@Autonomous(name = "AutonomousSample",group = "SAMPLE") +public class AutonomousSample extends AutonomousProgramTemplate { + @Override + public void runOpMode() { + Init(new Pose2d(0,0,0)); + robot.client.addData("Position","WAITING FOR REQUEST"); + AutonomousLocation location = AutonomousLocation.failed; + while (opModeIsNotActive()) { + location=robot.webcam.getLocation(); + robot.client.changeDate("Position",location.name()); + sleep(50); + } + + if(!WaitForStartRequest())return; + + robot.client.deleteDate("Position"); + switch (location){ + case left: + robot.strafeTo(robot.pose().position.plus(new Vector2d(0,24))); + robot.turnAngle(90); + robot.strafeTo(new Vector2d(24,0)); + break; + case centre: + DriveCommandPackage command=drive.drivingCommandsBuilder() + .StrafeTo(robot.pose().position.plus(new Vector2d(0,24))) + .TurnAngle(90) + .StrafeInDistance(Math.toRadians(-90),Math.sqrt(1152)) + .END(); + drive.runDriveCommandPackage(command); + break; + case right: + break; + case failed: + return; + } + + sleep(1145141919); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index b0bf5d1..916f366 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -4,6 +4,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -100,6 +101,12 @@ private void InitInManualDrive(){ } public void update() { + NowTimeMills=System.currentTimeMillis(); + + if(NowTimeMills-StartTimeMills>=90000){ + state=State.FinalState; + } + sensors.update(); servos.update(); if(RuntimeOption.driverUsingAxisPowerInsteadOfCurrentPower) { @@ -131,6 +138,15 @@ public void turnAngle(double angle){ if(RunningState==runningState.ManualDrive)return; drive.runDriveCommandPackage(drive.drivingCommandsBuilder().TurnAngle(angle).END()); } + public void strafeTo(Vector2d pose){ + if(RunningState==runningState.ManualDrive)return; + drive.runDriveCommandPackage(drive.drivingCommandsBuilder().StrafeTo(pose).END()); + } + + public Pose2d pose(){ + drive.update(); + return drive.RobotPosition; + } /** * 将会把BufPower全部分配给电机 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java new file mode 100644 index 0000000..d6a2aa1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.teamcode.utils.enums.runningState; + +public abstract class AutonomousProgramTemplate extends LinearOpMode { + public Robot robot; + public SimpleMecanumDrive drive; + + public void Init(Pose2d position){ + robot=new Robot(hardwareMap, runningState.Autonomous,telemetry); + drive=robot.enableSimpleMecanumDrive(position); + } + public boolean WaitForStartRequest(){ + while(opModeIsNotActive()){ + sleep(50); + } + + return opModeStopped(); + } + public boolean opModeIsNotActive(){ + return !opModeIsActive()&&!isStopRequested(); + } + public boolean opModeStopped(){ + return opModeIsActive() && ! isStopRequested(); + } +}