harddifferential.txt
61.8 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--c99 --list --split_sections --debug -c --asm --interleave -o.\flash\obj\harddifferential.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\harddifferential.d --cpu=Cortex-M4.fp --apcs=interwork -O1 --diag_suppress=9931,870 -I..\..\Libraries\CMSIS\Include -I..\..\Libraries\CMSIS\Device\ST\STM32F4xx\Include -I..\..\Libraries\STM32F4xx_StdPeriph_Driver\inc -I..\..\uCOS-III\uC-CPU -I..\..\uCOS-III\uC-LIB -I..\..\uCOS-III\uCOS-III\Ports -I..\..\uCOS-III\uCOS-III\Source -I..\..\uCOS-III\uC-CPU\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uC-LIB\Ports\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uCOS-III\Ports\ARM-Cortex-M4\Generic\RealView -I..\..\User -I..\..\User\bsp -I..\..\User\bsp\inc -I..\..\User\libapp -I..\..\RL-ARM\Config -I..\..\RL-ARM\Driver -I..\..\RL-ARM\RL-RTX\inc -I..\..\User\bsp\BSP -I..\..\RL-ARM\RL-CAN -I..\..\Libraries\DSP_LIB\Include -I..\..\MODBUS\modbus\rtu -I..\..\MODBUS\BARE\port -I..\..\MODBUS\modbus\include -I..\..\User\bsp\BSP -I..\..\PLC -I..\..\Avoid -I..\..\User\parameter -I..\..\User\LaserMotionCtr -I..\..\User\W5100S -I..\..\User\bsp -I..\..\User\CHASSIS -I..\..\User\CONTROLFUNCTION -I..\..\User\DATAUPDATE -I..\..\User\HARAWARE -I..\..\User\MOTORDRIVER -I..\..\User\NAVAGATION -I..\..\User\PLATFORM -I..\..\User\SENSOR -I.\RTE\_Flash -IC:\Users\YDJ\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\YDJ\AppData\Local\Arm\Packs\Keil\STM32F4xx_DFP\2.13.0\Drivers\CMSIS\Device\ST\STM32F4xx\Include -D__UVISION_VERSION=527 -D_RTE_ -DSTM32F407xx -DUSE_STDPERIPH_DRIVER -DSTM32F40_41xxx -D__RTX -D__FPU_USED=1 --omf_browse=.\flash\obj\harddifferential.crf ..\..\User\CHASSIS\HardDifferential.c]
THUMB
AREA ||i.HardDifferentialSpeedPirouette||, CODE, READONLY, ALIGN=2
HardDifferentialSpeedPirouette PROC
;;;265 //硬差速原地转弯
;;;266 void HardDifferentialSpeedPirouette()
000000 e92d5ff0 PUSH {r4-r12,lr}
;;;267 {
000004 ed2d8b02 VPUSH {d8}
;;;268 static float agvInAngle = 0,turnAngleDiff = 0,lastAngleDiff = 0;
;;;269 static u8 TurnStep = 0, TurnArriveFlag = 0;
;;;270 static int lastTime = 0;
;;;271 // static int turnCount = 0;
;;;272 if (TurnStep == 0) //求出位置信息
;;;273 {
;;;274 navi.Private.TarAngle = CalculatingDirectionAngle(StartPoint, TargetPoint); //求出目标方向角度
;;;275
;;;276 agv.Command.SetPlatformTurnAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle))*57.3;
;;;277 agv.Command.SetBaseSpeed = 0;
;;;278
;;;279 TurnArriveFlag = 0;
;;;280
;;;281 if(fabs(KincoStruct1.encodeSpeed) < 5 && fabs(KincoStruct2.encodeSpeed) < 5)
;;;282 {
;;;283 if(turnCount ++ >1000)
;;;284 {
;;;285 TurnStep = 1;
000008 2501 MOVS r5,#1
00000a 4e8b LDR r6,|L1.568|
00000c f8df822c LDR r8,|L1.572|
000010 f8dfa22c LDR r10,|L1.576|
000014 7830 LDRB r0,[r6,#0] ;272 ; TurnStep
000016 f8df922c LDR r9,|L1.580|
00001a f8dfb22c LDR r11,|L1.584|
00001e 2700 MOVS r7,#0
000020 ed9f8b8a VLDR d8,|L1.588|
000024 4c8b LDR r4,|L1.596|
000026 2800 CMP r0,#0 ;272
000028 d002 BEQ |L1.48|
;;;286 turnCount = 0;
;;;287 }
;;;288
;;;289 }
;;;290
;;;291
;;;292 }
;;;293 else if (TurnStep == 1) //开始转弯
00002a 2801 CMP r0,#1
00002c d057 BEQ |L1.222|
00002e e0e5 B |L1.508|
|L1.48|
000030 4889 LDR r0,|L1.600|
000032 edd02a02 VLDR s5,[r0,#8] ;274
000036 ed902a01 VLDR s4,[r0,#4] ;274
00003a edd01a00 VLDR s3,[r0,#0] ;274
00003e 4887 LDR r0,|L1.604|
000040 ed901a02 VLDR s2,[r0,#8] ;274
000044 edd00a01 VLDR s1,[r0,#4] ;274
000048 ed900a00 VLDR s0,[r0,#0] ;274
00004c f7fffffe BL CalculatingDirectionAngle
000050 ed880a09 VSTR s0,[r8,#0x24] ;274
000054 ed9a1a02 VLDR s2,[r10,#8] ;276
000058 eef00a40 VMOV.F32 s1,s0 ;276
00005c eeb00a41 VMOV.F32 s0,s2 ;276
000060 f7fffffe BL CalculatingCurrentAndTargetAngle
000064 ee100a10 VMOV r0,s0 ;276
000068 f7fffffe BL __aeabi_f2d
00006c ec410b10 VMOV d0,r0,r1 ;276
000070 f7fffffe BL __hardfp_fabs
000074 ed9f1b7a VLDR d1,|L1.608|
000078 ec510b10 VMOV r0,r1,d0 ;276
00007c ec532b11 VMOV r2,r3,d1 ;276
000080 f7fffffe BL __aeabi_dmul
000084 f7fffffe BL __aeabi_d2f
000088 6220 STR r0,[r4,#0x20] ;276 ; agv
00008a 6027 STR r7,[r4,#0] ;277 ; agv
00008c 7077 STRB r7,[r6,#1] ;279
00008e f8d90004 LDR r0,[r9,#4] ;281 ; KincoStruct1
000092 f7fffffe BL __aeabi_f2d
000096 ec410b10 VMOV d0,r0,r1 ;281
00009a f7fffffe BL __hardfp_fabs
00009e ec532b18 VMOV r2,r3,d8 ;281
0000a2 ec510b10 VMOV r0,r1,d0 ;281
0000a6 f7fffffe BL __aeabi_cdcmple
0000aa d217 BCS |L1.220|
0000ac f8db0004 LDR r0,[r11,#4] ;281 ; KincoStruct2
0000b0 f7fffffe BL __aeabi_f2d
0000b4 ec410b10 VMOV d0,r0,r1 ;281
0000b8 f7fffffe BL __hardfp_fabs
0000bc ec532b18 VMOV r2,r3,d8 ;281
0000c0 ec510b10 VMOV r0,r1,d0 ;281
0000c4 f7fffffe BL __aeabi_cdcmple
0000c8 d208 BCS |L1.220|
0000ca 4967 LDR r1,|L1.616|
0000cc 6808 LDR r0,[r1,#0] ;283 ; turnCount
0000ce 1c42 ADDS r2,r0,#1 ;283
0000d0 600a STR r2,[r1,#0] ;283 ; turnCount
0000d2 f5b07f7a CMP r0,#0x3e8 ;283
0000d6 dd01 BLE |L1.220|
0000d8 7035 STRB r5,[r6,#0] ;285
0000da 600f STR r7,[r1,#0] ;286 ; turnCount
|L1.220|
0000dc e08e B |L1.508|
|L1.222|
;;;294 {
;;;295 if (agv.Command.CurDirection == 3 || agv.Command.CurDirection == 4) //前进左右转
0000de 7f20 LDRB r0,[r4,#0x1c] ; agv
0000e0 2803 CMP r0,#3
0000e2 d001 BEQ |L1.232|
0000e4 2804 CMP r0,#4
0000e6 d11a BNE |L1.286|
|L1.232|
;;;296 {
;;;297 turnAngleDiff = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle);
0000e8 edd80a09 VLDR s1,[r8,#0x24]
0000ec ed9a0a02 VLDR s0,[r10,#8]
0000f0 f7fffffe BL CalculatingCurrentAndTargetAngle
0000f4 ed860a08 VSTR s0,[r6,#0x20]
;;;298
;;;299 agvInAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle));
0000f8 edd80a09 VLDR s1,[r8,#0x24]
0000fc ed9a0a02 VLDR s0,[r10,#8]
000100 f7fffffe BL CalculatingCurrentAndTargetAngle
000104 ee100a10 VMOV r0,s0
000108 f7fffffe BL __aeabi_f2d
00010c ec410b10 VMOV d0,r0,r1
000110 f7fffffe BL __hardfp_fabs
000114 ec510b10 VMOV r0,r1,d0
000118 f7fffffe BL __aeabi_d2f
00011c 61f0 STR r0,[r6,#0x1c] ; agvInAngle
|L1.286|
;;;300 }
;;;301 // else //后退左右转
;;;302 // {
;;;303 // turnAngleDiff = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle + PI, navi.Private.TarAngle);
;;;304 //
;;;305 // agvInAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle + PI, navi.Private.TarAngle));
;;;306 // }
;;;307 if (agvInAngle >= PI * 2)
00011e ed960a07 VLDR s0,[r6,#0x1c]
000122 4952 LDR r1,|L1.620|
000124 ee100a10 VMOV r0,s0
000128 4288 CMP r0,r1
00012a db05 BLT |L1.312|
;;;308
;;;309 agvInAngle -= PI * 2;
00012c eddf0a50 VLDR s1,|L1.624|
000130 ee300a60 VSUB.F32 s0,s0,s1
000134 ed860a07 VSTR s0,[r6,#0x1c]
|L1.312|
;;;310
;;;311 if (agvInAngle >= ADVANCE_STOPSLOPE_ANGLE)
000138 ed960a07 VLDR s0,[r6,#0x1c]
00013c 494d LDR r1,|L1.628|
00013e ee100a10 VMOV r0,s0
000142 4288 CMP r0,r1
000144 db02 BLT |L1.332|
;;;312 {
;;;313 agv.Command.SetBaseSpeed = TURNSPEED_MAX;
000146 2064 MOVS r0,#0x64
000148 6020 STR r0,[r4,#0] ; agv
00014a e027 B |L1.412|
|L1.332|
;;;314 }
;;;315 else if ((agvInAngle < ADVANCE_STOPSLOPE_ANGLE) && (agvInAngle > TURNINGOFF_ANGLE))
00014c ee100a10 VMOV r0,s0
000150 4949 LDR r1,|L1.632|
000152 4408 ADD r0,r0,r1
000154 4949 LDR r1,|L1.636|
000156 4288 CMP r0,r1
000158 d20e BCS |L1.376|
;;;316 {
;;;317 agv.Command.SetBaseSpeed = (int)mapping(agvInAngle, TURNINGOFF_ANGLE, ADVANCE_STOPSLOPE_ANGLE, TURNSPEED_MIN, TURNSPEED_MAX);
00015a ed9f2a49 VLDR s4,|L1.640|
00015e eef31a0e VMOV.F32 s3,#30.00000000
000162 ed9f1a48 VLDR s2,|L1.644|
000166 eddf0a48 VLDR s1,|L1.648|
00016a f7fffffe BL mapping
00016e eebd0ac0 VCVT.S32.F32 s0,s0
000172 ed840a00 VSTR s0,[r4,#0]
000176 e011 B |L1.412|
|L1.376|
;;;318 }
;;;319 else if ((agvInAngle <= TURNINGOFF_ANGLE)||(turnAngleDiff*lastAngleDiff < 0))
000178 ee100a10 VMOV r0,s0
00017c 493e LDR r1,|L1.632|
00017e 43c9 MVNS r1,r1
000180 4288 CMP r0,r1
000182 dd0a BLE |L1.410|
000184 ed960a08 VLDR s0,[r6,#0x20]
000188 edd60a09 VLDR s1,[r6,#0x24]
00018c ee200a20 VMUL.F32 s0,s0,s1
000190 eeb50ac0 VCMPE.F32 s0,#0.0
000194 eef1fa10 VMRS APSR_nzcv,FPSCR
000198 d200 BCS |L1.412|
|L1.410|
;;;320 {
;;;321 TurnArriveFlag = 1;
00019a 7075 STRB r5,[r6,#1]
|L1.412|
;;;322 }
;;;323 lastAngleDiff = turnAngleDiff;
00019c ed960a08 VLDR s0,[r6,#0x20]
0001a0 ed860a09 VSTR s0,[r6,#0x24]
;;;324 //出弯
;;;325 if (TurnArriveFlag == 1)
0001a4 7870 LDRB r0,[r6,#1] ; TurnArriveFlag
0001a6 2801 CMP r0,#1
0001a8 d128 BNE |L1.508|
;;;326 {
;;;327 //出弯
;;;328 lastAngleDiff = 0;
0001aa ed9f0a38 VLDR s0,|L1.652|
0001ae ed860a09 VSTR s0,[r6,#0x24]
;;;329
;;;330 agv.Command.SetBaseSpeed = 0;
0001b2 6027 STR r7,[r4,#0] ; agv
;;;331
;;;332 if (fabs(KincoStruct1.encodeSpeed) < 5 && fabs(KincoStruct2.encodeSpeed) < 5 && Rotate1.runState == 0)
0001b4 f8d90004 LDR r0,[r9,#4] ; KincoStruct1
0001b8 f7fffffe BL __aeabi_f2d
0001bc ec410b10 VMOV d0,r0,r1
0001c0 f7fffffe BL __hardfp_fabs
0001c4 ec532b18 VMOV r2,r3,d8
0001c8 ec510b10 VMOV r0,r1,d0
0001cc f7fffffe BL __aeabi_cdcmple
0001d0 d211 BCS |L1.502|
0001d2 f8db0004 LDR r0,[r11,#4] ; KincoStruct2
0001d6 f7fffffe BL __aeabi_f2d
0001da ec410b10 VMOV d0,r0,r1
0001de f7fffffe BL __hardfp_fabs
0001e2 ec532b18 VMOV r2,r3,d8
0001e6 ec510b10 VMOV r0,r1,d0
0001ea f7fffffe BL __aeabi_cdcmple
0001ee d202 BCS |L1.502|
0001f0 4827 LDR r0,|L1.656|
0001f2 7d80 LDRB r0,[r0,#0x16] ; Rotate1
0001f4 b150 CBZ r0,|L1.524|
|L1.502|
;;;333 {
;;;334 if (agv.Public.SystemTime - lastTime > 500)
;;;335 {
;;;336 if (agv.Command.CurDirection == 3 || agv.Command.CurDirection == 4)
;;;337 {
;;;338 traffic_land_marks.land_marks[agv.Command.standSiteID - 1].Direction = 1;
;;;339
;;;340 TurnStep = 0;
;;;341 }
;;;342 // else if (agv.Command.CurDirection == 5 || agv.Command.CurDirection == 6)
;;;343 // {
;;;344 // traffic_land_marks.land_marks[agv.Command.standSiteID - 1].Direction = 2;
;;;345
;;;346 // TurnStep = 0;
;;;347 // }
;;;348 }
;;;349 }
;;;350 else
;;;351 lastTime = agv.Public.SystemTime;
0001f6 f8d400bc LDR r0,[r4,#0xbc] ; agv
0001fa 62b0 STR r0,[r6,#0x28] ; lastTime
|L1.508|
;;;352 }
;;;353 }
;;;354 setMotorSpeedSlope(1.5);
0001fc eeb70a08 VMOV.F32 s0,#1.50000000
000200 ecbd8b02 VPOP {d8}
000204 e8bd5ff0 POP {r4-r12,lr}
000208 f7ffbffe B.W setMotorSpeedSlope
|L1.524|
00020c f8d400bc LDR r0,[r4,#0xbc] ;334 ; agv
000210 6ab1 LDR r1,[r6,#0x28] ;334 ; lastTime
000212 1a40 SUBS r0,r0,r1 ;334
000214 f5b07ffa CMP r0,#0x1f4 ;334
000218 d9f0 BLS |L1.508|
00021a 7f20 LDRB r0,[r4,#0x1c] ;336 ; agv
00021c 2803 CMP r0,#3 ;336
00021e d001 BEQ |L1.548|
000220 2804 CMP r0,#4 ;336
000222 d1eb BNE |L1.508|
|L1.548|
000224 7da0 LDRB r0,[r4,#0x16] ;338 ; agv
000226 491b LDR r1,|L1.660|
000228 1e40 SUBS r0,r0,#1 ;338
00022a eb000040 ADD r0,r0,r0,LSL #1 ;338
00022e eb0100c0 ADD r0,r1,r0,LSL #3 ;338
000232 8345 STRH r5,[r0,#0x1a] ;338
000234 7037 STRB r7,[r6,#0] ;340
000236 e7e1 B |L1.508|
;;;355 }
;;;356
ENDP
|L1.568|
DCD ||.data||
|L1.572|
DCD navi
|L1.576|
DCD CurrentCenterPoint
|L1.580|
DCD KincoStruct1
|L1.584|
DCD KincoStruct2
|L1.588|
00024c 00000000 DCFD 0x4014000000000000 ; 5
000250 40140000
|L1.596|
DCD agv
|L1.600|
DCD TargetPoint
|L1.604|
DCD StartPoint
|L1.608|
000260 66666666 DCFD 0x404ca66666666666 ; 57.299999999999997
000264 404ca666
|L1.616|
DCD turnCount
|L1.620|
DCD 0x40c90fda
|L1.624|
000270 40c90fda DCFS 0x40c90fda ; 6.2831850051879883
|L1.628|
DCD 0x3e99999a
|L1.632|
DCD 0xc30a3d70
|L1.636|
DCD 0x01a3d70a
|L1.640|
000280 42c80000 DCFS 0x42c80000 ; 100
|L1.644|
000284 3e99999a DCFS 0x3e99999a ; 0.30000001192092896
|L1.648|
000288 3cf5c28f DCFS 0x3cf5c28f ; 0.029999999329447746
|L1.652|
00028c 00000000 DCFS 0x00000000 ; 0
|L1.656|
DCD Rotate1
|L1.660|
DCD traffic_land_marks
AREA ||i.HardDifferentialSpeedRunTurnning||, CODE, READONLY, ALIGN=2
HardDifferentialSpeedRunTurnning PROC
;;;249 //硬差速弧线转弯
;;;250 void HardDifferentialSpeedRunTurnning()
000000 b570 PUSH {r4-r6,lr}
;;;251 {
;;;252 if (fabs(navi.Public.AngleDifference) >= ADVANCE_STOPSLOPE_ANGLE) //如果agv角度差大于预停车角度差
;;;253 {
;;;254 agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed;
000002 4d20 LDR r5,|L2.132|
000004 4c20 LDR r4,|L2.136|
000006 6820 LDR r0,[r4,#0] ;252 ; navi
000008 f7fffffe BL __aeabi_f2d
00000c ec410b10 VMOV d0,r0,r1 ;252
000010 f7fffffe BL __hardfp_fabs
000014 ed9f1b1d VLDR d1,|L2.140|
000018 ec510b10 VMOV r0,r1,d0 ;252
00001c ec532b11 VMOV r2,r3,d1 ;252
000020 f7fffffe BL __aeabi_cdrcmple
000024 d808 BHI |L2.56|
000026 f9b50018 LDRSH r0,[r5,#0x18] ; agv
00002a 6028 STR r0,[r5,#0] ; agv
;;;255 setMotorSpeedSlope(0.5);
00002c e8bd4070 POP {r4-r6,lr}
000030 eeb60a00 VMOV.F32 s0,#0.50000000
000034 f7ffbffe B.W setMotorSpeedSlope
|L2.56|
;;;256 }
;;;257 else //如果agv角度差小于预停车角度差,开始减速至设定出弯速度
;;;258 {
;;;259 agv.Command.SetBaseSpeed = mapping(fabs(navi.Public.AngleDifference), 0, ADVANCE_STOPSLOPE_ANGLE, ARC_TURNSPEED_MIN, agv.Command.DispatchSpeed);
000038 6820 LDR r0,[r4,#0] ; navi
00003a f7fffffe BL __aeabi_f2d
00003e ec410b10 VMOV d0,r0,r1
000042 f7fffffe BL __hardfp_fabs
000046 ec510b10 VMOV r0,r1,d0
00004a f7fffffe BL __aeabi_d2f
00004e ee000a10 VMOV s0,r0
000052 f9b50018 LDRSH r0,[r5,#0x18] ; agv
000056 eddf1a0f VLDR s3,|L2.148|
00005a ee000a90 VMOV s1,r0
00005e ed9f1a0e VLDR s2,|L2.152|
000062 eeb82ae0 VCVT.F32.S32 s4,s1
000066 eddf0a0d VLDR s1,|L2.156|
00006a f7fffffe BL mapping
00006e eebd0ac0 VCVT.S32.F32 s0,s0
000072 ed850a00 VSTR s0,[r5,#0]
;;;260 setMotorSpeedSlope(0.2);
000076 e8bd4070 POP {r4-r6,lr}
00007a ed9f0a09 VLDR s0,|L2.160|
00007e f7ffbffe B.W setMotorSpeedSlope
;;;261 }
;;;262 }
;;;263
ENDP
000082 0000 DCW 0x0000
|L2.132|
DCD agv
|L2.136|
DCD navi
|L2.140|
00008c 33333333 DCFD 0x3fd3333333333333 ; 0.29999999999999999
000090 3fd33333
|L2.148|
000094 42c80000 DCFS 0x42c80000 ; 100
|L2.152|
000098 3e99999a DCFS 0x3e99999a ; 0.30000001192092896
|L2.156|
00009c 00000000 DCFS 0x00000000 ; 0
|L2.160|
0000a0 3e4ccccd DCFS 0x3e4ccccd ; 0.20000000298023224
AREA ||i.HardDifferentialSpeedStraight||, CODE, READONLY, ALIGN=2
HardDifferentialSpeedStraight PROC
;;;200 //硬差速直行
;;;201 void HardDifferentialSpeedStraight()
000000 b570 PUSH {r4-r6,lr}
;;;202 {
000002 ed2d8b04 VPUSH {d8-d9}
;;;203 float DIS = TwoPointDistance(StartPoint, TargetPoint);
000006 4881 LDR r0,|L3.524|
000008 edd02a02 VLDR s5,[r0,#8]
00000c ed902a01 VLDR s4,[r0,#4]
000010 edd01a00 VLDR s3,[r0,#0]
000014 487e LDR r0,|L3.528|
000016 ed901a02 VLDR s2,[r0,#8]
00001a edd00a01 VLDR s1,[r0,#4]
00001e ed900a00 VLDR s0,[r0,#0]
000022 f7fffffe BL TwoPointDistance
000026 eeb08a40 VMOV.F32 s16,s0
;;;204 if(DIS > 10000)
00002a ee180a10 VMOV r0,s16
00002e 4979 LDR r1,|L3.532|
;;;205 navi.Public.DistanceAgvToTARGET = DIS - fabs(navi.Public.DistanceAgvToSTART) - 400;
000030 4c79 LDR r4,|L3.536|
000032 4288 CMP r0,r1 ;204
000034 dd1c BLE |L3.112|
000036 68e0 LDR r0,[r4,#0xc] ; navi
000038 f7fffffe BL __aeabi_f2d
00003c ec410b10 VMOV d0,r0,r1
000040 f7fffffe BL __hardfp_fabs
000044 eeb09a40 VMOV.F32 s18,s0
000048 eef09a60 VMOV.F32 s19,s1
00004c ee180a10 VMOV r0,s16
000050 f7fffffe BL __aeabi_f2d
000054 ec532b19 VMOV r2,r3,d9
000058 f7fffffe BL __aeabi_dsub
00005c ed9f1b6f VLDR d1,|L3.540|
000060 ec532b11 VMOV r2,r3,d1
000064 f7fffffe BL __aeabi_dsub
000068 f7fffffe BL __aeabi_d2f
00006c 6120 STR r0,[r4,#0x10] ; navi
00006e e015 B |L3.156|
|L3.112|
;;;206 else
;;;207 navi.Public.DistanceAgvToTARGET = DIS - fabs(navi.Public.DistanceAgvToSTART);
000070 68e0 LDR r0,[r4,#0xc] ; navi
000072 f7fffffe BL __aeabi_f2d
000076 ec410b10 VMOV d0,r0,r1
00007a f7fffffe BL __hardfp_fabs
00007e eeb09a40 VMOV.F32 s18,s0
000082 eef09a60 VMOV.F32 s19,s1
000086 ee180a10 VMOV r0,s16
00008a f7fffffe BL __aeabi_f2d
00008e ec532b19 VMOV r2,r3,d9
000092 f7fffffe BL __aeabi_dsub
000096 f7fffffe BL __aeabi_d2f
00009a 6120 STR r0,[r4,#0x10] ; navi
|L3.156|
;;;208
;;;209 if (agv.Command.CurDirection == agv.Command.NextDirection) //如果当前方向和下次方向相同
00009c 4d61 LDR r5,|L3.548|
00009e 7f29 LDRB r1,[r5,#0x1c] ; agv
0000a0 7f68 LDRB r0,[r5,#0x1d] ; agv
0000a2 4281 CMP r1,r0
0000a4 d103 BNE |L3.174|
;;;210 {
;;;211 agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //设定速度为系统下发速度
0000a6 f9b50018 LDRSH r0,[r5,#0x18] ; agv
0000aa 6028 STR r0,[r5,#0] ; agv
0000ac e068 B |L3.384|
|L3.174|
;;;212 } //如果记下来是弧线转弯,做减速处理
;;;213 else if (agv.Command.NextDirection == 7 || agv.Command.NextDirection == 8 || agv.Command.NextDirection == 9 || agv.Command.NextDirection == 10)
0000ae 2807 CMP r0,#7
0000b0 d015 BEQ |L3.222|
0000b2 2808 CMP r0,#8
0000b4 d013 BEQ |L3.222|
0000b6 2809 CMP r0,#9
0000b8 d011 BEQ |L3.222|
0000ba 280a CMP r0,#0xa
0000bc d00f BEQ |L3.222|
;;;214 {
;;;215 if (navi.Public.DistanceAgvToTARGET >= agv.Parameter.AdvanceParkPos) //agv到终点距离大于预停车距离
;;;216 {
;;;217 agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //按系统下发速度走
;;;218 }
;;;219 else //agv到终点距离小于预停车距离,减速到设定自动转弯速度
;;;220 {
;;;221 agv.Command.SetBaseSpeed = mapping(navi.Public.DistanceAgvToTARGET, 0, agv.Parameter.AdvanceParkPos, agv.Command.NextDispatchSpeed, agv.Command.DispatchSpeed);
;;;222 }
;;;223 }
;;;224 else //停车或者原地转
;;;225 {
;;;226 if (navi.Public.DistanceAgvToTARGET >= agv.Parameter.AdvanceParkPos) //如果agv到目标点距离大于预停车距离
0000be 8f28 LDRH r0,[r5,#0x38] ; agv
0000c0 ed940a04 VLDR s0,[r4,#0x10]
0000c4 ee000a90 VMOV s1,r0
0000c8 eef80a60 VCVT.F32.U32 s1,s1
0000cc eeb40ae0 VCMPE.F32 s0,s1
0000d0 eef1fa10 VMRS APSR_nzcv,FPSCR
0000d4 db2c BLT |L3.304|
;;;227 {
;;;228 agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //设定速度等于系统下发速度
0000d6 f9b50018 LDRSH r0,[r5,#0x18] ; agv
0000da 6028 STR r0,[r5,#0] ; agv
0000dc e050 B |L3.384|
|L3.222|
0000de 8f28 LDRH r0,[r5,#0x38] ;215 ; agv
0000e0 ed940a04 VLDR s0,[r4,#0x10] ;215
0000e4 ee000a90 VMOV s1,r0 ;215
0000e8 eef80a60 VCVT.F32.U32 s1,s1 ;215
0000ec eeb40ae0 VCMPE.F32 s0,s1 ;215
0000f0 eef1fa10 VMRS APSR_nzcv,FPSCR ;215
0000f4 db03 BLT |L3.254|
0000f6 f9b50018 LDRSH r0,[r5,#0x18] ;217 ; agv
0000fa 6028 STR r0,[r5,#0] ;217 ; agv
0000fc e040 B |L3.384|
|L3.254|
0000fe f9b51018 LDRSH r1,[r5,#0x18] ;221 ; agv
000102 ee001a90 VMOV s1,r1 ;221
000106 f9b5101a LDRSH r1,[r5,#0x1a] ;221 ; agv
00010a eeb82ae0 VCVT.F32.S32 s4,s1 ;221
00010e ee001a90 VMOV s1,r1 ;221
000112 eef81ae0 VCVT.F32.S32 s3,s1 ;221
000116 ee000a90 VMOV s1,r0 ;221
00011a eeb81a60 VCVT.F32.U32 s2,s1 ;221
00011e eddf0a42 VLDR s1,|L3.552|
000122 f7fffffe BL mapping
000126 eebd0ac0 VCVT.S32.F32 s0,s0 ;221
00012a ed850a00 VSTR s0,[r5,#0] ;221
00012e e027 B |L3.384|
|L3.304|
;;;229 }
;;;230 else if (navi.Public.DistanceAgvToTARGET <= agv.Parameter.AdvanceParkPos && navi.Public.DistanceAgvToTARGET > LIGHT_STOP_SIGNAL_DIS ) //小于预停车距离,大于到点判断范围10MM,驱动器不能给太小速度,不能直接到0,设置了预停车速度
000130 ee000a90 VMOV s1,r0
000134 eef80a60 VCVT.F32.U32 s1,s1
000138 eeb40ae0 VCMPE.F32 s0,s1
00013c eef1fa10 VMRS APSR_nzcv,FPSCR
000140 d81c BHI |L3.380|
000142 ee101a10 VMOV r1,s0
000146 4a39 LDR r2,|L3.556|
000148 4291 CMP r1,r2
00014a dd17 BLE |L3.380|
;;;231 {
;;;232 agv.Command.SetBaseSpeed = mapping(navi.Public.DistanceAgvToTARGET, LIGHT_STOP_SIGNAL_DIS, agv.Parameter.AdvanceParkPos, agv.Parameter.AdvanceParkSpeed, agv.Command.DispatchSpeed);
00014c f9b51018 LDRSH r1,[r5,#0x18] ; agv
000150 ee001a90 VMOV s1,r1
000154 8ee9 LDRH r1,[r5,#0x36] ; agv
000156 eeb82ae0 VCVT.F32.S32 s4,s1
00015a ee001a90 VMOV s1,r1
00015e eef81a60 VCVT.F32.U32 s3,s1
000162 ee000a90 VMOV s1,r0
000166 eeb81a60 VCVT.F32.U32 s2,s1
00016a eef30a0e VMOV.F32 s1,#30.00000000
00016e f7fffffe BL mapping
000172 eebd0ac0 VCVT.S32.F32 s0,s0
000176 ed850a00 VSTR s0,[r5,#0]
00017a e001 B |L3.384|
|L3.380|
;;;233 }
;;;234 else
;;;235 agv.Command.SetBaseSpeed = agv.Parameter.AdvanceParkSpeed;
00017c 8ee8 LDRH r0,[r5,#0x36] ; agv
00017e 6028 STR r0,[r5,#0] ; agv
|L3.384|
;;;236 }
;;;237 if (navi.Public.DistanceAgvToSTART < START_SPEEDUP_RANGE)
000180 492b LDR r1,|L3.560|
;;;238 {
;;;239 if (fabs(KincoStruct1.encodeSpeed - KincoStruct2.encodeSpeed) > START_SPEEDDIFF_LIMIT) //速度差太大
000182 68e0 LDR r0,[r4,#0xc] ; navi
000184 4e2b LDR r6,|L3.564|
000186 4c2c LDR r4,|L3.568|
000188 4288 CMP r0,r1 ;237
00018a da37 BGE |L3.508|
00018c ed960a01 VLDR s0,[r6,#4]
000190 edd40a01 VLDR s1,[r4,#4]
000194 ee300a60 VSUB.F32 s0,s0,s1
000198 ee100a10 VMOV r0,s0
00019c f7fffffe BL __aeabi_f2d
0001a0 ec410b10 VMOV d0,r0,r1
0001a4 f7fffffe BL __hardfp_fabs
0001a8 ed9f1b24 VLDR d1,|L3.572|
0001ac ec510b10 VMOV r0,r1,d0
0001b0 ec532b11 VMOV r2,r3,d1
0001b4 f7fffffe BL __aeabi_cdrcmple
0001b8 d218 BCS |L3.492|
;;;240 {
;;;241 agv.Command.SetBaseSpeed = fabs(KincoStruct1.encodeSpeed + KincoStruct2.encodeSpeed) / 2;
0001ba ed960a01 VLDR s0,[r6,#4]
0001be edd40a01 VLDR s1,[r4,#4]
0001c2 ee300a20 VADD.F32 s0,s0,s1
0001c6 ee100a10 VMOV r0,s0
0001ca f7fffffe BL __aeabi_f2d
0001ce ec410b10 VMOV d0,r0,r1
0001d2 f7fffffe BL __hardfp_fabs
0001d6 ed9f1b1b VLDR d1,|L3.580|
0001da ec510b10 VMOV r0,r1,d0
0001de ec532b11 VMOV r2,r3,d1
0001e2 f7fffffe BL __aeabi_dmul
0001e6 f7fffffe BL __aeabi_d2iz
0001ea 6028 STR r0,[r5,#0] ; agv
|L3.492|
;;;242 }
;;;243 setMotorSpeedSlope(0.3);
0001ec ed9f0a17 VLDR s0,|L3.588|
0001f0 ecbd8b04 VPOP {d8-d9}
0001f4 e8bd4070 POP {r4-r6,lr}
0001f8 f7ffbffe B.W setMotorSpeedSlope
|L3.508|
;;;244 }
;;;245 else
;;;246 setMotorSpeedSlope(1.5);
0001fc ecbd8b04 VPOP {d8-d9}
000200 eeb70a08 VMOV.F32 s0,#1.50000000
000204 e8bd4070 POP {r4-r6,lr}
000208 f7ffbffe B.W setMotorSpeedSlope
;;;247 }
;;;248
ENDP
|L3.524|
DCD TargetPoint
|L3.528|
DCD StartPoint
|L3.532|
DCD 0x461c4000
|L3.536|
DCD navi
|L3.540|
00021c 00000000 DCFD 0x4079000000000000 ; 400
000220 40790000
|L3.548|
DCD agv
|L3.552|
000228 00000000 DCFS 0x00000000 ; 0
|L3.556|
DCD 0x41f00000
|L3.560|
DCD 0x43960000
|L3.564|
DCD KincoStruct1
|L3.568|
DCD KincoStruct2
|L3.572|
00023c 00000000 DCFD 0x4034000000000000 ; 20
000240 40340000
|L3.580|
000244 00000000 DCFD 0x3fe0000000000000 ; 0.5
000248 3fe00000
|L3.588|
00024c 3e99999a DCFS 0x3e99999a ; 0.30000001192092896
AREA ||i.chassisControlAuto||, CODE, READONLY, ALIGN=2
chassisControlAuto PROC
;;;100 float SpeedDifValue;//根据转弯半径大小,计算固定速度差值
;;;101 void chassisControlAuto(void)
000000 b530 PUSH {r4,r5,lr}
;;;102 {
;;;103 //根据转弯半径大小,计算固定速度差值
;;;104 SpeedDifValue = P_SETUP_WHEEL_Y_DIS * ((float)agv.Command.SetBaseSpeed) / (navi.Private.SetCalculationRadius) / 2;
000002 4978 LDR r1,|L4.484|
000004 6808 LDR r0,[r1,#0] ; agv
000006 ee000a10 VMOV s0,r0
00000a eeb80ac0 VCVT.F32.S32 s0,s0
00000e eddf0a76 VLDR s1,|L4.488|
000012 4a76 LDR r2,|L4.492|
000014 ee600a20 VMUL.F32 s1,s0,s1
000018 ed921a08 VLDR s2,[r2,#0x20]
00001c 4a74 LDR r2,|L4.496|
00001e ee800a81 VDIV.F32 s0,s1,s2
000022 eef60a00 VMOV.F32 s1,#0.50000000
000026 ee600a20 VMUL.F32 s1,s0,s1
00002a edc20a0b VSTR s1,[r2,#0x2c]
;;;105
;;;106 switch (agv.Command.CurDirection)
00002e 7f0c LDRB r4,[r1,#0x1c] ; agv
;;;107 {
;;;108 case 0:
;;;109 KincoStruct1.setSpeed = 0;
000030 ed9f1a70 VLDR s2,|L4.500|
000034 4d6d LDR r5,|L4.492|
;;;110
;;;111 KincoStruct2.setSpeed = 0;
;;;112 break;
;;;113
;;;114 case 1:
;;;115 if (agv.Command.SetBaseSpeed == 0)
;;;116 {
;;;117 KincoStruct1.setSpeed = 0;
;;;118
;;;119 KincoStruct2.setSpeed = 0;
;;;120 }
;;;121 else
;;;122 {
;;;123 KincoStruct1.setSpeed = agv.Command.SetBaseSpeed + navi.Private.OutputOffset; //设定速度加补偿值
;;;124
;;;125 KincoStruct2.setSpeed = agv.Command.SetBaseSpeed - navi.Private.OutputOffset; //设定速度减补偿值
;;;126 }
;;;127 break;
;;;128
;;;129 case 2:
;;;130 if (agv.Command.SetBaseSpeed == 0)
;;;131 {
;;;132 KincoStruct1.setSpeed = 0;
;;;133
;;;134 KincoStruct2.setSpeed = 0;
;;;135 }
;;;136 else
;;;137 {
;;;138 KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed - navi.Private.OutputOffset); //设定速度加补偿值,后退要给负速度
;;;139
;;;140 KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed + navi.Private.OutputOffset); //设定速度减补偿值,后退要给负速度
;;;141 }
;;;142 break;
;;;143
;;;144 case 3:
;;;145 KincoStruct1.setSpeed = -agv.Command.SetBaseSpeed; //前进原地左转,左轮给负速度,以当前行进方向分左右
000036 4243 RSBS r3,r0,#0
000038 496f LDR r1,|L4.504|
00003a 4a70 LDR r2,|L4.508|
00003c ed950a0a VLDR s0,[r5,#0x28] ;123
000040 2c0b CMP r4,#0xb ;106
000042 d270 BCS |L4.294|
000044 e8dff004 TBB [pc,r4] ;106
000048 060b223b DCB 0x06,0x0b,0x22,0x3b
00004c 48556270 DCB 0x48,0x55,0x62,0x70
000050 859ab100 DCB 0x85,0x9a,0xb1,0x00
000054 ed811a00 VSTR s2,[r1,#0] ;109
000058 ed821a00 VSTR s2,[r2,#0] ;111
;;;146
;;;147 KincoStruct2.setSpeed = agv.Command.SetBaseSpeed; //前进原地左转,右轮给正速度,以当前行进方向分左右
;;;148 break;
;;;149
;;;150 case 4:
;;;151 KincoStruct1.setSpeed = agv.Command.SetBaseSpeed; //前进原地右转,左轮给正速度,以当前行进方向分左右
;;;152
;;;153 KincoStruct2.setSpeed = -agv.Command.SetBaseSpeed; //前进原地右转,右轮给负速度,以当前行进方向分左右
;;;154 break;
;;;155
;;;156 case 5:
;;;157 KincoStruct1.setSpeed = agv.Command.SetBaseSpeed; //后退原地左转,左轮给正速度,以当前行进方向分左右
;;;158
;;;159 KincoStruct2.setSpeed = -agv.Command.SetBaseSpeed; //后退原地左转,右轮给负速度,以当前行进方向分左右
;;;160 break;
;;;161
;;;162 case 6:
;;;163 KincoStruct1.setSpeed = -agv.Command.SetBaseSpeed; //后退原地右转,左轮给负速度,以当前行进方向分左右
;;;164
;;;165 KincoStruct2.setSpeed = agv.Command.SetBaseSpeed; //后退原地右转,右轮给正速度,以当前行进方向分左右
;;;166 break;
;;;167
;;;168 case 7: //前进弧线左转,左轮设定速度-固定速度差-补偿值
;;;169 KincoStruct1.setSpeed = agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset;
;;;170 //前进弧线左转,右轮设定速度+固定速度差+补偿值
;;;171 KincoStruct2.setSpeed = agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset;
;;;172 break;
;;;173
;;;174 case 8: //前进弧线右转,左轮设定速度+固定速度差+补偿值
;;;175 KincoStruct1.setSpeed = agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset;
;;;176 //前进弧线右转,左轮设定速度-固定速度差-补偿值
;;;177 KincoStruct2.setSpeed = agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset;
;;;178 break;
;;;179
;;;180 case 9: //后退弧线左转,左轮设定速度+固定速度差-补偿值,给负速度
;;;181 KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset);
;;;182 //后退弧线左转,右轮设定速度-固定速度差+补偿值,给负速度
;;;183 KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset);
;;;184 break;
;;;185
;;;186 case 10: //后退弧线右转,左轮设定速度-固定速度差+补偿值,给负速度
;;;187 KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset);
;;;188 //后退弧线右转,左轮设定速度+固定速度差-补偿值,给负速度
;;;189 KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset);
;;;190 break;
;;;191
;;;192 default:
;;;193 KincoStruct1.setSpeed = 0;
;;;194
;;;195 KincoStruct2.setSpeed = 0;
;;;196 break;
;;;197 }
;;;198 }
00005c bd30 POP {r4,r5,pc}
00005e b180 CBZ r0,|L4.130|
000060 ee000a90 VMOV s1,r0 ;123
000064 eef80ae0 VCVT.F32.S32 s1,s1 ;123
000068 ee700a80 VADD.F32 s1,s1,s0 ;123
00006c edc10a00 VSTR s1,[r1,#0] ;123
000070 ee000a90 VMOV s1,r0 ;125
000074 eef80ae0 VCVT.F32.S32 s1,s1 ;125
000078 ee300ac0 VSUB.F32 s0,s1,s0 ;125
00007c ed820a00 VSTR s0,[r2,#0] ;125
000080 bd30 POP {r4,r5,pc}
|L4.130|
000082 ed811a00 VSTR s2,[r1,#0] ;117
000086 ed821a00 VSTR s2,[r2,#0] ;119
00008a bd30 POP {r4,r5,pc}
00008c b190 CBZ r0,|L4.180|
00008e ee000a90 VMOV s1,r0 ;138
000092 eef80ae0 VCVT.F32.S32 s1,s1 ;138
000096 ee700a60 VSUB.F32 s1,s0,s1 ;138
00009a edc10a00 VSTR s1,[r1,#0] ;138
00009e ee000a90 VMOV s1,r0 ;140
0000a2 eef80ae0 VCVT.F32.S32 s1,s1 ;140
0000a6 ee300a80 VADD.F32 s0,s1,s0 ;140
0000aa eeb10a40 VNEG.F32 s0,s0 ;140
0000ae ed820a00 VSTR s0,[r2,#0] ;140
0000b2 bd30 POP {r4,r5,pc}
|L4.180|
0000b4 ed811a00 VSTR s2,[r1,#0] ;132
0000b8 ed821a00 VSTR s2,[r2,#0] ;134
0000bc bd30 POP {r4,r5,pc}
0000be ee003a10 VMOV s0,r3 ;145
0000c2 eeb80ac0 VCVT.F32.S32 s0,s0 ;145
0000c6 ed810a00 VSTR s0,[r1,#0] ;145
0000ca ee000a10 VMOV s0,r0 ;147
0000ce eeb80ac0 VCVT.F32.S32 s0,s0 ;147
0000d2 ed820a00 VSTR s0,[r2,#0] ;147
0000d6 bd30 POP {r4,r5,pc}
0000d8 ee000a10 VMOV s0,r0 ;151
0000dc eeb80ac0 VCVT.F32.S32 s0,s0 ;151
0000e0 ed810a00 VSTR s0,[r1,#0] ;151
0000e4 ee003a10 VMOV s0,r3 ;153
0000e8 eeb80ac0 VCVT.F32.S32 s0,s0 ;153
0000ec ed820a00 VSTR s0,[r2,#0] ;153
0000f0 bd30 POP {r4,r5,pc}
0000f2 ee000a10 VMOV s0,r0 ;157
0000f6 eeb80ac0 VCVT.F32.S32 s0,s0 ;157
0000fa ed810a00 VSTR s0,[r1,#0] ;157
0000fe ee003a10 VMOV s0,r3 ;159
000102 eeb80ac0 VCVT.F32.S32 s0,s0 ;159
000106 ed820a00 VSTR s0,[r2,#0] ;159
00010a bd30 POP {r4,r5,pc}
00010c ee003a10 VMOV s0,r3 ;163
000110 eeb80ac0 VCVT.F32.S32 s0,s0 ;163
000114 ed810a00 VSTR s0,[r1,#0] ;163
000118 ee000a10 VMOV s0,r0 ;165
00011c eeb80ac0 VCVT.F32.S32 s0,s0 ;165
000120 ed820a00 VSTR s0,[r2,#0] ;165
|L4.292|
000124 bd30 POP {r4,r5,pc}
|L4.294|
000126 e057 B |L4.472|
000128 ee010a10 VMOV s2,r0 ;169
00012c eeb81ac1 VCVT.F32.S32 s2,s2 ;169
000130 ee311a60 VSUB.F32 s2,s2,s1 ;169
000134 ee311a40 VSUB.F32 s2,s2,s0 ;169
000138 ed811a00 VSTR s2,[r1,#0] ;169
00013c ee010a10 VMOV s2,r0 ;171
000140 eeb81ac1 VCVT.F32.S32 s2,s2 ;171
000144 ee710a20 VADD.F32 s1,s2,s1 ;171
000148 ee300a80 VADD.F32 s0,s1,s0 ;171
00014c ed820a00 VSTR s0,[r2,#0] ;171
000150 e7e8 B |L4.292|
000152 ee010a10 VMOV s2,r0 ;175
000156 eeb81ac1 VCVT.F32.S32 s2,s2 ;175
00015a ee311a20 VADD.F32 s2,s2,s1 ;175
00015e ee311a00 VADD.F32 s2,s2,s0 ;175
000162 ed811a00 VSTR s2,[r1,#0] ;175
000166 ee010a10 VMOV s2,r0 ;177
00016a eeb81ac1 VCVT.F32.S32 s2,s2 ;177
00016e ee710a60 VSUB.F32 s1,s2,s1 ;177
000172 ee300ac0 VSUB.F32 s0,s1,s0 ;177
000176 ed820a00 VSTR s0,[r2,#0] ;177
00017a e7d3 B |L4.292|
00017c ee010a10 VMOV s2,r0 ;181
000180 eeb81ac1 VCVT.F32.S32 s2,s2 ;181
000184 ee311a20 VADD.F32 s2,s2,s1 ;181
000188 ee311a00 VADD.F32 s2,s2,s0 ;181
00018c eeb11a41 VNEG.F32 s2,s2 ;181
000190 ed811a00 VSTR s2,[r1,#0] ;181
000194 ee010a10 VMOV s2,r0 ;183
000198 eeb81ac1 VCVT.F32.S32 s2,s2 ;183
00019c ee710a60 VSUB.F32 s1,s2,s1 ;183
0001a0 ee300a60 VSUB.F32 s0,s0,s1 ;183
0001a4 ed820a00 VSTR s0,[r2,#0] ;183
0001a8 e7bc B |L4.292|
0001aa ee010a10 VMOV s2,r0 ;187
0001ae eeb81ac1 VCVT.F32.S32 s2,s2 ;187
0001b2 ee311a60 VSUB.F32 s2,s2,s1 ;187
0001b6 ee301a41 VSUB.F32 s2,s0,s2 ;187
0001ba ed811a00 VSTR s2,[r1,#0] ;187
0001be ee010a10 VMOV s2,r0 ;189
0001c2 eeb81ac1 VCVT.F32.S32 s2,s2 ;189
0001c6 ee710a20 VADD.F32 s1,s2,s1 ;189
0001ca ee300a80 VADD.F32 s0,s1,s0 ;189
0001ce eeb10a40 VNEG.F32 s0,s0 ;189
0001d2 ed820a00 VSTR s0,[r2,#0] ;189
0001d6 e7a5 B |L4.292|
|L4.472|
0001d8 ed811a00 VSTR s2,[r1,#0] ;193
0001dc ed821a00 VSTR s2,[r2,#0] ;195
0001e0 e7a0 B |L4.292|
;;;199
ENDP
0001e2 0000 DCW 0x0000
|L4.484|
DCD agv
|L4.488|
0001e8 43b90000 DCFS 0x43b90000 ; 370
|L4.492|
DCD navi
|L4.496|
DCD ||.data||
|L4.500|
0001f4 00000000 DCFS 0x00000000 ; 0
|L4.504|
DCD KincoStruct1
|L4.508|
DCD KincoStruct2
AREA ||i.chassisControlManual||, CODE, READONLY, ALIGN=2
chassisControlManual PROC
;;;4 float arriveAngle = 0.015;
;;;5 void chassisControlManual(void)
000000 e92d41f0 PUSH {r4-r8,lr}
;;;6 {
000004 ed2d8b04 VPUSH {d8-d9}
;;;7 static int i = 0;
;;;8 static float lastAngle = 0,diffAngle = 0,alreadyTurnAngle = 0;
;;;9 static float setSpeed = 0,lastSetSpeed = 0;
;;;10 alreadyTurnAngle = CurrentCenterPoint.CurAngle - lastAngle;
000008 488f LDR r0,|L5.584|
00000a 4c90 LDR r4,|L5.588|
00000c ed909a02 VLDR s18,[r0,#8]
000010 ed940a02 VLDR s0,[r4,#8]
000014 ee390a40 VSUB.F32 s0,s18,s0
000018 ed840a04 VSTR s0,[r4,#0x10]
;;;11 if(alreadyTurnAngle > PI)
00001c ee100a10 VMOV r0,s0
000020 498b LDR r1,|L5.592|
;;;12 alreadyTurnAngle -= 2*PI;
000022 eddf0a8c VLDR s1,|L5.596|
000026 4288 CMP r0,r1 ;11
000028 dd03 BLE |L5.50|
00002a ee300a60 VSUB.F32 s0,s0,s1
00002e ed840a04 VSTR s0,[r4,#0x10]
|L5.50|
;;;13 if(alreadyTurnAngle < -PI)
000032 ed940a04 VLDR s0,[r4,#0x10]
000036 4988 LDR r1,|L5.600|
000038 ee100a10 VMOV r0,s0
00003c 4288 CMP r0,r1
00003e d903 BLS |L5.72|
;;;14 alreadyTurnAngle += 2*PI;
000040 ee300a20 VADD.F32 s0,s0,s1
000044 ed840a04 VSTR s0,[r4,#0x10]
|L5.72|
;;;15 diffAngle = fabs(fabs(alreadyTurnAngle) - agv.Command.SetChassisTurnAngle);//fabs(CurrentCenterPoint.CurAngle - lastAngle);
000048 4f84 LDR r7,|L5.604|
00004a 6a78 LDR r0,[r7,#0x24] ; agv
00004c f7fffffe BL __aeabi_f2d
000050 ec410b18 VMOV d8,r0,r1
000054 6920 LDR r0,[r4,#0x10] ; alreadyTurnAngle
000056 f7fffffe BL __aeabi_f2d
00005a ec410b10 VMOV d0,r0,r1
00005e f7fffffe BL __hardfp_fabs
000062 ec532b18 VMOV r2,r3,d8
000066 ec510b10 VMOV r0,r1,d0
00006a f7fffffe BL __aeabi_dsub
00006e ec410b10 VMOV d0,r0,r1
000072 f7fffffe BL __hardfp_fabs
000076 ec510b10 VMOV r0,r1,d0
00007a f7fffffe BL __aeabi_d2f
00007e ee000a10 VMOV s0,r0
000082 ed840a03 VSTR s0,[r4,#0xc]
;;;16
;;;17 switch (agv.Command.HandMotorState)
000086 f897c005 LDRB r12,[r7,#5] ; agv
00008a 2200 MOVS r2,#0
;;;18 {
;;;19 case 0x00:
;;;20 KincoStruct1.setSpeed = 0;
00008c ed9f1a74 VLDR s2,|L5.608|
000090 4d74 LDR r5,|L5.612|
;;;21 KincoStruct2.setSpeed = 0;
000092 4e75 LDR r6,|L5.616|
;;;22 lastAngle = CurrentCenterPoint.CurAngle;
;;;23 break;
;;;24 case 0x01: //前进
;;;25 KincoStruct1.setSpeed = agv.Parameter.HandSpeed;
;;;26 KincoStruct2.setSpeed = agv.Parameter.HandSpeed;
;;;27
;;;28 break;
;;;29 case 0x02: //后退
;;;30 KincoStruct1.setSpeed = -agv.Parameter.HandSpeed;
;;;31 KincoStruct2.setSpeed = -agv.Parameter.HandSpeed;
;;;32
;;;33
;;;34 break;
;;;35 break;
;;;36 case 0x05: //前动力左转90
;;;37
;;;38 if(diffAngle >=0.5)
000094 f04f537c MOV r3,#0x3f000000
;;;39 {
;;;40 setSpeed = agv.Parameter.HandTurnSpeed;
;;;41 }
;;;42 else if(diffAngle >arriveAngle)
;;;43 {
;;;44 setSpeed = mapping(diffAngle,arriveAngle,0.5,30,agv.Parameter.HandTurnSpeed);
;;;45 }
;;;46 else
;;;47 {
;;;48 agv.Command.HandMotorState = 0;
;;;49 setSpeed = 0;
;;;50 }
;;;51
;;;52 if(setSpeed > lastSetSpeed)
;;;53 {
;;;54 setSpeed = lastSetSpeed + 0.3;
000098 ed9f8b74 VLDR d8,|L5.620|
00009c 4639 MOV r1,r7 ;15
00009e f8df81ac LDR r8,|L5.588|
0000a2 f8b10042 LDRH r0,[r1,#0x42] ;40
0000a6 8f89 LDRH r1,[r1,#0x3c] ;25
0000a8 edd80a01 VLDR s1,[r8,#4] ;42
0000ac f1bc0f07 CMP r12,#7 ;17
0000b0 d20b BCS |L5.202|
0000b2 e8dff00c TBB [pc,r12] ;17
0000b6 040b DCB 0x04,0x0b
0000b8 18b9b926 DCB 0x18,0xb9,0xb9,0x26
0000bc 7000 DCB 0x70,0x00
0000be ed851a00 VSTR s2,[r5,#0] ;20
0000c2 ed861a00 VSTR s2,[r6,#0] ;21
0000c6 ed849a02 VSTR s18,[r4,#8] ;22
|L5.202|
0000ca e0ad B |L5.552|
0000cc ee001a10 VMOV s0,r1 ;25
0000d0 eeb80a40 VCVT.F32.U32 s0,s0 ;25
0000d4 ed850a00 VSTR s0,[r5,#0] ;25
0000d8 ee001a10 VMOV s0,r1 ;26
0000dc eeb80a40 VCVT.F32.U32 s0,s0 ;26
0000e0 ed860a00 VSTR s0,[r6,#0] ;26
0000e4 e0a0 B |L5.552|
0000e6 4248 RSBS r0,r1,#0 ;30
0000e8 ee000a10 VMOV s0,r0 ;30
0000ec eeb80ac0 VCVT.F32.S32 s0,s0 ;30
0000f0 ed850a00 VSTR s0,[r5,#0] ;30
0000f4 ee000a10 VMOV s0,r0 ;31
0000f8 eeb80ac0 VCVT.F32.S32 s0,s0 ;31
0000fc ed860a00 VSTR s0,[r6,#0] ;31
000100 e092 B |L5.552|
000102 ee101a10 VMOV r1,s0 ;38
000106 4299 CMP r1,r3 ;38
000108 db06 BLT |L5.280|
00010a ee000a10 VMOV s0,r0 ;40
00010e eeb80a40 VCVT.F32.U32 s0,s0 ;40
000112 ed840a05 VSTR s0,[r4,#0x14] ;40
000116 e014 B |L5.322|
|L5.280|
000118 eeb40ae0 VCMPE.F32 s0,s1 ;42
00011c eef1fa10 VMRS APSR_nzcv,FPSCR ;42
000120 dd0c BLE |L5.316|
000122 ee010a10 VMOV s2,r0 ;44
000126 eef31a0e VMOV.F32 s3,#30.00000000 ;44
00012a eeb82a41 VCVT.F32.U32 s4,s2 ;44
00012e eeb61a00 VMOV.F32 s2,#0.50000000 ;44
000132 f7fffffe BL mapping
000136 ed840a05 VSTR s0,[r4,#0x14] ;44
00013a e002 B |L5.322|
|L5.316|
00013c 717a STRB r2,[r7,#5] ;48
00013e ed841a05 VSTR s2,[r4,#0x14] ;49
|L5.322|
000142 edd40a05 VLDR s1,[r4,#0x14] ;52
000146 ed940a06 VLDR s0,[r4,#0x18] ;52
00014a eef40ac0 VCMPE.F32 s1,s0 ;52
00014e eef1fa10 VMRS APSR_nzcv,FPSCR ;52
000152 dd0b BLE |L5.364|
000154 ee100a10 VMOV r0,s0
000158 f7fffffe BL __aeabi_f2d
00015c ec532b18 VMOV r2,r3,d8
000160 f7fffffe BL __aeabi_dadd
000164 f7fffffe BL __aeabi_d2f
000168 6160 STR r0,[r4,#0x14] ; setSpeed
00016a e00b B |L5.388|
|L5.364|
;;;55 }
;;;56 else if(setSpeed < lastSetSpeed)
00016c d20a BCS |L5.388|
;;;57 {
;;;58 setSpeed = lastSetSpeed - 0.3;
00016e ee100a10 VMOV r0,s0
000172 f7fffffe BL __aeabi_f2d
000176 ec532b18 VMOV r2,r3,d8
00017a f7fffffe BL __aeabi_dsub
00017e f7fffffe BL __aeabi_d2f
000182 6160 STR r0,[r4,#0x14] ; setSpeed
|L5.388|
;;;59 }
;;;60 KincoStruct1.setSpeed = -setSpeed;
000184 ed940a05 VLDR s0,[r4,#0x14]
000188 eef10a40 VNEG.F32 s1,s0
00018c edc50a00 VSTR s1,[r5,#0]
;;;61 KincoStruct2.setSpeed = setSpeed;
000190 ed860a00 VSTR s0,[r6,#0]
;;;62
;;;63 break;
000194 e048 B |L5.552|
;;;64 case 0x06: //前动力右转90
;;;65 if(diffAngle >=0.5)
000196 ee101a10 VMOV r1,s0
00019a 4299 CMP r1,r3
00019c db06 BLT |L5.428|
;;;66 {
;;;67 setSpeed = agv.Parameter.HandTurnSpeed;
00019e ee000a10 VMOV s0,r0
0001a2 eeb80a40 VCVT.F32.U32 s0,s0
0001a6 ed840a05 VSTR s0,[r4,#0x14]
0001aa e014 B |L5.470|
|L5.428|
;;;68 }
;;;69 else if(diffAngle >arriveAngle)
0001ac eeb40ae0 VCMPE.F32 s0,s1
0001b0 eef1fa10 VMRS APSR_nzcv,FPSCR
0001b4 dd0c BLE |L5.464|
;;;70 {
;;;71 setSpeed = mapping(diffAngle,arriveAngle,0.5,30,agv.Parameter.HandTurnSpeed);
0001b6 ee010a10 VMOV s2,r0
0001ba eef31a0e VMOV.F32 s3,#30.00000000
0001be eeb82a41 VCVT.F32.U32 s4,s2
0001c2 eeb61a00 VMOV.F32 s2,#0.50000000
0001c6 f7fffffe BL mapping
0001ca ed840a05 VSTR s0,[r4,#0x14]
0001ce e002 B |L5.470|
|L5.464|
;;;72 }
;;;73 else
;;;74 {
;;;75 agv.Command.HandMotorState = 0;
0001d0 717a STRB r2,[r7,#5]
;;;76 setSpeed = 0;
0001d2 ed841a05 VSTR s2,[r4,#0x14]
|L5.470|
;;;77 }
;;;78 if(setSpeed > lastSetSpeed)
0001d6 edd40a05 VLDR s1,[r4,#0x14]
0001da ed940a06 VLDR s0,[r4,#0x18]
0001de eef40ac0 VCMPE.F32 s1,s0
0001e2 eef1fa10 VMRS APSR_nzcv,FPSCR
0001e6 dd0b BLE |L5.512|
;;;79 {
;;;80 setSpeed = lastSetSpeed + 0.3;
0001e8 ee100a10 VMOV r0,s0
0001ec f7fffffe BL __aeabi_f2d
0001f0 ec532b18 VMOV r2,r3,d8
0001f4 f7fffffe BL __aeabi_dadd
0001f8 f7fffffe BL __aeabi_d2f
0001fc 6160 STR r0,[r4,#0x14] ; setSpeed
0001fe e00b B |L5.536|
|L5.512|
;;;81 }
;;;82 else if(setSpeed < lastSetSpeed)
000200 d20a BCS |L5.536|
;;;83 {
;;;84 setSpeed = lastSetSpeed - 0.3;
000202 ee100a10 VMOV r0,s0
000206 f7fffffe BL __aeabi_f2d
00020a ec532b18 VMOV r2,r3,d8
00020e f7fffffe BL __aeabi_dsub
000212 f7fffffe BL __aeabi_d2f
000216 6160 STR r0,[r4,#0x14] ; setSpeed
|L5.536|
;;;85 }
;;;86 KincoStruct1.setSpeed = setSpeed;
000218 ed940a05 VLDR s0,[r4,#0x14]
00021c ed850a00 VSTR s0,[r5,#0]
;;;87 KincoStruct2.setSpeed = -setSpeed;
000220 eeb10a40 VNEG.F32 s0,s0
000224 ed860a00 VSTR s0,[r6,#0]
|L5.552|
;;;88
;;;89 break;
;;;90
;;;91 default:
;;;92 break;
;;;93 }
;;;94 lastSetSpeed = setSpeed;
000228 ed940a05 VLDR s0,[r4,#0x14]
00022c ed840a06 VSTR s0,[r4,#0x18]
;;;95 setMotorSpeedSlope(agv.Parameter.HandSpeedSlope);
000230 8ff8 LDRH r0,[r7,#0x3e] ; agv
000232 ecbd8b04 VPOP {d8-d9}
000236 ee000a10 VMOV s0,r0
00023a e8bd41f0 POP {r4-r8,lr}
00023e eeb80a40 VCVT.F32.U32 s0,s0
000242 f7ffbffe B.W setMotorSpeedSlope
;;;96
;;;97 }
;;;98
ENDP
000246 0000 DCW 0x0000
|L5.584|
DCD CurrentCenterPoint
|L5.588|
DCD ||.data||
|L5.592|
DCD 0x40490fda
|L5.596|
000254 40c90fda DCFS 0x40c90fda ; 6.2831850051879883
|L5.600|
DCD 0xc0490fda
|L5.604|
DCD agv
|L5.608|
000260 00000000 DCFS 0x00000000 ; 0
|L5.612|
DCD KincoStruct1
|L5.616|
DCD KincoStruct2
|L5.620|
00026c 33333333 DCFD 0x3fd3333333333333 ; 0.29999999999999999
000270 3fd33333
AREA ||i.chassisGetAutoSpeed||, CODE, READONLY, ALIGN=1
chassisGetAutoSpeed PROC
;;;357 //无轨硬差速模型运动控制
;;;358 void chassisGetAutoSpeed(u8 *CurrentDir)
000000 7800 LDRB r0,[r0,#0]
;;;359 {
;;;360 if (*CurrentDir == 1 || *CurrentDir == 2) //直行
000002 2801 CMP r0,#1
000004 d013 BEQ |L6.46|
000006 2802 CMP r0,#2
000008 d011 BEQ |L6.46|
;;;361 {
;;;362 HardDifferentialSpeedStraight(); //硬差速直行
;;;363 }
;;;364 else if (*CurrentDir == 3 || *CurrentDir == 4 || *CurrentDir == 5 || *CurrentDir == 6) //原地旋转
00000a 2803 CMP r0,#3
00000c d011 BEQ |L6.50|
00000e 2804 CMP r0,#4
000010 d00f BEQ |L6.50|
000012 2805 CMP r0,#5
000014 d00d BEQ |L6.50|
000016 2806 CMP r0,#6
000018 d00b BEQ |L6.50|
;;;365 {
;;;366 HardDifferentialSpeedPirouette(); //硬差速原地旋转
;;;367 }
;;;368 else if (*CurrentDir == 7 || *CurrentDir == 8 || *CurrentDir == 9 || *CurrentDir == 10) //弧线转弯
00001a 2807 CMP r0,#7
00001c d005 BEQ |L6.42|
00001e 2808 CMP r0,#8
000020 d003 BEQ |L6.42|
000022 2809 CMP r0,#9
000024 d001 BEQ |L6.42|
000026 280a CMP r0,#0xa
000028 d105 BNE |L6.54|
|L6.42|
;;;369 {
;;;370 HardDifferentialSpeedRunTurnning(); //弧线转弯
00002a f7ffbffe B.W HardDifferentialSpeedRunTurnning
|L6.46|
00002e f7ffbffe B.W HardDifferentialSpeedStraight
|L6.50|
000032 f7ffbffe B.W HardDifferentialSpeedPirouette
|L6.54|
;;;371 }
;;;372 }
000036 4770 BX lr
;;;373 #endif
ENDP
AREA ||.data||, DATA, ALIGN=2
TurnStep
000000 00 DCB 0x00
TurnArriveFlag
000001 000000 DCB 0x00,0x00,0x00
arriveAngle
000004 3c75c28f DCFS 0x3c75c28f ; 0.014999999664723873
lastAngle
000008 00000000 DCFS 0x00000000 ; 0
diffAngle
00000c 00000000 DCFS 0x00000000 ; 0
alreadyTurnAngle
000010 00000000 DCFS 0x00000000 ; 0
setSpeed
000014 00000000 DCFS 0x00000000 ; 0
lastSetSpeed
000018 00000000 DCFS 0x00000000 ; 0
agvInAngle
00001c 00000000 DCFS 0x00000000 ; 0
turnAngleDiff
000020 00000000 DCFS 0x00000000 ; 0
lastAngleDiff
000024 00000000 DCFS 0x00000000 ; 0
lastTime
DCD 0x00000000
SpeedDifValue
DCD 0x00000000
;*** Start embedded assembler ***
#line 1 "..\\..\\User\\CHASSIS\\HardDifferential.c"
AREA ||.rev16_text||, CODE
THUMB
EXPORT |__asm___18_HardDifferential_c_c9731b9a____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___18_HardDifferential_c_c9731b9a____REV16| PROC
#line 130
rev16 r0, r0
bx lr
ENDP
AREA ||.revsh_text||, CODE
THUMB
EXPORT |__asm___18_HardDifferential_c_c9731b9a____REVSH|
#line 144
|__asm___18_HardDifferential_c_c9731b9a____REVSH| PROC
#line 145
revsh r0, r0
bx lr
ENDP
AREA ||.rrx_text||, CODE
THUMB
EXPORT |__asm___18_HardDifferential_c_c9731b9a____RRX|
#line 300
|__asm___18_HardDifferential_c_c9731b9a____RRX| PROC
#line 301
rrx r0, r0
bx lr
ENDP
;*** End embedded assembler ***