Skip to content

Commit d7d8e78

Browse files
6-BennyLi-96-BennyLi-9
committed
加入基于 Action 的 MecanumDrive
1 parent 52e893e commit d7d8e78

File tree

7 files changed

+334
-95
lines changed

7 files changed

+334
-95
lines changed

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

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -111,4 +111,19 @@ public Pose2d NEXT() {
111111
pose.heading.toDouble() + DeltaTrajectory.heading.toDouble()
112112
);
113113
}
114+
115+
@Override
116+
public double getBufVal() {
117+
return BufPower;
118+
}
119+
120+
@Override
121+
public Pose2d getPose() {
122+
return pose;
123+
}
124+
125+
@Override
126+
public TrajectoryType getState() {
127+
return trajectoryType;
128+
}
114129
}

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

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,4 +107,19 @@ public Pose2d NEXT() {
107107
pose.heading.toDouble() + DeltaTrajectory.heading.toDouble()
108108
);
109109
}
110+
111+
@Override
112+
public double getBufVal() {
113+
return BufPower;
114+
}
115+
116+
@Override
117+
public Pose2d getPose() {
118+
return pose;
119+
}
120+
121+
@Override
122+
public TrajectoryType getState() {
123+
return trajectoryType;
124+
}
110125
}

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,15 @@
22

33
import androidx.annotation.NonNull;
44

5-
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommand;
6-
import org.firstinspires.ftc.teamcode.DriveControls.Commands.DriveCommandPackage;
5+
import org.firstinspires.ftc.teamcode.DriveControls.OrderDefinition.DriveOrder;
6+
import org.firstinspires.ftc.teamcode.DriveControls.OrderDefinition.DriveOrderPackage;
77
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
88

99
import java.util.LinkedList;
1010

1111
public interface DriverProgram {
1212
void update();
13-
default void runCommandPackage(@NonNull DriveCommandPackage commandPackage){}
14-
default void runCommandPackage(@NonNull LinkedList<DriveCommand> commands){}
13+
default void runCommandPackage(@NonNull DriveOrderPackage orderPackage){}
14+
default void runCommandPackage(@NonNull LinkedList<DriveOrder> orders){}
1515
Classic getClassic();
1616
}

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

