33import static org .firstinspires .ftc .teamcode .Params .aem ;
44import static org .firstinspires .ftc .teamcode .Params .pem ;
55import static org .firstinspires .ftc .teamcode .Params .timeOutProtectionMills ;
6- import static org .firstinspires .ftc .teamcode .utils .clients .DashboardClient .Blue ;
76
87import androidx .annotation .NonNull ;
98
109import com .acmerobotics .dashboard .canvas .Canvas ;
1110import com .acmerobotics .dashboard .telemetry .TelemetryPacket ;
1211import com .acmerobotics .roadrunner .Action ;
13- import com .acmerobotics .roadrunner .Pose2d ;
14- import com .acmerobotics .roadrunner .Vector2d ;
1512import com .acmerobotics .roadrunner .ftc .Actions ;
1613
1714import org .firstinspires .ftc .teamcode .Global ;
2926import org .firstinspires .ftc .teamcode .utils .Functions ;
3027import org .firstinspires .ftc .teamcode .utils .PID .PidContent ;
3128import org .firstinspires .ftc .teamcode .utils .PID .PidProcessor ;
29+ import org .firstinspires .ftc .teamcode .utils .Position2d ;
3230import org .firstinspires .ftc .teamcode .utils .Timer ;
31+ import org .firstinspires .ftc .teamcode .utils .Vector2d ;
3332import org .firstinspires .ftc .teamcode .utils .annotations .DrivingPrograms ;
3433import org .firstinspires .ftc .teamcode .utils .clients .Client ;
3534import org .firstinspires .ftc .teamcode .utils .clients .DashboardClient ;
@@ -45,15 +44,15 @@ public class MecanumDrive implements DriverProgram {
4544 private final PidProcessor pidProcessor ;
4645 private final String [] ContentTags ;
4746
48- public final LinkedList <Pose2d > poseHistory = new LinkedList <>();
49- public Pose2d RobotPosition ;
47+ public final LinkedList <Position2d > poseHistory = new LinkedList <>();
48+ public Position2d RobotPosition ;
5049 public double BufPower =1f ;
5150
5251 public final Localizer localizer ;
5352
5453 public RobotState robotState ;
5554
56- public MecanumDrive (Pose2d RobotPosition ){
55+ public MecanumDrive (Position2d RobotPosition ){
5756 this .client =Global .client ;
5857 this .pidProcessor =Global .robot .pidProcessor ;
5958 this .robotState = Robot .robotState ;
@@ -79,7 +78,7 @@ public void update() {
7978 RobotPosition = localizer .getCurrentPose ();
8079
8180 client .dashboard .deletePacketByTag ("RobotPosition" );
82- client .dashboard .DrawRobot (RobotPosition , Blue , "RobotPosition" );
81+ client .dashboard .DrawRobot (RobotPosition , DashboardClient . Blue , "RobotPosition" );
8382
8483 poseHistory .add (RobotPosition );
8584 }
@@ -101,11 +100,11 @@ public void runOrderPackage(@NonNull LinkedList<DriveOrder> orders) {
101100
102101 Vector2d [] PoseList ;
103102 PoseList =new Vector2d [commandLists .length +1 ];
104- PoseList [0 ]=commandLists [0 ].pose .position ;
103+ PoseList [0 ]=commandLists [0 ].pose .asVector () ;
105104 Timer timer = new Timer ();
106105
107106 for (int i =0 ;i <commandLists .length ;++i ){
108- PoseList [i +1 ]=commandLists [i ].NEXT ().position ;
107+ PoseList [i +1 ]=commandLists [i ].NEXT ().asVector () ;
109108 }
110109
111110 Actions .runBlocking (new Action () {
@@ -115,11 +114,11 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
115114 DriveAction singleAction =commandLists [ID ];
116115 singleAction .RUN ();
117116 update ();
118- motors .updateDriveOptions (RobotPosition .heading . toDouble () );
117+ motors .updateDriveOptions (RobotPosition .heading );
119118
120119 BufPower = singleAction .BufPower ;
121- double dY = singleAction .getDeltaTrajectory ().position . y ;
122- double dX = singleAction .getDeltaTrajectory ().position . x ;
120+ double dY = singleAction .getDeltaTrajectory ().y ;
121+ double dX = singleAction .getDeltaTrajectory ().x ;
123122 final double distance =Math .sqrt (dX * dX + dY * dY );
124123 final double estimatedTime =distance /(Params .secPowerPerInch *BufPower );
125124 client .changeData ("distance" ,distance );
@@ -128,12 +127,12 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
128127 client .changeData ("DELTA" , singleAction .getDeltaTrajectory ().toString ());
129128
130129 timer .restart ();
131- while ((Math .abs (RobotPosition .position . x -PoseList [ID +1 ].x )> pem )
132- && (Math .abs (RobotPosition .position . y -PoseList [ID +1 ].y )> pem )
133- && (Math .abs (RobotPosition .heading . toDouble () -singleAction .NEXT ().heading . toDouble () )> aem )){
130+ while ((Math .abs (RobotPosition .x -PoseList [ID +1 ].x )> pem )
131+ && (Math .abs (RobotPosition .y -PoseList [ID +1 ].y )> pem )
132+ && (Math .abs (RobotPosition .heading -singleAction .NEXT ().heading )> aem )){
134133 double progress =(timer .stopAndGetDeltaTime () / 1000.0 ) / estimatedTime * 100 ;
135134 client .changeData ("progress" , progress +"%" );
136- Pose2d aim = Functions .getAimPositionThroughTrajectory (singleAction ,RobotPosition ,progress );
135+ Position2d aim = Functions .getAimPositionThroughTrajectory (singleAction ,RobotPosition ,progress );
137136
138137 if (timer .getDeltaTime ()>estimatedTime + timeOutProtectionMills && Params .Configs .useOutTimeProtection ){//保护机制
139138 robotState = RobotState .BrakeDown ;
@@ -142,14 +141,14 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
142141 }
143142
144143 if (Params .Configs .usePIDInAutonomous ){
145- if (Math .abs (aim .position . x - RobotPosition . position .x )> pem
146- || Math .abs (aim .position . y - RobotPosition . position .y )> pem
147- || Math .abs (aim .heading . toDouble () - RobotPosition .heading . toDouble () )> aem
144+ if (Math .abs (aim .x - RobotPosition .x )> pem
145+ || Math .abs (aim .y - RobotPosition .y )> pem
146+ || Math .abs (aim .heading - RobotPosition .heading )> aem
148147 || Params .Configs .alwaysRunPIDInAutonomous ){
149148 //间断地调用pid可能会导致pid的效果不佳
150- pidProcessor .registerInaccuracies (ContentTags [0 ], aim .position . x - RobotPosition . position .x );
151- pidProcessor .registerInaccuracies (ContentTags [1 ], aim .position . y - RobotPosition . position .y );
152- pidProcessor .registerInaccuracies (ContentTags [2 ], aim .heading . toDouble () - RobotPosition .heading . toDouble () );
149+ pidProcessor .registerInaccuracies (ContentTags [0 ], aim .x - RobotPosition .x );
150+ pidProcessor .registerInaccuracies (ContentTags [1 ], aim .y - RobotPosition .y );
151+ pidProcessor .registerInaccuracies (ContentTags [2 ], aim .heading - RobotPosition .heading );
153152
154153 pidProcessor .update ();
155154
@@ -158,13 +157,13 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
158157 motors .headingPower +=pidProcessor .getFulfillment (ContentTags [2 ]);
159158 }
160159 }else {
161- if (Math .abs (aim .position . x - RobotPosition . position .x )> pem
162- || Math .abs (aim .position . y - RobotPosition . position .y )> pem
163- || Math .abs (aim .heading . toDouble () - RobotPosition .heading . toDouble () )> aem ){
160+ if (Math .abs (aim .x - RobotPosition .x )> pem
161+ || Math .abs (aim .y - RobotPosition .y )> pem
162+ || Math .abs (aim .heading - RobotPosition .heading )> aem ){
164163 double [] fulfillment =new double []{
165- (aim .position . x - RobotPosition . position .x )*(Params .secPowerPerInch )*BufPower /2 ,
166- (aim .position . y - RobotPosition . position .y )*(Params .secPowerPerInch )*BufPower /2 ,
167- (aim .heading . toDouble () > RobotPosition .heading . toDouble () ? BufPower /2 :-BufPower /2 )
164+ (aim .x - RobotPosition .x )*(Params .secPowerPerInch )*BufPower /2 ,
165+ (aim .y - RobotPosition .y )*(Params .secPowerPerInch )*BufPower /2 ,
166+ (aim .heading > RobotPosition .heading ? BufPower /2 :-BufPower /2 )
168167 };
169168
170169 motors .xAxisPower +=fulfillment [0 ];
@@ -173,7 +172,7 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
173172 }
174173 }
175174
176- motors .updateDriveOptions (RobotPosition .heading . toDouble () );
175+ motors .updateDriveOptions (RobotPosition .heading );
177176 }
178177
179178 if (ID !=commandLists .length -1 ){
@@ -207,7 +206,7 @@ public Chassis getClassic() {
207206 }
208207
209208 @ Override
210- public Pose2d getCurrentPose () {
209+ public Position2d getCurrentPose () {
211210 return RobotPosition ;
212211 }
213212
0 commit comments