Skip to content

Commit 07b5af6

Browse files
6-BennyLi-96-BennyLi-9
committed
加入 DriveOrder ，并加入 DriveAction ， 同时对其进行优化
1 parent af4d2b0 commit 07b5af6

File tree

6 files changed

+180
-35
lines changed

6 files changed

+180
-35
lines changed

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

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
package org.firstinspires.ftc.teamcode.DriveControls.Actions;
2+
3+
import androidx.annotation.NonNull;
4+
5+
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
6+
import com.acmerobotics.roadrunner.Action;
7+
import com.acmerobotics.roadrunner.Pose2d;
8+
import com.acmerobotics.roadrunner.Vector2d;
9+
import com.acmerobotics.roadrunner.ftc.Actions;
10+
11+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrder;
12+
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
13+
import org.firstinspires.ftc.teamcode.utils.Complex;
14+
import org.firstinspires.ftc.teamcode.utils.Enums.TrajectoryType;
15+
import org.firstinspires.ftc.teamcode.utils.Enums.driveDirection;
16+
import org.firstinspires.ftc.teamcode.utils.Mathematics;
17+
18+
public class DriveAction implements DriveOrder {
19+
private final Classic classic;
20+
21+
/**
22+
* 为了简化代码书写，我们使用了<code>@Override</code>的覆写来保存数据。
23+
* <p>如果使用enum，则代码会明显过于臃肿</p>
24+
*/
25+
public abstract static class actionRunningNode implements Action {}
26+
public actionRunningNode MEAN;
27+
public double BufPower;
28+
public Pose2d DeltaTrajectory;
29+
public final Pose2d pose;
30+
/**
31+
* <code>面向开发者：</code> 不建议在DriveCommands中更改trajectoryType的值，而是在drivingCommandsBuilder中
32+
*/
33+
public TrajectoryType trajectoryType = null;
34+
35+
DriveAction(final Classic classic, double BufPower, Pose2d pose) {
36+
this.BufPower = BufPower;
37+
this.pose = pose;
38+
this.classic = classic;
39+
}
40+
41+
@Override
42+
public void SetPower(double power) {
43+
MEAN=new actionRunningNode() {
44+
@Override
45+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
46+
BufPower=power;
47+
BufPower= Mathematics.intervalClip(BufPower,-1,1);
48+
return false;
49+
}
50+
};
51+
}
52+
53+
@Override
54+
public void Turn(double radians) {
55+
MEAN=new actionRunningNode() {
56+
@Override
57+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
58+
classic.drive(driveDirection.turn, BufPower);
59+
return false;
60+
}
61+
};
62+
DeltaTrajectory = new Pose2d(0, 0, radians);
63+
}
64+
65+
@Override
66+
public void StrafeInDistance(double radians, double distance) {
67+
MEAN=new actionRunningNode() {
68+
@Override
69+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
70+
classic.SimpleRadiansDrive(BufPower, radians);
71+
return false;
72+
}
73+
};
74+
DeltaTrajectory = new Pose2d(
75+
(new Complex(new Vector2d(distance, 0))).times(new Complex(Math.toDegrees(radians)))
76+
.divide(new Complex(Math.toDegrees(radians)).magnitude())
77+
.toVector2d()
78+
, radians
79+
);
80+
}
81+
82+
@Override
83+
public void StrafeTo(Vector2d pose) {
84+
Complex cache = new Complex(this.pose.position.minus(pose));
85+
MEAN=new actionRunningNode() {
86+
@Override
87+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
88+
classic.SimpleRadiansDrive(BufPower, Math.toRadians(cache.toDegree()));
89+
return false;
90+
}
91+
};
92+
DeltaTrajectory = new Pose2d(cache.toVector2d(), this.pose.heading);
93+
}
94+
95+
@Override
96+
public void RUN() {
97+
Actions.runBlocking(MEAN);
98+
}
99+
100+
@Override
101+
public Pose2d getDeltaTrajectory() {
102+
return DeltaTrajectory;
103+
}
104+
105+
@NonNull
106+
@Override
107+
public Pose2d NEXT() {
108+
return new Pose2d(
109+
pose.position.x + DeltaTrajectory.position.x,
110+
pose.position.y + DeltaTrajectory.position.y,
111+
pose.heading.toDouble() + DeltaTrajectory.heading.toDouble()
112+
);
113+
}
114+
}

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

