Skip to content

Commit 60c1af1

Browse files
6-BennyLi-96-BennyLi-9
committed
优化代码
1 parent 2e9a67a commit 60c1af1

File tree

10 files changed

+52
-75
lines changed

10 files changed

+52
-75
lines changed

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

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -270,17 +270,17 @@ public void runDriveCommands(@NonNull LinkedList < DriveCommand > commands){
270270
double dX = Math.abs(PoseList[i + 1].x - PoseList[i].x);
271271
final double distance=Math.sqrt(dX * dX + dY * dY);
272272
final double estimatedTime=distance/(Params.vP /(1f/BufPower));
273-
client.addData("distance",distance);
274-
client.addData("estimatedTime",estimatedTime);
275-
client.addData("progress","0%");
276-
client.addData("DELTA",singleCommand.getDeltaTrajectory().toString());
273+
client.changeData("distance",distance);
274+
client.changeData("estimatedTime",estimatedTime);
275+
client.changeData("progress","0%");
276+
client.changeData("DELTA",singleCommand.getDeltaTrajectory().toString());
277277

278278
timer.restart();
279279
while ((Math.abs(RobotPosition.position.x-PoseList[i+1].x)> pem)
280280
&& (Math.abs(RobotPosition.position.y-PoseList[i+1].y)> pem)
281281
&& (Math.abs(RobotPosition.heading.toDouble()-singleCommand.NEXT().heading.toDouble())> aem)){
282282
double progress=(timer.stopAndGetDeltaTime() / 1000.0) / estimatedTime * 100;
283-
client.changeDate("progress", progress +"%");
283+
client.changeData("progress", progress +"%");
284284
Pose2d aim=getAimPositionThroughTrajectory(singleCommand,progress);
285285

286286
if(timer.getDeltaTime()>estimatedTime+ timeOutProtectionMills&& Params.Configs.useOutTimeProtection){//保护机制
@@ -325,12 +325,12 @@ public void runDriveCommands(@NonNull LinkedList < DriveCommand > commands){
325325
motors.updateDriveOptions(RobotPosition.heading.toDouble());
326326
}
327327

328-
client.deleteDate("distance");
329-
client.deleteDate("estimatedTime");
330-
client.deleteDate("progress");
331-
client.deleteDate("DELTA");
332328
state= State.WaitingAtPoint;
333329
}
330+
client.deleteData("distance");
331+
client.deleteData("estimatedTime");
332+
client.deleteData("progress");
333+
client.deleteData("DELTA");
334334

335335
classic.STOP();
336336
state= State.IDLE;

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Params.java

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,12 @@
11
package org.firstinspires.ftc.teamcode;
22

33
public final class Params {
4+
public final static class PIDParams{
5+
public static double[] kP;
6+
public static double[] kI;
7+
public static double[] kD;
8+
public static double[] MAX_I;
9+
}
410
public final static class Configs{
511
/**让机器自动在运行<code>update()</code>时，自动清除所有电机的<code>power</code>*/
612
public static boolean autoPrepareForNextOptionWhenUpdate = true;

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,13 +17,13 @@ public void runOpMode() {
1717
AutonomousLocation location = AutonomousLocation.failed;
1818
while (opModeIsNotActive()) {
1919
location=robot.webcam.getLocation();
20-
robot.client.changeDate("Position",location.name());
20+
robot.changeData("Position",location.name());
2121
sleep(50);
2222
}
2323

2424
if(WaitForStartRequest())return;
2525

26-
robot.client.deleteDate("Position");
26+
robot.client.deleteData("Position");
2727
switch (location){
2828
case left:
2929
robot.strafeTo(robot.pose().position.plus(new Vector2d(0,24)));

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/ClientUsage.java

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,23 +32,23 @@ private void operateThroughTime(){
3232
client.addData("按下次数","1");
3333
break;
3434
case 2:
35-
client.changeDate("按下次数","2");
35+
client.changeData("按下次数","2");
3636
break;
3737
case 3:
3838
client.addLine("第三次按下");
39-
client.deleteDate("按下次数");
39+
client.deleteData("按下次数");
4040
break;
4141
case 4:
4242
client.deleteLine("第三次按下");
4343
client.addData("按下次数","4");
4444
break;
4545
case 5:
4646
client.addLine("第五次按下");
47-
client.deleteDate("按下次数");
47+
client.deleteData("按下次数");
4848
break;
4949
case 6:
5050
client.changeLine("第五次按下","第六次按下");
51-
client.changeDate("按下次数","6");
51+
client.changeData("按下次数","6");
5252
break;
5353
case 7:
5454
client.clear();

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,6 @@ public void loop() {
2828
}
2929

3030
public void updateTPS(){
31-
robot.client.changeDate("TPS", 1000/(timer.restartAndGetDeltaTime()));
31+
robot.client.changeData("TPS", 1000/(timer.restartAndGetDeltaTime()));
3232
}
3333
}

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ public void update() {
121121
motors.update();
122122
}
123123

124-
client.changeDate("State",state.name());
124+
client.changeData("State",state.name());
125125
while(Params.Configs.waitForServoUntilThePositionIsInPlace && servos.InPlace()){
126126
//当前最方便的Sleep方案
127127
Actions.runBlocking(new SleepAction(0.1));
@@ -172,13 +172,13 @@ public void SetGlobalBufPower(double BufPower){
172172
@ExtractedInterfaces
173173
public void addData(String key,double val){client.addData(key, val);}
174174
@ExtractedInterfaces
175-
public void deleteDate(String key){try{client.deleteDate(key);}catch (Exception ignored){}}
175+
public void deleteDate(String key){try{client.deleteData(key);}catch (Exception ignored){}}
176176
@ExtractedInterfaces
177-
public void changeData(String key, String val){client.changeDate(key, val);}
177+
public void changeData(String key, String val){client.changeData(key, val);}
178178
@ExtractedInterfaces
179-
public void changeData(String key,int val){client.changeDate(key, val);}
179+
public void changeData(String key,int val){client.changeData(key, val);}
180180
@ExtractedInterfaces
181-
public void changeData(String key,double val){client.changeDate(key, val);}
181+
public void changeData(String key,double val){client.changeData(key, val);}
182182
@ExtractedInterfaces
183183
public void addLine(String val){client.addLine(val);}
184184
@ExtractedInterfaces

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Client.java

Lines changed: 9 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,6 @@
2525
*/
2626
public class Client {
2727
public static final class Drawing {
28-
private Drawing() {}
29-
3028
public static final String Blue="#3F51B5";
3129
public static final String Green="#4CAF50";
3230

@@ -35,6 +33,7 @@ private Drawing() {}
3533
* @param c 绘制的关键程序变量
3634
* @param t 机器位置
3735
*/
36+
@UtilFunctions
3837
public static void drawRobot(@NonNull Canvas c, @NonNull Pose2d t) {
3938
final double ROBOT_RADIUS = 9;
4039

@@ -47,28 +46,14 @@ public static void drawRobot(@NonNull Canvas c, @NonNull Pose2d t) {
4746
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
4847
}
4948

50-
/**
51-
* 智能地根据机器的点位，但是是瞬时的，一旦FtcDashboard有了更新，则很有可能机器的图案会消失
52-
* @param pose 机器位置
53-
*/
54-
@UtilFunctions
55-
public static void drawInstantRobot(@NonNull Pose2d pose){
56-
TelemetryPacket packet=new TelemetryPacket();
57-
packet.fieldOverlay().setStroke(Blue);
58-
drawRobot(packet.fieldOverlay(),pose);
59-
FtcDashboard.getInstance().sendTelemetryPacket(packet);
60-
}
61-
6249
/**
6350
* 默认为蓝色
6451
* @param pose 机器位置
6552
* @param packet 使用packet绘制机器
6653
*/
6754
@UtilFunctions
6855
public static void drawRobotUsingPacket(@NonNull Pose2d pose,@NonNull TelemetryPacket packet){
69-
packet.fieldOverlay().setStroke(Blue);
70-
drawRobot(packet.fieldOverlay(),pose);
71-
FtcDashboard.getInstance().sendTelemetryPacket(packet);
56+
drawRobotUsingPacket(pose,packet,Blue);
7257
}
7358
/**
7459
* @param pose 机器位置
@@ -77,7 +62,6 @@ public static void drawRobotUsingPacket(@NonNull Pose2d pose,@NonNull TelemetryP
7762
@UtilFunctions
7863
public static void drawRobotUsingPacket(@NonNull Pose2d pose,@NonNull TelemetryPacket packet,@NonNull String color){
7964
packet.fieldOverlay().setStroke(color);
80-
packet.fieldOverlay().setStroke(Blue);
8165
drawRobot(packet.fieldOverlay(),pose);
8266
FtcDashboard.getInstance().sendTelemetryPacket(packet);
8367
}
@@ -120,19 +104,13 @@ public void addData(String key,String val){
120104
/**
121105
* 注意：这是新的Data
122106
*/
123-
public void addData(String key,int val){
124-
addData(key,String.valueOf(val));
125-
}
126-
/**
127-
* 注意：这是新的Data
128-
*/
129-
public void addData(String key,double val){
107+
public void addData(String key,Object val){
130108
addData(key,String.valueOf(val));
131109
}
132110
/**
133111
* @throws RuntimeException 如果未能找到key所指向的值，将会抛出异常
134112
*/
135-
public void deleteDate(String key){
113+
public void deleteData(String key){
136114
if( data.containsKey(key)){
137115
data.remove(key);
138116
}else{
@@ -144,9 +122,9 @@ public void deleteDate(String key){
144122
/**
145123
* 自动创建新的行如果key所指向的值不存在
146124
*/
147-
public void changeDate(String key,String val){
125+
public void changeData(String key, String val){
148126
if(data.containsKey(key)){
149-
data.replace(key,new Pair<>(val, Objects.requireNonNull(data.get(key)).second));
127+
data.replace(key,new Pair<>(val, (Objects.requireNonNull(data.get(key))).second));
150128
}else{
151129
addData(key, val);
152130
}
@@ -155,24 +133,15 @@ public void changeDate(String key,String val){
155133
/**
156134
* 自动创建新的行如果key所指向的值不存在
157135
*/
158-
public void changeDate(String key,int val){
159-
changeDate(key,String.valueOf(val));
160-
}
161-
/**
162-
* 自动创建新的行如果key所指向的值不存在
163-
*/
164-
public void changeDate(String key,double val){
165-
changeDate(key,String.valueOf(val));
136+
public void changeData(String key, Object val){
137+
changeData(key,String.valueOf(val));
166138
}
167139

168140
public void addLine(String key){
169141
lines.put(key,++ID);
170142
update();
171143
}
172-
public void addLine(int key){
173-
addLine(String.valueOf(key));
174-
}
175-
public void addLine(double key){
144+
public void addLine(Object key){
176145
addLine(String.valueOf(key));
177146
}
178147
/**
@@ -224,7 +193,6 @@ public void update(){
224193
FtcDashboard.getInstance().sendTelemetryPacket(packet);
225194
}
226195

227-
228196
/**
229197
* 推荐使用的DrawRobot方法。可以自动使用packet进行draw
230198
* <p>

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

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
package org.firstinspires.ftc.teamcode.utils;
22

3+
import org.firstinspires.ftc.teamcode.utils.Annotations.UtilFunctions;
4+
35
public final class Functions {
6+
@UtilFunctions
47
public static double getCurrentTimeMills(){
5-
return System.nanoTime()/ 1.0E06;
8+
return System.nanoTime()/1.0E06;
69
}
710
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PID_processor.java

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
package org.firstinspires.ftc.teamcode.utils;
22

33

4+
import org.firstinspires.ftc.teamcode.Params;
5+
46
import java.util.Arrays;
57

68
public class PID_processor {
79
private static final int N = 3;
8-
private static double[] kP,kI,kD;
9-
private static double[] MAX_I;
1010

1111
private final double[] P,I,D;
1212
public double[] inaccuracies,lastInaccuracies,fulfillment;
@@ -24,18 +24,18 @@ public PID_processor(long TimeNow) {
2424

2525
//与底盘相关的kP理论值：SimpleMecanumDrive.Params.vP
2626
//TODO:预设...[0]为底盘X，[1]为底盘Y，[2]为底盘方向
27-
kP= new double[]{0.12, 0.15, 0.12};
28-
kI= new double[]{0, 0, 0};
29-
kD= new double[]{0.04, 0.05, 0.04};
27+
Params.PIDParams.kP= new double[]{0.12, 0.15, 0.12};
28+
Params.PIDParams.kI= new double[]{0, 0, 0};
29+
Params.PIDParams.kD= new double[]{0.04, 0.05, 0.04};
3030

31-
MAX_I= new double[]{100, 100, 0};//可以用0代替所有与角度有关的I
31+
Params.PIDParams.MAX_I= new double[]{100, 100, 0};//可以用0代替所有与角度有关的I
3232
}
3333

3434
private void I_processor(int ID){
3535
if(ID==2){//TODO:列出所有与角度有关的ID
3636
I[ID]=Mathematics.angleRationalize(I[ID]);
3737
}else{
38-
I[ID]=Mathematics.intervalClip(I[ID],-MAX_I[ID],MAX_I[ID]);
38+
I[ID]=Mathematics.intervalClip(I[ID],-Params.PIDParams.MAX_I[ID], Params.PIDParams.MAX_I[ID]);
3939
}
4040
}
4141

@@ -45,12 +45,12 @@ private void I_processor(int ID){
4545
* @param ID 要调用那一个数据的编号
4646
*/
4747
public void ModifyPID(long runTime,int ID){
48-
P[ID]=inaccuracies[ID]*kP[ID];
49-
I[ID]+=inaccuracies[ID]*kI[ID]*runTime;
48+
P[ID]=inaccuracies[ID]* Params.PIDParams.kP[ID];
49+
I[ID]+=inaccuracies[ID]* Params.PIDParams.kI[ID]*runTime;
5050

5151
I_processor(ID);
5252

53-
D[ID]=(inaccuracies[ID]-lastInaccuracies[ID])*kD[ID]/runTime;
53+
D[ID]=(inaccuracies[ID]-lastInaccuracies[ID])* Params.PIDParams.kD[ID]/runTime;
5454
lastInaccuracies[ID]=inaccuracies[ID];
5555

5656
fulfillment[ID]=P[ID]+I[ID]+D[ID];
@@ -69,7 +69,7 @@ public void simplePID(int ID){
6969
* 刷新所有PID
7070
*/
7171
public void update(){
72-
for(int i=0;i<kP.length;++i){
72+
for(int i = 0; i< Params.PIDParams.kP.length; ++i){
7373
simplePID(i);
7474
}
7575
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ public void start() {
2525

2626
@Override
2727
public void loop() {
28-
robot.client.changeDate("TPS", timer.restartAndGetDeltaTime() /1000);
28+
robot.client.changeData("TPS", timer.restartAndGetDeltaTime() /1000);
2929

3030
whileActivating();
3131
}

0 commit comments

Comments
 (0)