Lines changed: 191 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,191 @@
1+
package org.firstinspires.ftc.teamcode.DriveControls;
2+
3+
import static org.firstinspires.ftc.teamcode.Params.aem;
4+
import static org.firstinspires.ftc.teamcode.Params.pem;
5+
import static org.firstinspires.ftc.teamcode.Params.timeOutProtectionMills;
6+
7+
import androidx.annotation.NonNull;
8+
9+
import com.acmerobotics.dashboard.canvas.Canvas;
10+
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
11+
import com.acmerobotics.roadrunner.Action;
12+
import com.acmerobotics.roadrunner.Pose2d;
13+
import com.acmerobotics.roadrunner.Vector2d;
14+
import com.acmerobotics.roadrunner.ftc.Actions;
15+
16+
import org.firstinspires.ftc.teamcode.DriveControls.Actions.DriveAction;
17+
import org.firstinspires.ftc.teamcode.DriveControls.Localizers.DeadWheelSubassemblyLocalizer;
18+
import org.firstinspires.ftc.teamcode.DriveControls.Localizers.LocalizerDefinition.Localizer;
19+
import org.firstinspires.ftc.teamcode.DriveControls.OrderDefinition.DriveOrder;
20+
import org.firstinspires.ftc.teamcode.DriveControls.OrderDefinition.DriveOrderPackage;
21+
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
22+
import org.firstinspires.ftc.teamcode.Hardwares.basic.Motors;
23+
import org.firstinspires.ftc.teamcode.Params;
24+
import org.firstinspires.ftc.teamcode.Robot;
25+
import org.firstinspires.ftc.teamcode.Utils.Client;
26+
import org.firstinspires.ftc.teamcode.Utils.Client.Drawing;
27+
import org.firstinspires.ftc.teamcode.Utils.Enums.State;
28+
import org.firstinspires.ftc.teamcode.Utils.Functions;
29+
import org.firstinspires.ftc.teamcode.Utils.PID_processor;
30+
import org.firstinspires.ftc.teamcode.Utils.Timer;
31+
32+
import java.util.LinkedList;
33+
34+
public class MecanumDrive implements DriverProgram{
35+
public final Classic classic;
36+
private final Motors motors;
37+
private final Client client;
38+
private final PID_processor pidProcessor;
39+
40+
public final LinkedList<Pose2d> poseHistory = new LinkedList<>();
41+
public Pose2d RobotPosition;
42+
public double BufPower=1f;
43+
44+
public final Localizer localizer;
45+
46+
public State state;
47+
48+
public MecanumDrive(@NonNull Classic classic, Client client,
49+
PID_processor pidProcessor, State state, Pose2d RobotPosition){
50+
this.classic=classic;
51+
this.client=client;
52+
this.pidProcessor=pidProcessor;
53+
this.state=state;
54+
this.RobotPosition=RobotPosition;
55+
motors=classic.motors;
56+
57+
//TODO:更换Localizer如果需要
58+
localizer=new DeadWheelSubassemblyLocalizer(classic);
59+
}
60+
public MecanumDrive(@NonNull Robot robot,Pose2d RobotPosition){
61+
this(robot.classic,robot.client,robot.pidProcessor,robot.state,RobotPosition);
62+
}
63+
64+
@Override
65+
public void update() {
66+
localizer.update();
67+
RobotPosition = localizer.getCurrentPose();
68+
69+
client.DrawRobot(RobotPosition);
70+
71+
poseHistory.add(RobotPosition);
72+
}
73+
74+
@Override
75+
public void runCommandPackage(@NonNull DriveOrderPackage orderPackage) {
76+
runCommandPackage(orderPackage.getOrder());
77+
}
78+
79+
/**
80+
* @see SimpleMecanumDrive
81+
*/
82+
@Override
83+
public void runCommandPackage(@NonNull LinkedList<DriveOrder> orders) {
84+
DriveAction[] commandLists=new DriveAction[orders.size()];
85+
for (int i = 0 ; i < orders.size(); i++) {
86+
commandLists[i]= (DriveAction) orders.get(i);
87+
}
88+
89+
Vector2d[] PoseList;
90+
PoseList=new Vector2d[commandLists.length+1];
91+
PoseList[0]=commandLists[0].pose.position;
92+
Timer timer = new Timer();
93+
94+
for(int i=0;i<commandLists.length;++i){
95+
PoseList[i+1]=commandLists[i].NEXT().position;
96+
}
97+
98+
Actions.runBlocking(new Action() {
99+
public int ID=0;
100+
@Override
101+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
102+
DriveAction singleAction =commandLists[ID];
103+
singleAction.RUN();
104+
update();
105+
motors.updateDriveOptions(RobotPosition.heading.toDouble());
106+
107+
BufPower= singleAction.BufPower;
108+
double dY = singleAction.getDeltaTrajectory().position.y;
109+
double dX = singleAction.getDeltaTrajectory().position.x;
110+
final double distance=Math.sqrt(dX * dX + dY * dY);
111+
final double estimatedTime=distance/(Params.vP*BufPower);
112+
client.changeData("distance",distance);
113+
client.changeData("estimatedTime",estimatedTime);
114+
client.changeData("progress","0%");
115+
client.changeData("DELTA", singleAction.getDeltaTrajectory().toString());
116+
117+
timer.restart();
118+
while ((Math.abs(RobotPosition.position.x-PoseList[ID+1].x)> pem)
119+
&& (Math.abs(RobotPosition.position.y-PoseList[ID+1].y)> pem)
120+
&& (Math.abs(RobotPosition.heading.toDouble()-singleAction.NEXT().heading.toDouble())> aem)){
121+
double progress=(timer.stopAndGetDeltaTime() / 1000.0) / estimatedTime * 100;
122+
client.changeData("progress", progress +"%");
123+
Pose2d aim= Functions.getAimPositionThroughTrajectory(singleAction,RobotPosition,progress);
124+
125+
if(timer.getDeltaTime()>estimatedTime+ timeOutProtectionMills&& Params.Configs.useOutTimeProtection){//保护机制
126+
state=State.BrakeDown;
127+
motors.updateDriveOptions();
128+
break;
129+
}
130+
131+
if(Params.Configs.usePIDInAutonomous){
132+
if(Math.abs(aim.position.x- RobotPosition.position.x)> pem
133+
|| Math.abs(aim.position.y- RobotPosition.position.y)> pem
134+
|| Math.abs(aim.heading.toDouble()- RobotPosition.heading.toDouble())> aem
135+
|| Params.Configs.alwaysRunPIDInAutonomous ){
136+
//间断地调用pid可能会导致pid的效果不佳
137+
pidProcessor.inaccuracies[0]=aim.position.x- RobotPosition.position.x;
138+
pidProcessor.inaccuracies[1]=aim.position.y- RobotPosition.position.y;
139+
pidProcessor.inaccuracies[2]=aim.heading.toDouble()- RobotPosition.heading.toDouble();
140+
pidProcessor.update();
141+
142+
double[] fulfillment=pidProcessor.fulfillment;
143+
144+
motors.xAxisPower+=fulfillment[0];
145+
motors.yAxisPower+=fulfillment[1];
146+
motors.headingPower+=fulfillment[2];
147+
}
148+
}else{
149+
if(Math.abs(aim.position.x- RobotPosition.position.x)> pem
150+
|| Math.abs(aim.position.y- RobotPosition.position.y)> pem
151+
|| Math.abs(aim.heading.toDouble()- RobotPosition.heading.toDouble())> aem){
152+
double[] fulfillment=new double[]{
153+
(aim.position.x- RobotPosition.position.x)*(Params.vP)*BufPower/2,
154+
(aim.position.y- RobotPosition.position.y)*(Params.vP)*BufPower/2,
155+
(aim.heading.toDouble()> RobotPosition.heading.toDouble()? BufPower/2:-BufPower/2)
156+
};
157+
158+
motors.xAxisPower+=fulfillment[0];
159+
motors.yAxisPower+=fulfillment[1];
160+
motors.headingPower+=fulfillment[2];
161+
}
162+
}
163+
164+
motors.updateDriveOptions(RobotPosition.heading.toDouble());
165+
}
166+
167+
return ID!=commandLists.length-1;
168+
}
169+
@Override
170+
public void preview(@NonNull Canvas fieldOverlay){
171+
fieldOverlay.setStroke(Drawing.Green);
172+
for(int i=0;i<PoseList.length;++i){
173+
fieldOverlay.strokeLine(PoseList[i].x,PoseList[i].y,PoseList[i+1].x,PoseList[i+1].y);
174+
}
175+
}
176+
});
177+
178+
client.deleteData("distance");
179+
client.deleteData("estimatedTime");
180+
client.deleteData("progress");
181+
client.deleteData("DELTA");
182+
183+
classic.STOP();
184+
state= State.IDLE;
185+
}
186+
187+
@Override
188+
public Classic getClassic() {
189+
return classic;
190+
}
191+
}

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55
import com.acmerobotics.roadrunner.Pose2d;
66
import com.acmerobotics.roadrunner.Vector2d;
77

8+
import org.firstinspires.ftc.teamcode.Utils.Enums.TrajectoryType;
9+
810
public interface DriveOrder {
911
/**
1012
* 在该节点只修改电机BufPower，不会在定义时影响主程序
@@ -39,4 +41,8 @@ public interface DriveOrder {
3941
*/
4042
@NonNull
4143
Pose2d NEXT();
44+
45+
double getBufVal();
46+
Pose2d getPose();
47+
TrajectoryType getState();
4248
}

0 commit comments

Comments
 (0)