Lines changed: 9 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,14 @@
55
import com.acmerobotics.roadrunner.Pose2d;
66
import com.acmerobotics.roadrunner.Vector2d;
77

8+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrder;
89
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
910
import org.firstinspires.ftc.teamcode.utils.Complex;
1011
import org.firstinspires.ftc.teamcode.utils.Enums.TrajectoryType;
1112
import org.firstinspires.ftc.teamcode.utils.Enums.driveDirection;
1213
import org.firstinspires.ftc.teamcode.utils.Mathematics;
1314

14-
public class DriveCommand {
15+
public class DriveCommand implements DriveOrder {
1516
private final Classic classic;
1617

1718
/**
@@ -38,11 +39,7 @@ public void runCommand() {
3839
this.classic = classic;
3940
}
4041

41-
/**
42-
* 在该节点只修改电机BufPower，不会在定义时影响主程序
43-
*
44-
* @param power 目标设置的电机BufPower
45-
*/
42+
@Override
4643
public void SetPower(double power) {
4744
MEAN = new commandRunningNode() {
4845
@Override
@@ -53,11 +50,7 @@ public void runCommand() {
5350
};
5451
}
5552

56-
/**
57-
* 在该节点让机器旋转指定弧度
58-
*
59-
* @param radians 要转的弧度
60-
*/
53+
@Override
6154
public void Turn(double radians) {
6255
MEAN = new commandRunningNode() {
6356
@Override
@@ -68,12 +61,7 @@ public void runCommand() {
6861
DeltaTrajectory = new Pose2d(new Vector2d(0, 0), radians);
6962
}
7063

71-
/**
72-
* 在该节点让机器在指定角度行驶指定距离
73-
*
74-
* @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向）
75-
* @param distance 要行驶的距离
76-
*/
64+
@Override
7765
public void StrafeInDistance(double radians, double distance) {
7866
MEAN = new commandRunningNode() {
7967
@Override
@@ -88,11 +76,7 @@ public void runCommand() {
8876
, radians);
8977
}
9078

91-
/**
92-
* 在该节点让机器在不旋转的情况下平移
93-
*
94-
* @param pose 目标矢量点位
95-
*/
79+
@Override
9680
public void StrafeTo(Vector2d pose) {
9781
Complex cache = new Complex(this.pose.position.minus(pose));
9882
MEAN = new commandRunningNode() {
@@ -104,20 +88,17 @@ public void runCommand() {
10488
DeltaTrajectory = new Pose2d(cache.toVector2d(), this.pose.heading);
10589
}
10690

107-
/**
108-
* 不要在自动程序中调用这个函数，否则你会后悔的
109-
*/
91+
@Override
11092
public void RUN() {
11193
MEAN.runCommand();
11294
}
11395

96+
@Override
11497
public Pose2d getDeltaTrajectory() {
11598
return DeltaTrajectory;
11699
}
117100

118-
/**
119-
* @return 该Command节点的目标点位
120-
*/
101+
@Override
121102
@NonNull
122103
public Pose2d NEXT() {
123104
return new Pose2d(

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

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,14 @@
44

55
import com.acmerobotics.roadrunner.Vector2d;
66

7+
import org.firstinspires.ftc.teamcode.DriveControls.DriverProgram;
78
import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive;
89
import org.firstinspires.ftc.teamcode.utils.Enums.TrajectoryType;
910
import org.firstinspires.ftc.teamcode.utils.Mathematics;
1011

1112
public class drivingCommandsBuilder {
1213
private final DriveCommandPackage commandPackage;
13-
private final SimpleMecanumDrive drive;
14+
private final DriverProgram drive;
1415
private DriveCommand cache;
1516

1617
public drivingCommandsBuilder(@NonNull SimpleMecanumDrive drive) {
@@ -19,7 +20,7 @@ public drivingCommandsBuilder(@NonNull SimpleMecanumDrive drive) {
1920
this.drive = drive;
2021
}
2122

22-
drivingCommandsBuilder(SimpleMecanumDrive drive, DriveCommandPackage commandPackage) {
23+
drivingCommandsBuilder(DriverProgram drive, DriveCommandPackage commandPackage) {
2324
this.commandPackage = commandPackage;
2425
this.drive = drive;
2526
}
@@ -31,7 +32,7 @@ public drivingCommandsBuilder(@NonNull SimpleMecanumDrive drive) {
3132
*/
3233
public drivingCommandsBuilder SetPower(double power) {
3334
power = Mathematics.intervalClip(power, -1f, 1f);
34-
cache = new DriveCommand(drive.classic, commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
35+
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
3536
cache.SetPower(power);
3637
cache.trajectoryType = TrajectoryType.WithoutChangingPosition;
3738
commandPackage.commands.add(cache);
@@ -45,7 +46,7 @@ public drivingCommandsBuilder SetPower(double power) {
4546
*/
4647
public drivingCommandsBuilder TurnRadians(double radians) {
4748
radians = Mathematics.intervalClip(radians, -Math.PI, Math.PI);
48-
cache = new DriveCommand(drive.classic, commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
49+
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
4950
cache.Turn(radians);
5051
cache.trajectoryType = TrajectoryType.TurnOnly;
5152
commandPackage.commands.add(cache);
@@ -68,7 +69,7 @@ public drivingCommandsBuilder TurnAngle(double deg) {
6869
* @param distance 要行驶的距离
6970
*/
7071
public drivingCommandsBuilder StrafeInDistance(double radians, double distance) {
71-
cache = new DriveCommand(drive.classic, commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
72+
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
7273
cache.StrafeInDistance(radians, distance);
7374
cache.trajectoryType = TrajectoryType.LinerStrafe;
7475
commandPackage.commands.add(cache);
@@ -81,7 +82,7 @@ public drivingCommandsBuilder StrafeInDistance(double radians, double distance)
8182
* @param pose 目标矢量点位
8283
*/
8384
public drivingCommandsBuilder StrafeTo(Vector2d pose) {
84-
cache = new DriveCommand(drive.classic, commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
85+
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
8586
cache.StrafeTo(pose);
8687
cache.trajectoryType = TrajectoryType.LinerStrafe;
8788
commandPackage.commands.add(cache);

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

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
package org.firstinspires.ftc.teamcode.DriveControls;
2+
3+
import androidx.annotation.NonNull;
4+
5+
import com.acmerobotics.roadrunner.Pose2d;
6+
import com.acmerobotics.roadrunner.Vector2d;
7+
8+
public interface DriveOrder {
9+
/**
10+
* 在该节点只修改电机BufPower，不会在定义时影响主程序
11+
* @param power 目标设置的电机BufPower
12+
*/
13+
void SetPower(double power);
14+
/**
15+
* 在该节点让机器旋转指定弧度
16+
* @param radians 要转的弧度
17+
*/
18+
void Turn(double radians);
19+
/**
20+
* 在该节点让机器在指定角度行驶指定距离
21+
*
22+
* @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向）
23+
* @param distance 要行驶的距离
24+
*/
25+
void StrafeInDistance(double radians, double distance);
26+
/**
27+
* 在该节点让机器在不旋转的情况下平移
28+
*
29+
* @param pose 目标矢量点位
30+
*/
31+
void StrafeTo(Vector2d pose);
32+
/**
33+
* 不要在自动程序中调用这个函数，否则你会后悔的
34+
*/
35+
void RUN();
36+
Pose2d getDeltaTrajectory();
37+
/**
38+
* @return 该Command节点的目标点位
39+
*/
40+
@NonNull
41+
Pose2d NEXT();
42+
}

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,13 @@
44

55
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommand;
66
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommandPackage;
7+
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
78

89
import java.util.LinkedList;
910

1011
public interface DriverProgram {
1112
void update();
1213
default void runCommandPackage(@NonNull DriveCommandPackage commandPackage){}
1314
default void runCommandPackage(@NonNull LinkedList<DriveCommand> commands){}
15+
Classic getClassic();
1416
}

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -147,7 +147,12 @@ public void runCommandPackage(@NonNull LinkedList <DriveCommand> commands){
147147
classic.STOP();
148148
state= State.IDLE;
149149
}
150-
150+
151+
@Override
152+
public Classic getClassic() {
153+
return classic;
154+
}
155+
151156
/**
152157
* @param driveCommandPackage 要执行的DriveCommandPackage，不建议在使用时才定义driveCommandPackage，虽然没有任何坏处
153158
*/

0 commit comments

Comments
 (0)