-
Notifications
You must be signed in to change notification settings - Fork 0
/
MotorControl.c
3134 lines (2808 loc) · 121 KB
/
MotorControl.c
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
/*
* MotorControl.c
*
* Created on: Jun 21, 2017
* Author: ye
*/
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include<math.h>
#include "MotorControl.h"
#include "ExchangingInfoStruct.h"
#include"CANOpenShellMasterOD.h"
#include "CanOpenShell.h" //引用 extern CO_Data* CANOpenShellOD_Data;
#include"Log.h"
#include "InitAndSelfTest.h"//获取举升电机初始位置
/*声明结构体数组 agvreportstruct innerstruct， 定义在InitAndSelfTest.c中*/
extern struct StructInner innerstruct;
extern struct StructAgvReport agvreportstruct;
int delaytimecount=20000000;
/*转向纠偏角度*/
INTEGER32 Clockwis_Degree1=0x0000238E;
INTEGER32 Clockwis_Degree2=0x0000471C;
INTEGER32 Clockwis_Degree3=0x00006AAB;
INTEGER32 Clockwis_Degree4=0x00008E39;
INTEGER32 Clockwis_Degree5=0x0000B1C7;
INTEGER32 AntiClockwis_Degree1=0xFFFFDC72;
INTEGER32 AntiClockwis_Degree2=0xFFFFB8E4;
INTEGER32 AntiClockwis_Degree3=0xFFFF9555;
INTEGER32 AntiClockwis_Degree4=0xFFFF71C7;
INTEGER32 AntiClockwis_Degree5=0xFFFF4E39;
/*纠偏标志位*/
int AdjustFlag=0;
/*行走时用于计算距离的轮子周长*/
float CRight=0.6473389;
float CForward=0.6473389;
float CLeft=0.6473389;
float CBack=0.6473389;
/*前后轮交替纠偏计数器*/
int AdjustCount=0; //每次while循环++，并在while外面=0
int AdjustBackFlag=30; //除以AdjustBackFlag 不能整除前轮纠偏，能整除后轮纠偏,并在while外面=0
/*充电标志位*/
int ChargeFlag=0;//用于K1 K2互斥，作为K1闭合时的标志位
/*纠偏持续时间*/
int Adjust=100000000;//100000000大概1.2s左右；300000000=4.5s左右
/*位置模式电机，启动后 与 开始while检测是否到位之间需要一个0,5S左右延迟*/
int position_delay=50000000; //100000000大概1.2s左右；300000000=4.5s左右
/******行走电机参数 速度模式******/
/* 速度
* 面对电机轴，暂规定：顺时针=前进=正值；逆时针=后退=负值
* 如果电机默认相反，只需调整 顺时针=前进=负值 逆时针=后退=正值
* 直行正常速度=轮速1 m/s = 转速 5215189 counts/s
* 直行慢速=正常速度 *1/4= 轮速0.25 m/s = 转速 1303797 counts/s */
/*1m/s正常速度*/
//INTEGER32 Clockwise_WalkNomalSpeed=0x004F93D5; //1910rpm
//INTEGER32 Clockwise_WalkSlowSpeed =0x0013E4F5; //477rpm
//
//INTEGER32 Anticlockwise_WalkNomalSpeed=0xFFB06C2B;//-1910rpm
//INTEGER32 Anticlockwise_WalkSlowSpeed =0XFFEC1B0B; //-477rpm
/*1.2 m/s正常速度*/
//INTEGER32 Clockwise_WalkNomalSpeed=0x005F7E33; //2291.83118rpm
//INTEGER32 Clockwise_WalkSlowSpeed =0x0017DF8D; //577.9577rpm
//
//INTEGER32 Anticlockwise_WalkNomalSpeed=0xFFA081CD;
//INTEGER32 Anticlockwise_WalkSlowSpeed =0xFFE82073;
INTEGER32 Clockwise_WalkNomalSpeed=0x005F7E33; //2291.83118rpm
INTEGER32 Clockwise_WalkSlowSpeed =0x005F7E33; //577.9577rpm
INTEGER32 Anticlockwise_WalkNomalSpeed=0xFFA081CD;
INTEGER32 Anticlockwise_WalkSlowSpeed =0xFFA081CD;
/*1.36 m/s==2600rpm正常速度*/
//INTEGER32 Clockwise_WalkNomalSpeed=0x006C5555; //2600rpm
//INTEGER32 Clockwise_WalkSlowSpeed =0x001B1555; //650rpm
//
//INTEGER32 Anticlockwise_WalkNomalSpeed=0xFF93AAAB;
//INTEGER32 Anticlockwise_WalkSlowSpeed =0XFFE4EAAB;
/*寻找RFID的速度，应小与0.1788m/s(以0.4减速度，4cm正好到0)*/
//float LookingRFIDSpeed=0.1788; //单位m/s，轮子的速度
float LookingRFIDSpeed=0.1; //单位m/s，轮子的速度
INTEGER32 LookingRFIDSpeedCount=0x1; //将LookingRFIDSpeed转换为电机指令值
/*行走速度模式，启动/停止*/
INTEGER16 WalkStart = 0x000F;
INTEGER16 WalkStop = 0x010F;
INTEGER16 WalkStopDisable = 0x0107;//行走速度模式断使能
/********************行走电机参数 位置模式********************/
/*行走电机距离参数*/
INTEGER32 WalkingPositive=0x009b4040;
INTEGER32 WalkingNegative=0xff64bfc0;
/*自旋90度位置*/
//INTEGER32 RotatePositionPositive=0x00119952;
//INTEGER32 RotatePositionNegative=0xFFEE66AE;
/*自旋1度位置*/
INTEGER32 RotatePositionPositive=0x1;
INTEGER32 RotatePositionNegative=0x1;
/*距离停止点距离*/
INTEGER32 CommandPosition1=0x1;
INTEGER32 CommandPosition2=0x1;
INTEGER32 CommandPosition3=0x1;
INTEGER32 CommandPosition4=0x1;
int positivetime=1;
/*寻找RFID最小距离理论值 1m/s==1.25m 1.2m/s==1.8m 1.3m/s==2.1125m
* LookingRFIDPoint应大于理论值，单位米m*/
float LookingRFIDPoint=1.4; //2m开始发送指令
/*寻找RFID最小距离 转换count*/
INTEGER32 LookingRFIDPointCount=0x1;
/*行走电机开始寻找RFID为了匀速运行，将停止点向前挪一个相对距离，十进制单位米m
* 此距离用于计算 LookingRFIDWalkingPositiveCount 和 LookingRFIDWalkingNegativeCount*/
float LookingRFIDForwardStopPoint=2;
float LookingRFIDCrabStopPoint=2;
/* LookingRFIDForwardStopPoint或者LookingRFIDCrabStopPoint
* 转换count*/
INTEGER32 LookingRFIDWalkingPositiveCount=0x1;
INTEGER32 LookingRFIDWalkingNegativeCount=0x1;
/*读到RFID，修正停止点为当前位置前FixedFordwardStopPoint距离，相对位置
* 0.04=4厘米，此值固定，tag标签从边缘到中心=4cm*/
float FixedFordwardStopPoint=0.04;
float FixedCrabStopPoint=0.04;
/*FixedFordwardStopPoint或者FixedCrabStopPoint
* 转换count*/
INTEGER32 WalkingPositiveFixed=0x009b4040;
INTEGER32 WalkingNegativeFixed=0xff64bfc0;
/*LookingRFIDCount用于限制寻找RFID函数只进入一次，初始=0，进入后=1，agvstop中置0*/
int LookingRFIDCount=0;
/*行走过程中蔽障标帜位，用于避免每个while都发送bit4=0->1，每次运动前，将标帜位置0; AvoidFlag=0:初始状态，AvoidFlag=1蔽障未触发，=2蔽障外层触发 =3蔽障内层触发*/
int AvoidFlag=0;
/*启动/停止*/
INTEGER16 WalkStartPosition = 0x003F; //绝对
INTEGER16 WalkStartPositionReletive = 0x007F; //相对
INTEGER16 WalkStopPosition = 0x016F;
INTEGER16 WalkStopPositionDisable=0x0167; //行走位置模式断始能
INTEGER16 WalkBitfourZero=0x006F;
/******转向电机参数 位置模式******/
/* 方向控制--通过Target Position 607A 位置数值的正负HEX；
* 面对电机轴俯视，暂规定：电机顺时针旋转=位置数值为正; 逆时针旋转=位置数值为负
* 如果电机默认相反，只需调整 使电机顺时针旋转时，位置数值=正 即可
*/
/*轮子与长边平行时，作为0位置*/
INTEGER32 Rotator_zero=0x00000000;
/* 横移位置=90°
* 轮子旋转90°电机需要旋转90*25=2250°=6.25圈 * 4096= 25600 counts=0x64 00
* 顺时针= 0x6400
* 逆时针= 0xFFFF9C00
* */
INTEGER32 Clockwise_Rotator_Crab= 0X000C8000;
INTEGER32 Anticlockwise_Rotator_Crab=0XFFF38000;
/* 自旋位置=65.22485°
*轮子旋转arctan(1300/600)°电机需要旋转arctan(1300/600)*25=1630.621486°/360 =4.529504圈*4096=18552.8489=18553 counts=0X4879 (换算回角度=65.22539°)
*顺时针=0x4879;
*逆时针=0xFFFFB787
* */
//INTEGER32 Clockwise_Rotator_Self=0X00090F1B;
//INTEGER32 Anticlockwise_Rotator_Self=0XFFF6F0E5;
INTEGER32 Clockwise_Rotator_Self=0X00090F1B ;
INTEGER32 Anticlockwise_Rotator_Self=0XFFF6F0E5;
/*启动停止*/
INTEGER16 RotatorStart = 0x003F;
INTEGER16 RotatorDelay = 0x002F; //需测试：此处延迟的1S 是否会对接受信息造成影响
INTEGER16 RotatorStop = 0x012F; //不断使能
INTEGER16 RotatorStopDisable = 0x012F; //断使能
/*限位开关*/
// UNS8 limitswitch=0x0; //把OD限位开关的变量复制过来，防止读取+写入同时进行!
int limitswitch_rotator5_up=0;//旋转电机I-上限位开关
int limitswitch_rotator5_down=0;//旋转电机I-下限位开关
int limitswitch_rotator6_up=0;//旋转电机II-上限位开关
int limitswitch_rotator6_down=0;//旋转电机II-下限位开关
int limitswitch_rotator7_up=0;//旋转电机III-上限位开关
int limitswitch_rotator7_down=0;//旋转电机III-下限位开关
int limitswitch_rotator8_up=0;//旋转电机VI-上限位开关
int limitswitch_rotator8_down=0;//旋转电机VI-下限位开关
/******举升电机参数 位置模式******/
/* 方向控制--通过Target Position 607A 位置数值的正负HEX；
* 面对电机轴俯视，暂规定：电机逆时针旋转（位置数值为正）：螺杆举升；顺时针旋转（位置数值为负）：螺杆下降
* 如果电机默认相反，只需调整 使电机顺时针旋转时，位置信息为正即可
*
* 举升电机位于底部时，位置=0
* 需举升40mm距离，用时 4.2S（不包含加减速过程）*/
/*位置：电机需要转182圈 = 2981888 counts = 002D8000*/
INTEGER32 LiftZeroPosition =0x00000000;
INTEGER32 LiftTargetPosition=0x002D8000;
/*启动停止*/
INTEGER16 LiftStart = 0x003F;
INTEGER16 LiftDelay = 0x002F;//需测试：此处延迟的1S 是否会对接受信息造成影响
//INTEGER16 LiftStop = 0x012F; //不断使能
INTEGER16 LiftStop = 0x0127; //bit 3 enable=0 //断使能
/******继电器输出******/
/* K1充电 第0位=0x01;
* K2控制电机 第1位=0x02;
* K4=绿色灯 第2位=0x04;
* K5=黄色灯 第3位=0x08;
* K6=红色灯 第4位=0x10;
* 将0位置1：OUT8_Relay_bit1_8 |= 0x01
* 将0位清0：OUT8_Relay_bit1_8 &= ~0x01*/
UNS8 K1=0x01;
UNS8 K2=0x02;
UNS8 K4=0x04;
UNS8 K5=0x08;
UNS8 K6=0x10;
UNS8 K9=0x40;
////闭合继电器 输出1
//#define RelayClose(a) OUT8_Relay_bit1_8 |=a
//
////断开继电器 输出0
//#define RelayOpen(b) OUT8_Relay_bit1_8 &=~b
/*计算距离*/
//float ForwardSlowPointDistance=3.5;//假定减速点距离车位开始位置为7.18米
//float ForwardStopPointDistance=7.24;//假定减速点距离车位开始位置为7.18米
//
//float BackSlowPointDistance=3.5;//假定减速点距离车位开始位置为7.18米
//float BackStopPointDistance=7.24;//假定减速点距离车位开始位置为9.18米
//
//float LeftSlowPointDistance=1.1;//假定减速点距离车位开始位置为7.18米
//float LeftStopPointDistance=3.00;//假定减速点距离车位开始位置为7.18米
//
//float RightSlowPointDistance=1.1;//假定减速点距离车位开始位置为7.18米
//float RightStopPointDistance=2.90;//假定减速点距离车位开始位置为7.18米
INTEGER64 encodeMAX=2147483647;//编码器是INTEGER32格式，理论上可以表示-2147483648----2147483647之间的所有整数，现在假定当转数达到正最大时，再加1会变成负最大，同样，
//当转数为负最大时，再减小会变成正最大。
INTEGER32 encodeOldR1,encodeOldR2,encodeOldR3,encodeOldR4;//用来计算行走电机行走距离时，存储编码器旧值
float meterDistance=0;//转化成米数时的行走距离，
INTEGER64 distance=0;//存储编码器数值的累计行走距离，此距离值用整数表示，设成64位整数值，可以表示2的63次方的整数值
int correctNaviError=0;//当导航读到超过0xfc00但又小于0xffff的数值时，无法判断究竟是导航错误还是本来应该读0xffff的地方少读了
//一位数据，因此用这个标志位来辅助，如果这个标志位很多次被触发，说明导航出了错误，如果只是被触发几次，说明是该读全1的
//地方漏读了某几位，暂时以一次运行中被触发50次来做标准，实际值要根据测试实例更改。
//记录直行电机的初始速度值，因为直行时有两种速度，一种是正常速度，一种是爬行速度
INTEGER32 motor1=0;
INTEGER32 motor2=0;
INTEGER32 motor3=0;
INTEGER32 motor4=0;
//使用行走方向的后面两个rfid读卡器值的变化作为判断小车到达指定位置并停止的依据，因为rfid不清空，当小车启动时，默认四个rfid读卡器都能读到标签，
//当行进方向的两个前轮读到新的rfid标签时，其实它们两个读到的是应该后两轮的停止标签，所以这个变化不能用，此时后两轮的读卡器中存储的依然是起始位
//的标签，等到后两轮读到的标签更新后，就认为小车到达了下一个车位，应该停止运动，如果要严格判断的话，此时还可以判断前两轮读到的标签的最后1位加上
//后两轮读到标签的最后1位的和是不是10,
char rfid[4];
int norfidstop=0;//如果检测不到rfid,需要有一个停止条件让循环终止,这就是那个停止条件的flag,因为程序刚刚启动时,那个停止条件可能也满足,所以用这个
//变量来延迟一下停止条件的运行时间.
/*执行命令函数，客户端收到的上位机命令Command[]作为参数传进来，在此处按位解析，并分别调用相应的程序执行
* Command[]有固定格式，以AA开头，以EE结束，
* AA后面是控制字，分别是："BB"(控制agv行走)，"CC"(控制agv举升和下降)，"DD"(控制agv充电)，"WR"(控制agv轮子的旋转)，"RR"(控制小车的自旋）
* BB(1,2,3,4)(0,1):12346分别代表向前、向后、左移、右移,0,1代表爬行速度、正常速度
* CC(0,1)：0,1分别代表下降、举升
* DD(0,1):0,1分别代表去充电、停止充电
* WR(0,1,2):012分别代表回零位，横移位，自旋位
* RR(0,1)(x):01分别代表顺时针转、逆时针转，x为'0','1','2','3',分别代表转动90、180、270、360
*/
int ExecuteCommand(char* Command)
{
if(Command[2]=='B'&&Command[3]=='B') //控制agv的运动，1：向前行走; 2：向后行走; 3：左移; 4:右移;
{
if(Command[4]=='1')//电机向前运动
{
float slowpoint=string2float(Command);
char rfidTarget=Command[15];
int ret=AgvFoward(Command[5],slowpoint,rfidTarget);
printf("前进返回的值是%d\n",ret);
return ret;
}
else if(Command[4]=='2')//向后运动
{
float slowpoint=string2float(Command);
char rfidTarget=Command[15];
int ret=AgvBack(Command[5],slowpoint,rfidTarget);
printf("后退返回的值是%d\n",ret);
return ret;
}
else if(Command[4]=='3')//左移
{
float slowpoint=string2float(Command);
char rfidTarget=Command[15];
return AgvLeft(Command[5],slowpoint,rfidTarget);
}
else if(Command[4]=='4')//右移
{
float slowpoint=string2float(Command);
char rfidTarget=Command[15];
return AgvRight(Command[5],slowpoint,rfidTarget);
}
else//命令出现错误
{
Log("agv运动命令出错，下发命令不是前、后、左、右、停");
return -1;
}
return 0;//如果agv执行此条命令成功，返回0;实际上这条命令永远也不会执行，因为前面所有情况都包含return.
}
else if(Command[2]=='C'&&Command[3]=='C') //控制agv举升和下降
{
return AgvUpDown(Command[4]);
}
else if(Command[2]=='D'&&Command[3]=='D') //控制agv充电
{
return AgvCharge('c');
}
else if(Command[2]=='W'&&Command[3]=='R') //控制agv轮子的旋转,0:轮子回零位; 1:横移位; 2:自旋位;
{
if(Command[4]=='0')//轮子回零位
{
return RotateWheel2Zero();
}
else if(Command[4]=='1')//横移位
{
return RotateWheel2LeftRight();
}
else if(Command[4]=='2')//自旋位
{
return RotateWheel2SelfRotating();
}
else//命令出现错误
{
Log("agv轮子旋转命令出错，不是规定的3个位置");
return -1;
}
}
else if(Command[2]=='R'&&Command[3]=='R')//agv小车旋转
{
return RotateAgv(Command[4],Command[5]);
}
else//命令出现错误
{
Log("上位机下发命令出错，不是运动、举升卸载、充电、轮子旋转、小车旋转");
return -1;
}
return 0;//命令执行完成，返回0;此return不会执行。
}
/*！！！！！！！！！！！！！！！！错了！！！！！！！！！！！！！*/
//零位时，接近限位开关！0位不能在转盘的中间位置！ 因为转盘最大旋转100度，若在中间位置则两边转角只有50度，无法满足90度要求！！！！
/*小车四个轮子有三个位置，一是零位，即与小车长边平行，此时轮子可以向顶位转动，去和宽边平行，也可以向自旋位转动，到位后小车可以自旋，
* 二是顶位，即与小车宽边平行，此时轮子接近90°限位开关，只能向零位或自旋位转动。三是自旋位，此时轮子接近另一个限位开关，只能向零位转动。
* 轮子的三个位置要断电保存，所以每当轮子转换位置成功后要将其最新位置写入文件，设零位标志为0,顶位标志为1,自旋位为2，agv上电时轮子调到零位，
* 上位机在发送直行、自旋等命令时，根据轮子的当前位置和行进方向确定是否需要调整轮子的位置。
*/
int RotateWheel2Zero()//轮子回零位
{
//innerstruct.StopCommand=0;//把停止命令清空
/*将BIT4=0 避免纠偏未把bit4置0*/
// M2_TPDO_control_word=Bit4Zero(M2_TPDO_control_word); //bit 4=0 A &= ~0x10
M2_TPDO_control_word=(M2_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
//第一次发送 现将position改为不是0，再发送0位置，避免0位置PDO与主站上线自动发送的0信息重复放弃发送
M2_5_TPDO_Target_Position=0x00000001;
M2_6_TPDO_Target_Position=0x00000001;
M2_7_TPDO_Target_Position=0x00000001;
M2_8_TPDO_Target_Position=0x00000001;
sendPDOevent(CANOpenShellOD_Data);
//向四个电机发送回零位命令
M2_5_TPDO_Target_Position=Rotator_zero;
M2_6_TPDO_Target_Position=Rotator_zero;
M2_7_TPDO_Target_Position=Rotator_zero;
M2_8_TPDO_Target_Position=Rotator_zero;
sendPDOevent(CANOpenShellOD_Data);
/*启动*/
M2_TPDO_control_word = RotatorStart;
sendPDOevent(CANOpenShellOD_Data);
int delaycount=0;
while(delaycount<=position_delay) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
// sleep(1);//此处等待1s，旋转电机旋转1s后程序才会继续走，而紧急停止和限位开关停止命令在while循环中;若旋转电机旋转方向反了，则无法立刻停下!!!!
// M2_TPDO_control_word=RotatorDelay;//修改bit4=0
// sendPDOevent(CANOpenShellOD_Data);
while(agvreportstruct.MotorError==0) //使用agvreportstruct.MotorError 判断EMCY是否触发，若触发agvreportstruct.MotorError=1，停止while并返回-1
{
//问题：直接使用IN5_Limit_Switch_bit1_8用于判断，判断时，是否可被写入？？是否冲突？？！
//旋转电机I
limitswitch_rotator5_up =(IN5_Limit_Switch_bit1_8 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator5_up=1；若bit0=0，则limitswitch_rotator5_up=0
limitswitch_rotator5_down =(IN5_Limit_Switch_bit1_8 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator5_down=1；若bit1=0，则limitswitch_rotator5_down=0
//旋转电机II
limitswitch_rotator6_up =(IN5_Limit_Switch_bit1_8 & (1<<2)) ? 1 : 0; //判断bit2=1，则limitswitch_rotator6_up=1；若bit2=0，则limitswitch_rotator6_up=0
limitswitch_rotator6_down =(IN5_Limit_Switch_bit1_8 & (1<<3)) ? 1 : 0; //判断bit3=1，则limitswitch_rotator6_down=1；若bit3=0，则limitswitch_rotator6_down=0
//旋转电机III
limitswitch_rotator7_up =(IN5_Limit_Switch_bit1_8 & (1<<6)) ? 1 : 0; //判断bit6=1，则limitswitch_rotator7_up=1；若bit6=0，则limitswitch_rotator7_up=0
limitswitch_rotator7_down =(IN5_Limit_Switch_bit1_8 & (1<<7)) ? 1 : 0; //判断bit7=1，则limitswitch_rotator7_down=1；若bit7=0，则limitswitch_rotator7_down=0
//旋转电机IV --IN5_Limit_Switch_bit9_10
limitswitch_rotator8_up =(IN5_Limit_Switch_bit9_10 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator8_up=1；若bit0=0，则limitswitch_rotator8_up=0
limitswitch_rotator8_down =(IN5_Limit_Switch_bit9_10 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator8_down=1；若bit1=0，则limitswitch_rotator8_down=0
/*判断电机是否到达目标位置*/
if(((R5_statusword & (1<<(10)))!=0) &&
((R6_statusword & (1<<(10)))!=0) &&
((R7_statusword & (1<<(10)))!=0) &&
((R8_statusword & (1<<(10)))!=0) )//如果四个旋转电机都已转到零位且停止转动
{
agvreportstruct.WheelDirection=0;//告诉上位机当前位置=0
Log("旋转电机到达0位置");
printf("旋转电机到达0位置\n");
M2_TPDO_control_word=RotatorStop;//修改bit4=0
sendPDOevent(CANOpenShellOD_Data);
return 0;//命令执行成功，返回正常值
}
/*判断限位开关*/
if(limitswitch_rotator5_up==1||limitswitch_rotator5_down==1 ||
limitswitch_rotator6_up==1||limitswitch_rotator6_down==1 ||
limitswitch_rotator7_up==1||limitswitch_rotator7_down==1 ||
limitswitch_rotator8_up==1||limitswitch_rotator8_down==1 )//取出5-8位的LimitSwitch数据，判断是否大于0
{
AgvEmcyStop();//停止所有电机，红灯亮
Log("轮子转向零位过程中限位开关被触发");
printf("轮子转向零位过程中限位开关被触发\n");
return -1; //遇到故障，返回-1，表明命令并位正常执行
}
if(innerstruct.StopCommand==1)//如果收到停止命令，正常情况下轮子转动过程中不能停止，但遇到特殊情况强行停止也可以，只是要记录到日志里
{
Log("轮子转向零位过程中收到了停止命令");
printf("轮子转向零位过程中收到了停止命令\n");
return AgvStop();//上位机下达的停止命令正常执行，所以返回AgvStop=0。
}
}
printf("轮子转向零位过程中收到EMCY\n");
Log("轮子转向零位过程中收到EMCY");
return -1;
}
int RotateWheel2LeftRight()//轮子转到横移位
{
//int motorcode[4]={1,2,3,4};//根据经验值确定四个旋转电机转动90°时编码器要变化的数值
//int initposition[4]={R5_position,R6_position,R7_position,R8_position};//获得四个旋转电机当前的编码器的值，
//如果电机足够精确，向宽边转动时，四个轮子肯定在零位，此时四个电机的编码器其实是固定值，要变化的值也是固定值，那么上面两个变量都不需要，只要将motorcode的内容写到命令中去就可以。
// innerstruct.StopCommand=0;//把停止命令清空
/*将BIT4=0 避免纠偏未把bit4置0*/
// M2_TPDO_control_word=Bit4Zero(M2_TPDO_control_word); //bit 4=0
M2_TPDO_control_word=(M2_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
//向四个电机发送转到左移右移位命令,此命令为位置命令，命令中包含initposition+motorcode
/*1.转向电机：横移位置
*旋转电机1 3=逆时针=0XFF FF 9C 00 (noidID 5 7)
*旋转电机2 4=顺时针=0X64 00 (noidID 6 8)*/
M2_5_TPDO_Target_Position=Anticlockwise_Rotator_Crab;
M2_7_TPDO_Target_Position=Anticlockwise_Rotator_Crab;
M2_6_TPDO_Target_Position=Clockwise_Rotator_Crab;
M2_8_TPDO_Target_Position=Clockwise_Rotator_Crab;
sendPDOevent(CANOpenShellOD_Data);
/*启动*/
M2_TPDO_control_word = RotatorStart;
sendPDOevent(CANOpenShellOD_Data);
int delaycount=0;
while(delaycount<=position_delay) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
while(agvreportstruct.MotorError==0)
{
//问题：直接使用IN5_Limit_Switch_bit1_8用于判断，判断时，是否可被写入？？是否冲突？？！
//旋转电机I
limitswitch_rotator5_up =(IN5_Limit_Switch_bit1_8 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator5_up=1；若bit0=0，则limitswitch_rotator5_up=0
limitswitch_rotator5_down =(IN5_Limit_Switch_bit1_8 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator5_down=1；若bit1=0，则limitswitch_rotator5_down=0
//旋转电机II
limitswitch_rotator6_up =(IN5_Limit_Switch_bit1_8 & (1<<2)) ? 1 : 0; //判断bit2=1，则limitswitch_rotator6_up=1；若bit2=0，则limitswitch_rotator6_up=0
limitswitch_rotator6_down =(IN5_Limit_Switch_bit1_8 & (1<<3)) ? 1 : 0; //判断bit3=1，则limitswitch_rotator6_down=1；若bit3=0，则limitswitch_rotator6_down=0
//旋转电机III
limitswitch_rotator7_up =(IN5_Limit_Switch_bit1_8 & (1<<6)) ? 1 : 0; //判断bit6=1，则limitswitch_rotator7_up=1；若bit6=0，则limitswitch_rotator7_up=0
limitswitch_rotator7_down =(IN5_Limit_Switch_bit1_8 & (1<<7)) ? 1 : 0; //判断bit7=1，则limitswitch_rotator7_down=1；若bit7=0，则limitswitch_rotator7_down=0
//旋转电机IV --IN5_Limit_Switch_bit9_10
limitswitch_rotator8_up =(IN5_Limit_Switch_bit9_10 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator8_up=1；若bit0=0，则limitswitch_rotator8_up=0
limitswitch_rotator8_down =(IN5_Limit_Switch_bit9_10 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator8_down=1；若bit1=0，则limitswitch_rotator8_down=0
/*判断旋转电机是否到达目标位置*/
if(((R5_statusword & (1<<(10)))!=0) &&
((R6_statusword & (1<<(10)))!=0) &&
((R7_statusword & (1<<(10)))!=0) &&
((R8_statusword & (1<<(10)))!=0) )//如果四个旋转电机都已转到与宽边平行位且停止转动
{
agvreportstruct.WheelDirection=1;//告诉上位机当前位置=1
Log("旋转电机到达横移位置=1");
printf("旋转电机到达横移位置=1\n");
M2_TPDO_control_word=RotatorStop;//修改bit4=0
sendPDOevent(CANOpenShellOD_Data);
return 0;//命令执行成功，返回正常值
}
/*判断限位开关*/
if(limitswitch_rotator5_up==1||limitswitch_rotator5_down==1 ||
limitswitch_rotator6_up==1||limitswitch_rotator6_down==1 ||
limitswitch_rotator7_up==1||limitswitch_rotator7_down==1 ||
limitswitch_rotator8_up==1||limitswitch_rotator8_down==1)//旋转轮子 碰触限位开关，停止电机！发生错误！
{
AgvEmcyStop();//停止所有电机，红灯亮
Log("轮子转向横移位过程中限位开关被触发");
printf("轮子转向横移位过程中限位开关被触发\n");
return -1;
}
else if(innerstruct.StopCommand==1)//如果收到停止命令，正常情况下轮子转动过程中不能停止，但遇到特殊情况强行停止也可以，只是要记录到日志里
{
Log("轮子转向横移位过程中收到了停止命令");
printf("轮子转向横移位过程中收到了停止命令\n");
return AgvStop();
}
}
Log("轮子转向横移位过程中收到了EMCY");
printf("轮子转向横移位过程中收到了EMCY\n");
return -1;
}
int RotateWheel2SelfRotating()//轮子转向自旋位
{
//int motorcode[4]={1,2,3,4};//根据经验值确定四个旋转电机转到自旋方向时编码器要变化的数值
//int initposition[4]={R5_position,R6_position,R7_position,R8_position};//获得四个旋转电机当前的编码器的值，
//如果电机足够精确，向宽边转动时，四个轮子肯定在零位，此时四个电机的编码器其实是固定值，要变化的值也是固定值，那么上面两个变量都不需要，只要将motorcode的内容写到命令中去就可以。
// innerstruct.StopCommand=0;
/*将BIT4=0 避免纠偏未把bit4置0*/
// M2_TPDO_control_word=Bit4Zero(M2_TPDO_control_word); //bit 4=0
M2_TPDO_control_word=(M2_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
//向四个电机发送转到自旋位命令，此命令为位置命令，命令中包含initposition+motorcode
/*1.转向电机：自旋位置
*旋转电机1 3=逆时针=0xFFFFB787 (noidID 5 7)
*旋转电机2 4=顺时针=0x4879 (noidID 6 8)*/
M2_5_TPDO_Target_Position=Anticlockwise_Rotator_Self;
M2_7_TPDO_Target_Position=Anticlockwise_Rotator_Self;
M2_6_TPDO_Target_Position=Clockwise_Rotator_Self;
M2_8_TPDO_Target_Position=Clockwise_Rotator_Self;
sendPDOevent(CANOpenShellOD_Data);
/*启动*/
M2_TPDO_control_word = RotatorStart;
sendPDOevent(CANOpenShellOD_Data);
int delaycount=0;
while(delaycount<=position_delay) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
while(agvreportstruct.MotorError==0)
{
//问题：直接使用IN5_Limit_Switch_bit1_8用于判断，判断时，是否可被写入？？是否冲突？？！
//旋转电机I
limitswitch_rotator5_up =(IN5_Limit_Switch_bit1_8 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator5_up=1；若bit0=0，则limitswitch_rotator5_up=0
limitswitch_rotator5_down =(IN5_Limit_Switch_bit1_8 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator5_down=1；若bit1=0，则limitswitch_rotator5_down=0
//旋转电机II
limitswitch_rotator6_up =(IN5_Limit_Switch_bit1_8 & (1<<2)) ? 1 : 0; //判断bit2=1，则limitswitch_rotator6_up=1；若bit2=0，则limitswitch_rotator6_up=0
limitswitch_rotator6_down =(IN5_Limit_Switch_bit1_8 & (1<<3)) ? 1 : 0; //判断bit3=1，则limitswitch_rotator6_down=1；若bit3=0，则limitswitch_rotator6_down=0
//旋转电机III
limitswitch_rotator7_up =(IN5_Limit_Switch_bit1_8 & (1<<6)) ? 1 : 0; //判断bit6=1，则limitswitch_rotator7_up=1；若bit6=0，则limitswitch_rotator7_up=0
limitswitch_rotator7_down =(IN5_Limit_Switch_bit1_8 & (1<<7)) ? 1 : 0; //判断bit7=1，则limitswitch_rotator7_down=1；若bit7=0，则limitswitch_rotator7_down=0
//旋转电机IV --IN5_Limit_Switch_bit9_10
limitswitch_rotator8_up =(IN5_Limit_Switch_bit9_10 & (1<<0)) ? 1 : 0; //判断bit0=1，则limitswitch_rotator8_up=1；若bit0=0，则limitswitch_rotator8_up=0
limitswitch_rotator8_down =(IN5_Limit_Switch_bit9_10 & (1<<1)) ? 1 : 0; //判断bit1=1，则limitswitch_rotator8_down=1；若bit1=0，则limitswitch_rotator8_down=0
/*判断旋转电机是否到达目标位置*/
if(((R5_statusword & (1<<(10)))!=0) &&
((R6_statusword & (1<<(10)))!=0) &&
((R7_statusword & (1<<(10)))!=0) &&
((R8_statusword & (1<<(10)))!=0) )//如果四个旋转电机都已转到与宽边平行位且停止转动
{
agvreportstruct.WheelDirection=2;//告诉上位机当前位置=2
Log("旋转电机到达自旋位置=2");
printf("旋转电机到达自旋位置=2\n");
M2_TPDO_control_word=RotatorStop;//修改bit4=0
sendPDOevent(CANOpenShellOD_Data);
return 0;//命令执行成功，返回正常值
}
/*判断限位开关*/
else if(limitswitch_rotator5_up==1||limitswitch_rotator5_down==1 ||
limitswitch_rotator6_up==1||limitswitch_rotator6_down==1 ||
limitswitch_rotator7_up==1||limitswitch_rotator7_down==1 ||
limitswitch_rotator8_up==1||limitswitch_rotator8_down==1)//旋转轮子 碰触限位开关，停止电机！发生错误！
{
AgvEmcyStop();//停止所有电机，红灯亮
Log("轮子转向自旋过程中限位开关被触发");
printf("轮子转向自旋过程中限位开关被触发\n");
return -1;
}
else if(innerstruct.StopCommand==1)//如果收到停止命令，正常情况下轮子转动过程中不能停止，但遇到特殊情况强行停止也可以，只是要记录到日志里
{
Log("轮子转向自旋过程中收到了停止命令");
printf("轮子转向自旋过程中收到了停止命令\n");
return AgvStop();
}
}
Log("轮子转向自旋过程中收到了EMCY");
printf("轮子转向自旋过程中收到了EMCY\n");
return -1;
}
/*agv自旋程序，上位机向小车发送旋转方向和旋转角度，此程序被调用之前，上位机必须已经向rotatewheel发送调整至自旋状态的命令，并成功执行。
* 此程序运行时需要实时考虑防撞传感器是否触发，将导航传感器信号和rfid信号作为旋转是否到位的判定条件
*/
int RotateAgv(char direc,char angle)//direction '1'代表逆时针，'0'代表顺时针; angle '0' 代表90°，'1'代表180°，‘2’代表270°,'3'代表360°。
{
/*将bit4置0*/
M1_TPDO_control_word=(M1_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
int rfidcountflag=0;
int time=(int)angle-47;//判断要旋转几个90°，因为int '0'=48,('0'的ascii码是48),所以减去47后就代表转一个90°，
printf("time is %d\n",time);
innerstruct.StopCommand=0;//把电机停止命令清空
int log_count=0; //碰到障碍物等待时，向LOG中写入信息的时间间隔
int wait_timeout=0; //等待碰到障碍物等待时，超时时间
/*轮子到自旋位置*/
if(RotateWheel2SelfRotating()!=0)
{
printf("自旋前转向电机到自旋位置失败\n");
Log("自旋前转向电机到自旋位置失败");
return -1;
}
else
{
printf("自旋前转向电机到自旋位置完成\n");
Log("自旋前转向电机到自旋位置完成");
}
//计算旋转距离，减少while时间
if(SetRotatePosition(time)!=0)
{
printf("计算旋转距离失败\n");
Log("计算旋转距离失败");
return -1;
}
else
{
printf("计算旋转距离完成\n");
Log("计算旋转距离完成");
}
if(agvreportstruct.RFIDSenser[0][3]=='0'||agvreportstruct.RFIDSenser[1][3]=='0'||agvreportstruct.RFIDSenser[2][3]=='0'||agvreportstruct.RFIDSenser[3][3]=='0')//如果自旋前所有rfid读卡器都收不到信号，说明agv此时没在车位上
{
Log("agv检测不到rfid初始位，无法自旋");
printf("agv检测不到rfid初始位，无法自旋\n");
//return -1; return EmcyStop();
}
INTEGER32 r1position=R1_position;
//printf("r1position is %d\n",r1position);
/*臂章没有触发*/
if(agvreportstruct.HitAvoidSenser[0]==0 && agvreportstruct.HitAvoidSenser[1]==0 &&agvreportstruct.HitAvoidSenser[2]==0 && agvreportstruct.HitAvoidSenser[3]==0)//防撞信号没有触发
{
//黄灯K5灭 绿灯K4亮，K5=open K4=close
RelayClose(K4);
RelayOpen(K5);
RelayOpen(K6);
/**************************************************************************/
/*重设速度为正常值*/
M1_TPDO_Profile_Velocity=Clockwise_WalkNomalSpeed;
printf("重设行走电机位置模式速度==Clockwise_WalkNomalSpeed\n");
/**************************************************************************/
//向四个直行电机发送按照爬行速度，以速度*direction的方式旋转的命令，此命令在while循环内部，如果防撞信号始终没有触发，则此
//命令由于pdo特性不会发出，如果触发了防撞信号，则电机会停下，防撞信号消失后，这个电机运行命令会执行，保证电机能重新启动。
if(direc=='1')//1=逆时针旋转
{
//AGV旋转时，以Slow慢速旋转
/* AGV逆时针旋转
* 行走电机1 4 =前进 1=正数 4=负数；
* 行走电机2 3 =后退 2=正数 3=负数
* 左侧轮后退；右侧轮前进*/
M1_1_TPDO_Target_Position =RotatePositionPositive;
M1_2_TPDO_Target_Position =RotatePositionPositive ;
M1_3_TPDO_Target_Position =RotatePositionNegative;
M1_4_TPDO_Target_Position =RotatePositionNegative;
sendPDOevent(CANOpenShellOD_Data);
/*启动*/
M1_TPDO_control_word = WalkStartPositionReletive;
sendPDOevent(CANOpenShellOD_Data);
}
else if(direc=='0')//0=顺时针旋转
{
//AGV旋转时，以Slow慢速旋转
/* AGV顺时针旋转：
* 行走电机1 4 =后退 1=负数 4=正数；
* 行走电机2 3=前进 2=负数 3=正数
* (左侧轮前进；右侧轮后退)*/
M1_1_TPDO_Target_Position =RotatePositionNegative;
M1_2_TPDO_Target_Position =RotatePositionNegative ;
M1_3_TPDO_Target_Position =RotatePositionPositive;
M1_4_TPDO_Target_Position =RotatePositionPositive;
sendPDOevent(CANOpenShellOD_Data);
/*启动*/
M1_TPDO_control_word = WalkStartPositionReletive;
sendPDOevent(CANOpenShellOD_Data);
}
}//end 无防撞信号判断
else//开始转动前，蔽障被触发，报错并结束自旋
{
Log("自旋动作有障碍物");
printf("自旋动作有障碍物\n");
return -1;
}
int delaycount=0;
while(delaycount<=100000000) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
while(agvreportstruct.MotorError==0)
{
// /*将bit4置0*/
// M1_TPDO_control_word=(M1_TPDO_control_word&(~0x10));//bit 4=0
// sendPDOevent(CANOpenShellOD_Data);
if(innerstruct.StopCommand==1)//如果收到停止命令，正常情况下轮子转动过程中不能停止，但遇到特殊情况强行停止也可以，只是要记录到日志里
{
Log("agv转向过程中收到了停止命令");
printf("agv转向过程中收到了停止命令\n");
return AgvStop();
}
/****判断行走电机是否运行到位！！！*****/
if(((R1_statusword & (1<<(10)))!=0) ||
((R2_statusword & (1<<(10)))!=0) ||
((R3_statusword & (1<<(10)))!=0) ||
((R4_statusword & (1<<(10)))!=0) )//如果任意一个行走电机到位，则立刻停止所有行走电机
{
Log("自旋动作完成");
printf("自旋动作完成\n");
AgvStop();
return 0;
}
//蔽障没有触发
if(agvreportstruct.HitAvoidSenser[0]==0 && agvreportstruct.HitAvoidSenser[1]==0 &&agvreportstruct.HitAvoidSenser[2]==0 && agvreportstruct.HitAvoidSenser[3]==0)//防撞信号没有触发
{
/*将bit4置0*/
M1_TPDO_control_word=(M1_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
//黄灯K5灭 绿灯K4亮，K5=open K4=close
RelayClose(K4);
RelayOpen(K5);
RelayOpen(K6);
//此处应该重设正常速度，防止蔽障将速度设置为0，需要加入标帜位，防止每次都发送启动命令
}//end 无防撞信号判断
//蔽障触发
else if(agvreportstruct.HitAvoidSenser[0]==0x02 || agvreportstruct.HitAvoidSenser[1]==0x02 ||
agvreportstruct.HitAvoidSenser[2]==0x02 || agvreportstruct.HitAvoidSenser[3]==0x02 ||
agvreportstruct.HitAvoidSenser[0]==0x03 || agvreportstruct.HitAvoidSenser[1]==0x03 ||
agvreportstruct.HitAvoidSenser[2]==0x03 || agvreportstruct.HitAvoidSenser[3]==0x03)//防撞信号1区被触发(旋转时不再考虑1区2区，有障碍就停下)，while程序并不停止，当防撞信号消失后，进入前一个判断条件
{
/*将bit4置0*/
M1_TPDO_control_word=(M1_TPDO_control_word&(~0x10));//bit 4=0
sendPDOevent(CANOpenShellOD_Data);
//黄灯K5亮 绿灯K4灭，K5=close K4=open
RelayClose(K5);
RelayOpen(K4);
RelayOpen(K6);
//向电机发送停止命令,while不退出，等待障碍物消失，继续运行
//此处应该将速度设置为0！让停下，需要加入标帜位，防止每次都发送启动命令
log_count++;
if (log_count==1) //首次触发以及每间隔几秒后，打印信息
{
Log("自旋时遇到障碍物，等待障碍物消失");
printf("自旋时遇到障碍物，等待障碍物消失\n");
wait_timeout++;//等待时间++，用于超时退出
}
if (log_count==10000000){log_count=0;}//大概几秒清零
if (wait_timeout == 100) //大概几分钟
{
AgvEmcyStop();//停止所有电机，红灯亮
Log("自旋时遇到障碍物，等待超时");
printf("自旋时遇到障碍物，等待超时\n");
return -1;
}
}
} //end while
Log("agv转向过程中收到了EMCY");
printf("agv转向过程中收到了EMCY\n");
return -1;
} //end AGV自旋
//电机举升下降程序：在举升程序运行前需要加入一个归零指令！！！！！
//问题：举升电机0位置使用startcode=R9_position，则0位置=初始位置，若举升电机初始位置不在0，则可能出错
int AgvUpDown(char direction)//direction为1或0,分别代表举升和下降
{
int limitswitch_up=0;//上限位开关
int limitswitch_down=0;//下限位开关
// innerstruct.StopCommand=0;
if(direction=='1')//举升
{
M3_TPDO_Target_Position=(LiftTargetPosition+UpMotorInitPosition);//将位置信息传给OD
sendPDOevent(CANOpenShellOD_Data);//发送PDO
M3_TPDO_control_word=LiftStart;//启动电机
sendPDOevent(CANOpenShellOD_Data);
int delaycount=0;
while(delaycount<=position_delay) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
}
if(direction=='0')//下降
{
//先发送一个不为0的值，以防第一次发送0与主站上线时遍历发送的0重叠导致放弃发送
M3_TPDO_Target_Position=(0x00000001+UpMotorInitPosition);
sendPDOevent(CANOpenShellOD_Data);//发送PDO
M3_TPDO_Target_Position=(LiftZeroPosition+UpMotorInitPosition);//将位置信息传给OD
sendPDOevent(CANOpenShellOD_Data);//发送PDO
M3_TPDO_control_word=LiftStart;//启动电机
sendPDOevent(CANOpenShellOD_Data);
int delaycount=0;
while(delaycount<=position_delay) //delay 0.5秒，等待电机开始运行再开始检测
{
delaycount++;
} //100000000大概1.2s左右；300000000=4.5s左右
}
while(agvreportstruct.MotorError==0)
{
//问题：直接使用IN5_Limit_Switch_bit1_8用于判断，判断时，是否可被写入？？是否冲突？？！
//取出举升上限位开关=bit4
limitswitch_up =(IN5_Limit_Switch_bit1_8 & (1<<4)) ? 1 : 0; //判断bit4=1，则limitswitch_up=1；若bit4=0，则limitswitch_up=0
//取出举升下限位开关=bit5
limitswitch_down =(IN5_Limit_Switch_bit1_8 & (1<<5)) ? 1 : 0; //判断bit5=1，则limitswitch_up=1；若bit5=0，则limitswitch_up=0
/*判断电机是否到达目标位置*/
if((R9_statusword & (1<<(10)))!=0)//电机状态显示执行完毕,检测6041 bit10，到达位置时bit10=1；直到下次运动开始时，bit10自动清零
{
/*R9_statusword与bit10=1取“与”，得到R9的bit10位 其余位=0；结果只能判断是否==0
* 若R9到达位置，则bit10=1，则"1!=0",if成立，跳出while
* 若R9未到达位置，则bit10=0，，则“0!=0”不成立，继续while循环*/
Log("举升或下降完成");
printf("举升或下降完成\n");
M3_TPDO_control_word=LiftStop;//修改bit4=0
sendPDOevent(CANOpenShellOD_Data);
return 0;
}
else if(innerstruct.StopCommand==1)//如果收到停止命令，正常情况下轮子转动过程中不能停止，但遇到特殊情况强行停止也可以，只是要记录到日志里
{
//发送停止命令
Log("agv举升或下降过程中收到了停止命令");
printf("agv举升或下降过程中收到了停止命令\n");
return AgvStop();
}
if(limitswitch_up==1||limitswitch_down==1)//上或下限位开关触发！
{
AgvEmcyStop();//停止所有电机，红灯亮
Log("agv举升或下降过程中限位开关触发");
printf("agv举升或下降过程中限位开关触发\n");
return -1;
}
}
Log("agv举升或下降过程中收到了EMCY");
printf("agv举升或下降过程中收到了EMCY\n");
return -1;
}
//下面是电机前行程序
int AgvFoward(char fastslow,float slowpoint,char rfidTarget)
{
if(WalkingPreperationSpeedSet(fastslow,1,slowpoint)!=0)
{
printf("电机速度设置出错，无法运动\n");
Log("电机速度设置出错，无法运动\n");
return -1;
}
int rfidnum[4];//分别代表rfid1,rfid2,rfid3,rfid4
int hitavoid[3];//存储行进方向前左右三个导航的名称
rfidnum[0]=0;rfidnum[1]=1;rfidnum[2]=2;rfidnum[3]=3;
hitavoid[0]=0;hitavoid[1]=1;hitavoid[2]=2;
int direc=1;//行进方向前
float PointDistance[2];//存储减速点和停止点距离
PointDistance[0]=slowpoint;
PointDistance[1]=slowpoint+2;
int AdjustNavi[2];//纠偏使用的导航,第一个是前导航，第二个是后导航
AdjustNavi[0]=1;AdjustNavi[1]=3;
INTEGER32 M2_position[24];//纠偏所需的旋转电机的位置值，从0-23依次是前轮、后轮所需的24个赋值。
//前轮偏左
M2_position[0]=Clockwis_Degree3;
M2_position[1]=Clockwis_Degree3;
M2_position[2]=Rotator_zero;
M2_position[3]=Rotator_zero;
//前轮偏右
M2_position[4]=AntiClockwis_Degree3;
M2_position[5]=AntiClockwis_Degree3;
M2_position[6]=Rotator_zero;
M2_position[7]=Rotator_zero;
//前轮归零*/
M2_position[8]=Rotator_zero;
M2_position[9]=Rotator_zero;
M2_position[10]=Rotator_zero;
M2_position[11]=Rotator_zero;
//后轮偏右
M2_position[12]=Rotator_zero;
M2_position[13]=Rotator_zero;
M2_position[14]=Clockwis_Degree4;
M2_position[15]=Clockwis_Degree4;
//后轮偏左
M2_position[16]=Rotator_zero;
M2_position[17]=Rotator_zero;
M2_position[18]=AntiClockwis_Degree4;
M2_position[19]=AntiClockwis_Degree4;
//后轮归零
M2_position[20]=Rotator_zero;
M2_position[21]=Rotator_zero;
M2_position[22]=Rotator_zero;
M2_position[23]=Rotator_zero;
rfid[0]=rfidTarget;
rfid[1]=rfid[0];
rfid[2]=rfid[0];
rfid[3]=rfid[0];
//printf("rfid[0]==%d, rfid[1]==%d,rfid[2]==%d,rfid[3]==%d\n",rfid[0]-48,rfid[1]-48,rfid[2],rfid[3]);
//printf("rfidnum[1]=1==%d\n",rfidnum[1]=1);