Skip to content

Commit aa8f3ea

Browse files
6-BennyLi-96-BennyLi-9
committed
加入 DrivingActionsBuilder
1 parent d7db4c9 commit aa8f3ea

File tree

4 files changed

+107
-35
lines changed

4 files changed

+107
-35
lines changed

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

Lines changed: 26 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,36 @@
11
package org.firstinspires.ftc.teamcode.DriveControls.Actions;
22

3+
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommand;
4+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrder;
5+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrderPackage;
6+
37
import java.util.LinkedList;
48

5-
public class DriveActionPackage {
9+
public class DriveActionPackage implements DriveOrderPackage {
610
public LinkedList<DriveAction> actions;
711

812
DriveActionPackage(){
913
actions=new LinkedList<>();
1014
}
15+
16+
@Override
17+
public LinkedList<DriveOrder> getOrder() {
18+
LinkedList<DriveOrder> res=new LinkedList<>();
19+
for (DriveOrder order : actions) {
20+
res.push(order);
21+
}
22+
return res;
23+
}
24+
25+
@Override
26+
public void setOrder(LinkedList<DriveOrder> val) {
27+
if(val != null) {
28+
actions=new LinkedList<>();
29+
for (DriveOrder order : val) {
30+
actions.push((DriveAction) order);
31+
}
32+
}else{
33+
throw new NullPointerException();
34+
}
35+
}
1136
}

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

Lines changed: 54 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,16 @@
22

33
import androidx.annotation.NonNull;
44

5+
import com.acmerobotics.roadrunner.Vector2d;
6+
7+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrderBuilder;
8+
import org.firstinspires.ftc.teamcode.DriveControls.DriveOrderPackage;
59
import org.firstinspires.ftc.teamcode.DriveControls.DriverProgram;
610
import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive;
11+
import org.firstinspires.ftc.teamcode.utils.Enums.TrajectoryType;
12+
import org.firstinspires.ftc.teamcode.utils.Mathematics;
713

8-
public class DrivingActionsBuilder {
14+
public class DrivingActionsBuilder implements DriveOrderBuilder {
915
private final DriveActionPackage actionPackage;
1016
private final DriverProgram drive;
1117
private DriveAction cache;
@@ -20,4 +26,51 @@ public DrivingActionsBuilder(@NonNull SimpleMecanumDrive drive) {
2026
this.drive = drive;
2127
}
2228

29+
@Override
30+
public DriveOrderBuilder SetPower(double power) {
31+
power = Mathematics.intervalClip(power, -1f, 1f);
32+
cache = new DriveAction(drive.getClassic(), actionPackage.actions.getLast().BufPower, actionPackage.actions.getLast().NEXT());
33+
cache.SetPower(power);
34+
cache.trajectoryType = TrajectoryType.WithoutChangingPosition;
35+
actionPackage.actions.add(cache);
36+
return new DrivingActionsBuilder(drive, actionPackage);
37+
}
38+
39+
@Override
40+
public DriveOrderBuilder TurnRadians(double radians) {
41+
radians = Mathematics.intervalClip(radians, -Math.PI, Math.PI);
42+
cache = new DriveAction(drive.getClassic(), actionPackage.actions.getLast().BufPower, actionPackage.actions.getLast().NEXT());
43+
cache.Turn(radians);
44+
cache.trajectoryType = TrajectoryType.TurnOnly;
45+
actionPackage.actions.add(cache);
46+
return new DrivingActionsBuilder(drive, actionPackage);
47+
}
48+
49+
@Override
50+
public DriveOrderBuilder TurnAngle(double deg) {
51+
return TurnRadians(Math.toRadians(deg));
52+
}
53+
54+
@Override
55+
public DriveOrderBuilder StrafeInDistance(double radians, double distance) {
56+
cache = new DriveAction(drive.getClassic(), actionPackage.actions.getLast().BufPower, actionPackage.actions.getLast().NEXT());
57+
cache.StrafeInDistance(radians, distance);
58+
cache.trajectoryType = TrajectoryType.LinerStrafe;
59+
actionPackage.actions.add(cache);
60+
return new DrivingActionsBuilder(drive, actionPackage);
61+
}
62+
63+
@Override
64+
public DriveOrderBuilder StrafeTo(Vector2d pose) {
65+
cache = new DriveAction(drive.getClassic(), actionPackage.actions.getLast().BufPower, actionPackage.actions.getLast().NEXT());
66+
cache.StrafeTo(pose);
67+
cache.trajectoryType = TrajectoryType.LinerStrafe;
68+
actionPackage.actions.add(cache);
69+
return new DrivingActionsBuilder(drive, actionPackage);
70+
}
71+
72+
@Override
73+
public DriveOrderPackage END() {
74+
return actionPackage;
75+
}
2376
}

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

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

55
import com.acmerobotics.roadrunner.Vector2d;
66

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

12-
public class DrivingCommandsBuilder {
13+
public class DrivingCommandsBuilder implements DriveOrderBuilder {
1314
private final DriveCommandPackage commandPackage;
1415
private final DriverProgram drive;
1516
private DriveCommand cache;
@@ -25,11 +26,6 @@ public DrivingCommandsBuilder(@NonNull SimpleMecanumDrive drive) {
2526
this.drive = drive;
2627
}
2728

28-
/**
29-
* 在该节点只修改电机BufPower，不会在定义时影响主程序
30-
*
31-
* @param power 目标设置的电机BufPower
32-
*/
3329
public DrivingCommandsBuilder SetPower(double power) {
3430
power = Mathematics.intervalClip(power, -1f, 1f);
3531
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
@@ -39,11 +35,6 @@ public DrivingCommandsBuilder SetPower(double power) {
3935
return new DrivingCommandsBuilder(drive, commandPackage);
4036
}
4137

42-
/**
43-
* 在该节点让机器旋转指定弧度
44-
*
45-
* @param radians 要转的弧度[-PI,PI)
46-
*/
4738
public DrivingCommandsBuilder TurnRadians(double radians) {
4839
radians = Mathematics.intervalClip(radians, -Math.PI, Math.PI);
4940
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
@@ -53,21 +44,10 @@ public DrivingCommandsBuilder TurnRadians(double radians) {
5344
return new DrivingCommandsBuilder(drive, commandPackage);
5445
}
5546

56-
/**
57-
* 在该节点让机器旋转指定角度
58-
*
59-
* @param deg 要转的角度[-180,180)
60-
*/
6147
public DrivingCommandsBuilder TurnAngle(double deg) {
6248
return TurnRadians(Math.toRadians(deg));
6349
}
6450

65-
/**
66-
* 在该节点让机器在指定角度行驶指定距离
67-
*
68-
* @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向）
69-
* @param distance 要行驶的距离
70-
*/
7151
public DrivingCommandsBuilder StrafeInDistance(double radians, double distance) {
7252
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
7353
cache.StrafeInDistance(radians, distance);
@@ -76,11 +56,6 @@ public DrivingCommandsBuilder StrafeInDistance(double radians, double distance)
7656
return new DrivingCommandsBuilder(drive, commandPackage);
7757
}
7858

79-
/**
80-
* 在该节点让机器在不旋转的情况下平移
81-
*
82-
* @param pose 目标矢量点位
83-
*/
8459
public DrivingCommandsBuilder StrafeTo(Vector2d pose) {
8560
cache = new DriveCommand(drive.getClassic(), commandPackage.commands.getLast().BufPower, commandPackage.commands.getLast().NEXT());
8661
cache.StrafeTo(pose);
@@ -89,9 +64,6 @@ public DrivingCommandsBuilder StrafeTo(Vector2d pose) {
8964
return new DrivingCommandsBuilder(drive, commandPackage);
9065
}
9166

92-
/**
93-
* 结束该DriveCommandPackage
94-
*/
9567
public DriveCommandPackage END() {
9668
return commandPackage;
9769
}

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

Lines changed: 25 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,35 @@
22

33
import com.acmerobotics.roadrunner.Vector2d;
44

5-
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommandPackage;
6-
75
public interface DriveOrderBuilder {
6+
/**
7+
* 在该节点只修改电机BufPower，不会在定义时影响主程序
8+
* @param power 目标设置的电机BufPower
9+
*/
810
DriveOrderBuilder SetPower(double power);
11+
/**
12+
* 在该节点让机器旋转指定弧度
13+
* @param radians 要转的弧度[-PI,PI)
14+
*/
915
DriveOrderBuilder TurnRadians(double radians);
16+
/**
17+
* 在该节点让机器旋转指定角度
18+
* @param deg 要转的角度[-180,180)
19+
*/
1020
DriveOrderBuilder TurnAngle(double deg);
21+
/**
22+
* 在该节点让机器在指定角度行驶指定距离
23+
* @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向）
24+
* @param distance 要行驶的距离
25+
*/
1126
DriveOrderBuilder StrafeInDistance(double radians, double distance);
27+
/**
28+
* 在该节点让机器在不旋转的情况下平移
29+
* @param pose 目标矢量点位
30+
*/
1231
DriveOrderBuilder StrafeTo(Vector2d pose);
13-
DriveCommandPackage END();
32+
/**
33+
* 结束该 DriveOrderBuilder
34+
*/
35+
DriveOrderPackage END();
1436
}

0 commit comments

Comments
 (0)