加入 Timer

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java

@@ -25,6 +25,7 @@
 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.firstinspires.ftc.teamcode.utils.Timer;
 
 import java.util.Arrays;
 import java.util.LinkedList;
@@ -261,7 +262,7 @@ public void runDriveCommands(@NonNull LinkedList < DriveCommand > commands){
 		yList=new double[commandLists.length+1];
 		xList[0]=commandLists[0].pose.position.x;
 		yList[0]=commandLists[0].pose.position.y;
-		long st,et;
+		Timer timer = new Timer();
 		for ( int i = 0, commandListsLength = commandLists.length; i < commandListsLength; i++ ) {
 			DriveCommand singleCommand = commandLists[i];
 			singleCommand.RUN();
@@ -290,16 +291,15 @@ public void runDriveCommands(@NonNull LinkedList < DriveCommand > commands){
 			client.addData("progress","0%");
 			client.addData("DELTA",singleCommand.getDeltaTrajectory().toString());
 
-			st=System.currentTimeMillis();
+			timer.restart();
 			while ((Math.abs(RobotPosition.position.x-xList[i+1])> pem)
 				&& (Math.abs(RobotPosition.position.y-yList[i+1])> pem)
 				&& (Math.abs(RobotPosition.heading.toDouble()-singleCommand.NEXT().heading.toDouble())> aem)){
-				et=System.currentTimeMillis();
-				double progress=((et - st) / 1000.0) / estimatedTime * 100;
+				double progress=(timer.stopAndGetDeltaTime() / 1000.0) / estimatedTime * 100;
 				client.changeDate("progress", progress +"%");
 				Pose2d aim=getAimPositionThroughTrajectory(singleCommand,progress);
 
-				if(et>st+estimatedTime+ timeOutProtectionMills&& Params.Configs.useOutTimeProtection){//保护机制
+				if(timer.getDeltaTime()>estimatedTime+ timeOutProtectionMills&& Params.Configs.useOutTimeProtection){//保护机制
 					state=State.BrakeDown;
 					motors.updateDriveOptions();
 					break;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/ManualCodeSample.java

@@ -5,17 +5,18 @@
 
 import org.firstinspires.ftc.teamcode.Robot;
 import org.firstinspires.ftc.teamcode.utils.Enums.runningState;
+import org.firstinspires.ftc.teamcode.utils.Timer;
 
 @TeleOp(name = "ManualCodeSample",group = "samples")
 public class ManualCodeSample extends OpMode {
 	public Robot robot;
-	public double st,et;
+	public Timer timer;
 
 	@Override
 	public void init() {
 		robot=new Robot(hardwareMap, runningState.ManualDrive,telemetry);
-		st=System.currentTimeMillis();
 		robot.client.addData("TPS","NEED TO START THE OpMode TO SEE THE VALUE.");
+		timer=new Timer();
 	}
 
 	@Override
@@ -27,8 +28,6 @@ public void loop() {
 	}
 
 	public void updateTPS(){
-		et=System.currentTimeMillis();
-		robot.client.changeDate("TPS", 1000/(et-st));
-		st=System.currentTimeMillis();
+		robot.client.changeDate("TPS", 1000/(timer.restartAndGetDeltaTime()));
 	}
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java

@@ -17,16 +17,17 @@
 import org.firstinspires.ftc.teamcode.Hardwares.Classic;
 import org.firstinspires.ftc.teamcode.Hardwares.Structure;
 import org.firstinspires.ftc.teamcode.Hardwares.Webcam;
-import org.firstinspires.ftc.teamcode.Hardwares.namespace.DeviceMap;
 import org.firstinspires.ftc.teamcode.Hardwares.basic.Motors;
 import org.firstinspires.ftc.teamcode.Hardwares.basic.Sensors;
 import org.firstinspires.ftc.teamcode.Hardwares.basic.Servos;
+import org.firstinspires.ftc.teamcode.Hardwares.namespace.DeviceMap;
 import org.firstinspires.ftc.teamcode.utils.Annotations.ExtractedInterfaces;
 import org.firstinspires.ftc.teamcode.utils.Client;
-import org.firstinspires.ftc.teamcode.utils.PID_processor;
 import org.firstinspires.ftc.teamcode.utils.Enums.ClipPosition;
 import org.firstinspires.ftc.teamcode.utils.Enums.State;
 import org.firstinspires.ftc.teamcode.utils.Enums.runningState;
+import org.firstinspires.ftc.teamcode.utils.PID_processor;
+import org.firstinspires.ftc.teamcode.utils.Timer;
 
 import java.util.Objects;
 
@@ -48,7 +49,7 @@ public class Robot {
 	public runningState RunningState;
 	private SimpleMecanumDrive drive=null;
 
-	public long StartTimeMills,NowTimeMills;
+	public Timer timer;
 
 	public Robot(@NonNull HardwareMap hardwareMap, @NonNull runningState state, @NonNull Telemetry telemetry){
 		this(hardwareMap,state,new Client(telemetry));
@@ -80,7 +81,7 @@ public Robot(@NonNull HardwareMap hardwareMap, @NonNull runningState state, @Non
 		}
 
 		RunningState=state;
-		StartTimeMills=System.currentTimeMillis();
+		timer=new Timer();
 		client.addData("State","UnKnow");
 	}
 
@@ -107,9 +108,7 @@ private void InitInManualDrive(){
 	}
 
 	public void update()  {
-		NowTimeMills=System.currentTimeMillis();
-
-		if(NowTimeMills-StartTimeMills>=90000){
+		if(timer.stopAndGetDeltaTime()>=90000){
 			state=State.FinalState;
 		}
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Functions.java

@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+public final class Functions {
+    public static double getCurrentTimeMills(){
+        return System.nanoTime()/ 1.0E06;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Mathematics.java

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.utils;
 
-public class Mathematics {
+public final class Mathematics {
 	public static double intervalClip(double value,double min,double max){
 		return Math.min(Math.max(min,value),max);
 	}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Templates/TeleopProgramTemplate.java

@@ -4,28 +4,28 @@
 
 import org.firstinspires.ftc.teamcode.Robot;
 import org.firstinspires.ftc.teamcode.utils.Enums.runningState;
+import org.firstinspires.ftc.teamcode.utils.Timer;
 
 public abstract class TeleopProgramTemplate extends OpMode {
 	public Robot robot;
-	public long st,et;
+	public Timer timer;
 	@Override
 	public void init() {
 		robot=new Robot(hardwareMap, runningState.ManualDrive,telemetry);
+		timer=new Timer();
 		whenInit();
 	}
 
 	@Override
 	public void start() {
 		super.start();
-		st=System.currentTimeMillis();
+		timer.restart();
 		robot.client.addData("TPS","WAIT FOR START");
 	}
 
 	@Override
 	public void loop() {
-		et=System.currentTimeMillis();
-		robot.client.changeDate("TPS", (double) (et - st) /1000);
-		st=et;
+		robot.client.changeDate("TPS", timer.restartAndGetDeltaTime() /1000);
 
 		whileActivating();
 	}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Timer.java

@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+public class Timer {
+    public double StartTime,EndTime;
+    public Timer(){
+        StartTime=Functions.getCurrentTimeMills();
+    }
+    /**重新定义<code>StartTime</code>*/
+    public final void restart(){
+        StartTime=Functions.getCurrentTimeMills();
+    }
+    /**定义<code>EndTime</code>*/
+    public final void stop(){
+        EndTime=Functions.getCurrentTimeMills();
+    }
+    /**获取<code>EndTime-StartTime</code>*/
+    public final double getDeltaTime(){
+        return EndTime-StartTime;
+    }
+    /**定义<code>EndTime</code>并获取<code>EndTime-StartTime</code>*/
+    public final double stopAndGetDeltaTime(){
+        stop();
+        return getDeltaTime();
+    }
+    /**定义<code>EndTime</code>并获取<code>EndTime-StartTime</code>并重新定义<code>StartTime</code>*/
+    public final double restartAndGetDeltaTime(){
+        double res=stopAndGetDeltaTime();
+        restart();
+        return res;
+    }
+}