-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy path20200904.diff
1789 lines (1709 loc) · 67.7 KB
/
20200904.diff
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
diff --cc modules/control/demo_control_component.cc
index 96be500,9ecfd21..0000000
--- a/modules/control/demo_control_component.cc
+++ b/modules/control/demo_control_component.cc
@@@ -1,306 -1,128 +1,385 @@@
- 锘�#include "modules/control/demo_control_component.h"
+ #include "modules/control/demo_control_component.h"
-#include "modules/control/diamond_control_pid.h"
#include <string>
+#include "math.h"
+
#include "cyber/cyber.h"
#include "cyber/time/rate.h"
-#include "math.h"
-namespace apollo
-{
-namespace control
-{
+#include "modules/common/adapters/adapter_gflags.h"
+#include "modules/control/control_wheel_angle_real.h"
+#include "modules/control/diamondauto_control_pid.h"
+
++#include "fstream"
+namespace apollo {
+namespace control {
using apollo::cyber::Rate;
using apollo::cyber::Time;
-bool ControlComponent::Init()
-{
- // Reader
- chassis_reader_ = node_->CreateReader<Chassis>(
- FLAGS_chassis_topic, [this](const std::shared_ptr<Chassis>& chassis)
- {
- chassis_.CopyFrom(*chassis);
- });
+bool ControlComponent::Init() {
+ // Chassis Reader
+ chassis_reader_ = node_->CreateReader<Chassis>(
+ FLAGS_chassis_topic, [this](const std::shared_ptr<Chassis>& chassis) {
+ chassis_.CopyFrom(*chassis);
+ });
+
+ // Magnetic Reader
+ // TODO: check
+ magnetic_reader_ = node_->CreateReader<Magnetic>(
+ FLAGS_magnetic_channel,
+ [this](const std::shared_ptr<Magnetic>& magnetic) {
+ magnetic_.CopyFrom(*magnetic);
+ });
- // create Writer
- control_cmd_writer_ = node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
+ // rfid Reader
+ // TODO: check
+ rfid_reader_ = node_->CreateReader<RFID>(
+ FLAGS_rfid_topic,
+ [this](const std::shared_ptr<RFID>& rfid) { rfid_.CopyFrom(*rfid); });
- // compute control message in aysnc
- async_action_ = cyber::Async(&ControlComponent::GenerateCommand, this);
+ // create Writer
+ control_cmd_writer_ =
+ node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
-
++
++ chassis_writer_ =
++ node_->CreateWriter<Chassis>(FLAGS_chassis_topic);
+ // compute control message in aysnc
+ async_action_ = cyber::Async(&ControlComponent::GenerateCommand, this);
- return true;
+ return true;
}
// write to channel
-void ControlComponent::GenerateCommand()
-{
- auto cmd = std::make_shared<ControlCommand>();
-
- // frequency, TODO: reset
- Rate rate(20.0);
-
- // 鍒濆鍖栧墠鍚庣瀵艰埅妫�娴嬪埌鐨勫亸宸��
- float front_lat_dev_mgs = 0; // TODO: lateral_dev_mgs need to changed according to MGS module.
- float back_lat_dev_mgs = 0;
-
- int find_rfid_des = 0; //璁板綍鏄惁妫�娴嬪埌缁堢偣鐨剅fid
- int dir = 0; //璁板綍琛岄┒鏂瑰悜, 0浠ｈ〃浠嶢鍒癇, 1浠ｈ〃浠嶣鍒癆
-
- // TODO锛氬彂閫佹壄鐭╋紝PID澶╅棷
- int motor_flag = 0; //鎺у埗椹卞姩鐢垫満姝ｅ弽杞�
- float motor_speed = 0; //椹卞姩鐢垫満杞��
- float motor_torque = 0; //椹卞姩鐢垫満杞煩
-
- int front_steering_dir = 0; //鎺у埗鍓嶆柟杞悜鐢垫満姝ｅ弽杞�
- int back_steering_dir = 0; //鎺у埗鍚庢柟杞悜鐢垫満姝ｅ弽杞�
-
- float front_steering_speed = 0; //rpm锛屽墠鏂硅浆鍚戠數鏈鸿浆閫�
- float back_steering_speed = 0; //rpm锛屽悗鏂硅浆鍚戠數鏈鸿浆閫�
-
- cmd -> set_front_steering_target(0);
- cmd -> set_back_steering_target(0); //鍒濆鏃跺墠鍚庤浆鍚戠數鏈鸿浆瑙掍负0
-
- while (true)
- {
- if (find_rfid_des == 1) //妫�娴嬪埌缁堢偣rfid
- motor_torque = pid_speed(0); //PID鎺у埗鐩爣鏄┍鍔ㄧ數鏈哄仠杞�
- // 浠�0涓鸿溅閫熺洰鏍囷紝鍚慶anbus鍙戦�佺粡杩嘝ID鍚庣殑杞煩
- else if (find_rfid_des == 0) //鏈娴嬪埌缁堢偣rfid
- motor_torque = pid_speed(1); //椹卞姩鐢垫満椹卞姩姹借溅浠�1m/s杩愬姩锛岃繖鏄疨ID鐩爣锛屽敖鍙兘鎺ヨ繎1
- // 浠�1涓鸿溅閫熺洰鏍囷紝鍚慶anbus鍙戦�佺粡杩嘝ID鍚庣殑杞煩
-
- // 閬ユ帶锛屼汉宸ョ粰杈撳叆
- if (dir == 0) //鑻ヨ椹舵柟鍚戜粠A鍒癇锛堝墠杩涳級
- motor_flag = 0;
- else if (dir == 1) //鑻ヨ椹舵柟鍚戜粠B鍒癆锛堝悗閫�锛�
- motor_flag = 1;
-
- if (/* motor_speed == 1 && */ motor_flag == 0) // 鑻ラ┍鍔ㄧ數鏈烘杞紙鍓嶈繘锛�
- {
- back_steering_dir = 2; //鍚庢柟杞悜鐢垫満涓嶈浆
- if (front_lat_dev_mgs < -4.5) //鑻ュ墠鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸宸�
- {
- front_steering_dir = 0; //鍒欏墠鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
- cmd -> set_front_steering_target(10); //杞悜閲忎负10锛屽緟鏍囧畾
- }
- else if (frong_lat_dev_mgs > 4.5) //鑻ュ墠鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸鍙�
- {
- front_steering_dir = 1; //鍒欏墠鏂硅浆鍚戠數鏈哄弽杞紙鍗冲悜宸︼級
- cmd -> set_front_steering_target(10); //杞悜閲忎负10锛屽緟鏍囧畾
- }
- else
- front_steering_dir = 2; //鍓嶆柟杞悜鐢垫満涓嶈浆
- }
- else if (/* motor_speed == 1*/ motor_flag == 1) // 鑻ラ┍鍔ㄧ數鏈哄弽杞紙鍊掕溅锛�
- {
- front_steering_dir = 2; //鍓嶆柟杞悜鐢垫満涓嶈浆
- if (back_lat_dev_mgs < -4.5) //鑻ュ悗鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸宸�
- {
- back_steering_dir = 0; //鍒欏悗鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
- cmd -> set_back_steering_target(10); //杞悜閲忎负10
- }
- else if (frong_lat_dev_mgs > 4.5) //鑻ュ悗鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸鍙�
- {
- back_steering_dir = 1; //鍒欏悗鏂硅浆鍚戠數鏈哄弽杞紙鍗冲悜宸︼級
- cmd -> set_back_steering_target(10); //杞悜閲忎负10
- }
- else
- back_steering_dir = 2; //鍚庢柟杞悜鐢垫満涓嶈浆
- }
- else{// 鑻ュ嚭鐜板紓甯�
- motor_speed = 0;
- front_steering_dir = 2;
- back_steering_dir = 2;
- }
-
- control_cmd_writer_->Write(cmd);
- rate.Sleep();
- }
+void ControlComponent::GenerateCommand() {
+ auto cmd = std::make_shared<ControlCommand>();
-
++ auto chassis_wheel_angle = std::make_shared<Chassis>();
+ // frequency
+ Rate rate(100.0);
+
++ //static int MANUAL_BRAKE_SIGNAL = 0;
++
+ float front_lat_dev_mgs = 0;
+ float rear_lat_dev_mgs = 0;
+
+ // 鑾峰彇褰撳墠杞﹁締閫熷害
+ veh_spd = chassis_.speed_mps();
+
+ // 鑾峰彇鍓嶅悗缂栫爜鍣ㄧ灛鏃惰搴﹀��
- front_encoder_angle_realtime = chassis_.front_encoder_angle();
- rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
-
++ if (isnan(chassis_.front_encoder_angle())){
++ front_encoder_angle_realtime = front_encoder_angle_previous;
++ }
++ else{
++ front_encoder_angle_realtime = chassis_.front_encoder_angle();
++ }
++ if (isnan(chassis_.rear_encoder_angle())){
++ rear_encoder_angle_realtime = rear_encoder_angle_previous;
++ }
++ else{
++ rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
++ }
+ // 鍒濆鍖栭┍鍔ㄧ數鏈烘鍖�
+ if (veh_spd <= 0.1) {
+ speed_motor_deadzone =
+ speed_motor_deadzone_calibration; // TODO: 钀藉湴鍚庢爣瀹氬��
+ } else {
+ speed_motor_deadzone =
+ r_wheel * m_veh * g * f_c /
+ (i_1 * i_0 *
+ yita_t); // 浣跨敤婊氬姩闃诲姏绯绘暟锛屾鏃舵鍖烘寚鐨勬槸鐞嗚鐢垫満闇�姹傝浆鐭�
+ }
+
+ // TODO:
+ // 绐佺劧鏂數鍋滆溅鍚庯紝闇�璁板綍鏈�鍚庢椂鍒荤殑鐘舵�佹暟鎹紝鍖呮嫭鍓嶅悗杞浆瑙掞紱閲嶆柊鍚姩鍚庡啀璇诲彇鏁版嵁鏂囦欢
+
+ // TODO: 鍦╳hile鍒ゆ柇鍓嶉渶瑕佺煡閬撹疆鑳庡垵濮嬪亸绉昏搴︼紝鏆傛椂鍏堢粰0銆�
+ front_wheel_angle_realtime = 0;
+ rear_wheel_angle_realtime = 0;
+
+ //涓婅繃楂樺帇鑷瀹屾垚涔嬪悗锛岃繘鍏ヨ嚜鍔ㄩ┚椹舵ā寮忓悗锛岃溅杈嗗浜巖eady鐘舵�佹椂
+ while (front_wheel_angle_realtime > 0.5) // 寰呮爣瀹�
+ {
+ front_motor_steering_dir = 2;
+ cmd->set_front_steering_switch(Chassis::STEERINGNEGATIVE);
-
++ cmd->set_front_wheel_target(-10.0);
+ // 鑾峰彇缂栫爜鍣ㄧ灛鏃惰搴﹀��
- front_encoder_angle_realtime = chassis_.front_encoder_angle();
++ // front_encoder_angle_realtime = chassis_.front_encoder_angle();
+
+ front_wheel_angle_realtime = update_wheel_angle(
+ front_wheel_angle_previous, front_encoder_angle_previous,
+ front_encoder_angle_realtime, encoder2wheel_gear_ratio);
+
+ front_wheel_angle_previous = front_wheel_angle_realtime;
+ front_encoder_angle_previous = front_encoder_angle_realtime;
+ }
+
+ while (rear_wheel_angle_realtime > 0.5) // 寰呮爣瀹�
+ {
+ rear_motor_steering_dir = 2; //鍒欏悗鏂硅浆鍚戠數鏈哄弽杞紙鍗冲悜宸︼級
+ cmd->set_rear_steering_switch(Chassis::STEERINGNEGATIVE);
-
++ cmd->set_rear_wheel_target(-10.0);
+ // 鑾峰彇缂栫爜鍣ㄧ灛鏃惰搴﹀��
- rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
++ // rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
+
+ rear_wheel_angle_realtime = update_wheel_angle(
+ rear_wheel_angle_previous, rear_encoder_angle_previous,
+ rear_encoder_angle_realtime, encoder2wheel_gear_ratio);
+
+ rear_wheel_angle_previous = rear_wheel_angle_realtime;
+ rear_encoder_angle_previous = rear_encoder_angle_realtime;
+ }
+
+ while (front_wheel_angle_realtime < -0.5) // 寰呮爣瀹�
+ {
+ front_motor_steering_dir = 1; //鍒欏墠鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
+ /*
+ Xavier鍚戝墠鍙橀鍣ㄥ彂閫侊細鍚戝墠杞悜鐢垫満鍙戦�佹杞懡浠わ細0B 06 10 00 00 01 4C
+ 60锛屽悓鏃跺彂閫侀瀹氳浆閫熷懡浠わ細0B 06 20 00 27 10 98 9C 锛堜篃鍙皟閫燂紝鍚庢湡鏍囧畾锛�
+ 鍚屾椂Xavier鍚戝洓鍚堜竴id_0x0C079AA7 鍙戦�� AA AA AA 55 AA AA AA
+ AA锛岄�嗗彉鍣�1宸ヤ綔锛屽墠杞悜鐢垫満椋庢墖1宸ヤ綔
+ */
+ // TODO: 娑堟伅鍙戦�侊紝鍒锋柊
+ cmd->set_front_steering_switch(Chassis::STEERINGPOSITIVE);
-
++ cmd->set_front_wheel_target(10.0);
+ // 鑾峰彇缂栫爜鍣ㄧ灛鏃惰搴﹀��
- front_encoder_angle_realtime = chassis_.front_encoder_angle();
++ // front_encoder_angle_realtime = chassis_.front_encoder_angle();
+
+ front_wheel_angle_realtime = update_wheel_angle(
+ front_wheel_angle_previous, front_encoder_angle_previous,
+ front_encoder_angle_realtime, encoder2wheel_gear_ratio);
+
+ front_wheel_angle_previous = front_wheel_angle_realtime;
+ front_encoder_angle_previous = front_encoder_angle_realtime;
+ }
+
+ while (rear_wheel_angle_realtime < -0.5) // 寰呮爣瀹�
+ {
+ rear_motor_steering_dir = 1; //鍒欏悗鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
+ /*
+ Xavier鍚戝悗鍙橀鍣ㄥ彂閫侊細鍚戝悗杞悜鐢垫満鍙戦�佹杞懡浠わ細0C 06 10 00 00 01 4D
+ D7锛屽悓鏃跺彂閫侀瀹氳浆閫熷懡浠わ細0C 06 20 00 27 10 99 2B 锛堜篃鍙皟閫燂紝鍚庢湡鏍囧畾锛�
+ 鍚屾椂Xavier鍚戝洓鍚堜竴 id_0x0C079AA7 鍙戦�� AA AA 55 AA AA AA AA
+ AA锛岄�嗗彉鍣�2宸ヤ綔锛屽悗杞悜鐢垫満椋庢墖2宸ヤ綔
+ */
+ // TODO: 娑堟伅鍙戦�侊紝鍒锋柊
+ cmd->set_rear_steering_switch(Chassis::STEERINGPOSITIVE);
-
++ cmd->set_rear_wheel_target(10.0);
+ // 鑾峰彇缂栫爜鍣ㄧ灛鏃惰搴﹀��
- rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
++ // rear_encoder_angle_realtime = chassis_.rear_encoder_angle();
+
+ rear_wheel_angle_realtime = update_wheel_angle(
+ rear_wheel_angle_previous, rear_encoder_angle_previous,
+ rear_encoder_angle_realtime, encoder2wheel_gear_ratio);
+
+ rear_wheel_angle_previous = rear_wheel_angle_realtime;
+ rear_encoder_angle_previous = rear_encoder_angle_realtime;
+ }
-
++ ofstream out("out.txt",ios::app);
++ out << front_encoder_angle_realtime << "\t\t" << rear_wheel_angle_realtime << "\n";
++ out.close();
+ /*
+ Xavier鍚戝墠鍙橀鍣ㄥ彂閫侊細鍚戝墠杞悜鐢垫満鍙戦�佸仠杞懡浠わ細0B 06 10 00 00 05 4D
+ A3 鍚屾椂Xavier鍚戝洓鍚堜竴id_0x0C079AA7 鍙戦�� AA AA AA AA AA AA AA
+ AA锛岄�嗗彉鍣�1鍋滄宸ヤ綔锛屽墠杞悜鐢垫満椋庢墖1鍋滄宸ヤ綔
+ */
- cmd->set_front_steering_switch(Chassis::STEERINGSTOP);
++ //cmd->set_front_steering_switch(Chassis::STEERINGSTOP);
+
+ /*
+ Xavier鍚戝悗鍙橀鍣ㄥ彂閫侊細鍚戝悗杞悜鐢垫満鍙戦�佸仠杞懡浠わ細0C 06 10 00 00 05 4C
+ 14 鍚屾椂Xavier鍚戝洓鍚堜竴id_0x0C079AA7 鍙戦�� AA AA AA AA AA AA AA
+ AA锛岄�嗗彉鍣�2鍋滄宸ヤ綔锛屽悗杞悜鐢垫満椋庢墖2鍋滄宸ヤ綔
+ */
+ cmd->set_rear_steering_switch(Chassis::STEERINGSTOP);
+
+ front_motor_steering_dir = 0; //鍒欏墠鏂硅浆鍚戠數鏈哄仠杞�
+ rear_motor_steering_dir = 0; //鍒欏悗鏂硅浆鍚戠數鏈哄仠杞�
+
+ while (true) {
+ // TODO: Configuration
- // 鍏堝彧鐪嬩粠A鍒癇
++ // 鎵嬪姩缁欏畾锛�0浠ｈ〃鍋滄锛�1浠ｈ〃浠嶢鍒癇锛�2浠ｈ〃浠嶣鍒癆
+ drivemotor_flag = 1;
+
- if (rfid_.id() == 2) { //妫�娴嬪埌B鐐箁fid
- drivemotor_torque = pid_speed(
- veh_spd, 0, speed_motor_deadzone); // PID鎺у埗鐩爣鏄┍鍔ㄧ數鏈哄仠杞�
- cmd->set_throttle(static_cast<float>(drivemotor_torque));
- }
- // 浠�0涓鸿溅閫熺洰鏍囷紝鍚慶anbus鍙戦�佺粡杩嘝ID鍚庣殑杞煩
-
- else { //鏈娴嬪埌B鐐箁fid
- drivemotor_torque = pid_speed(
- veh_spd, FLAGS_desired_v,
- speed_motor_deadzone); //椹卞姩鐢垫満椹卞姩姹借溅浠�1m/s杩愬姩锛岃繖鏄疨ID鐩爣锛屽敖鍙兘鎺ヨ繎1
- drivemotor_torque = 50.0;
- cmd->set_throttle(static_cast<float>(drivemotor_torque));
- }
- // 浠�1涓鸿溅閫熺洰鏍囷紝鍚慶anbus鍙戦�佺粡杩嘝ID鍚庣殑杞煩
++ switch (drivemotor_flag){
++ // 浠嶢鍒癇
++ case 1:{
++ if (rfid_.id() == 2){
++ // TODO: 鍒跺姩杞煩锛岄渶鏀规垚鏍囧畾鍊�
++ drivemotor_torque = 50;
++ cmd->set_front_brake(drivemotor_torque);
++ }
++ else{
++ drivemotor_torque = pid_speed(veh_spd, FLAGS_desired_v,speed_motor_deadzone);
++ cmd->set_front_throttle(drivemotor_torque);
++ }
++
++ // TODO锛氭娴嬪埌杞﹂�熶负0锛岄┗杞︾郴缁熸柇姘斿埞锛岃溅杈嗛┗杞﹀仠姝�
++ break;
++ }
++
++ // 浠嶣鍒癆
++ case 2:{
++ if (rfid_.id() == 1){
++ // TODO: 鍒跺姩杞煩锛岄渶鏀规垚鏍囧畾鍊�
++ drivemotor_torque = 50;
++ cmd->set_rear_brake(drivemotor_torque);
++ }
++ else{
++ drivemotor_torque = pid_speed(veh_spd, FLAGS_desired_v,speed_motor_deadzone);
++ cmd->set_rear_throttle(drivemotor_torque);
++ }
++
++ // TODO锛氭娴嬪埌杞﹂�熶负0锛岄┗杞︾郴缁熸柇姘斿埞锛岃溅杈嗛┗杞﹀仠姝�
++ break;
++ }
+
++ // 鍋滄鐘舵��
++ case 0:{
++ cmd->set_front_throttle(0);
++ cmd->set_rear_throttle(0);
++
++ // TODO锛氭娴嬪埌杞﹂�熶负0锛岄┗杞︾郴缁熸柇姘斿埞锛岃溅杈嗛┗杞﹀仠姝�
++ break;
++ }
++ }
++
+ /*
+ // 閬ユ帶锛屼汉宸ョ粰杈撳叆
+ if (veh_dir == 0) //鑻ヨ椹舵柟鍚戜粠A鍒癇锛堝墠杩涳紝A鏄2杈嗚溅澶勶紝B鏄満姊拌噦澶勶級
+ drivemotor_flag = 1;
+ else if (veh_dir == 1) //鑻ヨ椹舵柟鍚戜粠B鍒癆锛堝悗閫�锛�
+ drivemotor_flag = 2;
+ */
+
+ front_wheel_angle_realtime = update_wheel_angle(
+ front_wheel_angle_previous, front_encoder_angle_previous,
+ front_encoder_angle_realtime, encoder2wheel_gear_ratio);
+ rear_wheel_angle_realtime = update_wheel_angle(
+ rear_wheel_angle_previous, rear_encoder_angle_previous,
+ rear_encoder_angle_realtime, encoder2wheel_gear_ratio);
++
++ // 妫�娴嬪埌杞儙杞瓒呰繃30掳锛岃浆鍚戠數鏈哄仠杞�
++ if (abs(front_wheel_angle_realtime) > 30){
++ front_motor_steering_dir = 0;
++ cmd->set_front_steering_switch(Chassis::STEERINGSTOP);
++ }
++
++ if (abs(rear_wheel_angle_realtime) > 30){
++ rear_motor_steering_dir = 0;
++ cmd->set_rear_steering_switch(Chassis::STEERINGSTOP);
++ }
++
++
++ /*
++ // TODO锛氭敹鍒颁汉宸ヤ粙鍏ュ埗鍔ㄤ俊鍙疯繘琛屽埗鍔�
++ // TODO: manual_brake_signal:瀹氫箟鏂伴�氶亾涓撻棬鐢ㄤ簬鑷姩鎺у埗涓嬬殑浜哄伐浜や簰
++ MANUAL_BRAKE_SIGNAL = chassis_.manual_brake_signal();
++ if ( MANUAL_BRAKE_SIGNAL != 0 ){
++ // TODO: 鍒跺姩杞煩锛岄渶鏀规垚鏍囧畾鍊�
++ drivemotor_torque = 50;
++ cmd->set_brake(drivemotor_torque);
++ }*/
+
+ // 鍒濆鍖栧墠鍚庣瀵艰埅妫�娴嬪埌鐨勫亸宸�硷紝璁㈤槄纾佸鑸�氶亾鐨勬暟鎹�
++ // TODO: 妫�鏌ワ紝鍏辩敤浜嗕竴涓暟鎹�
+ front_lat_dev_mgs = magnetic_.lat_dev();
+ rear_lat_dev_mgs = -magnetic_.lat_dev();
+
+ // 缁欏畾椹卞姩鐢垫満鍙嶈浆鍛戒护锛堜娇杞﹁締鍓嶈繘浠嶢鍒癇锛�
+ if (/* motor_speed == 1 && */ drivemotor_flag == 1) {
+ rear_motor_steering_dir = 0; //鍚庢柟杞悜鐢垫満涓嶈浆
+ if (front_lat_dev_mgs < -4.5) //鑻ュ墠鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸宸�
+ {
+ front_motor_steering_dir = 1; //鍒欏墠鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
+ cmd->set_front_steering_switch(Chassis::STEERINGPOSITIVE);
++ cmd->set_front_wheel_target(10.0);
+ } else if (front_lat_dev_mgs > 4.5) //鑻ュ墠鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸鍙�
+ {
+ front_motor_steering_dir = 2; //鍒欏墠鏂硅浆鍚戠數鏈哄弽杞紙鍗冲悜宸︼級
+ cmd->set_front_steering_switch(Chassis::STEERINGNEGATIVE);
++ cmd->set_front_wheel_target(-10.0);
+ } else {
+ if (front_wheel_angle_realtime >= 0.5) // 褰撳墠鍓嶈疆杞涓烘锛屽悜鍙冲亸
+ {
+ front_motor_steering_dir = 2; // 鍓嶆柟杞悜鐢垫満鍙嶈浆锛堝悜宸︼級
+ cmd->set_front_steering_switch(Chassis::STEERINGNEGATIVE);
++ cmd->set_front_wheel_target(-10.0);
+ } else if ((front_wheel_angle_realtime > -0.5) &&
+ (front_wheel_angle_realtime < 0.5)) {
+ front_motor_steering_dir = 0; // 鍓嶆柟杞悜鐢垫満鍋滆浆
+ cmd->set_front_steering_switch(Chassis::STEERINGSTOP);
+ } else // 褰撳墠鍓嶈疆杞涓鸿礋锛屽悜宸﹀亸
+ {
+ front_motor_steering_dir = 1; // 鍓嶆柟杞悜鐢垫満姝ｈ浆锛堝悜鍙筹級
+ cmd->set_front_steering_switch(Chassis::STEERINGPOSITIVE);
++ cmd->set_front_wheel_target(10.0);
+ }
+ }
+ } else if (/* motor_speed == 1*/ drivemotor_flag ==
+ 2) // 鑻ラ┍鍔ㄧ數鏈烘杞紙鍊掕溅锛岃溅杈嗕粠B鍒癆锛�
+ {
+ front_motor_steering_dir = 0; //鍓嶆柟杞悜鐢垫満涓嶈浆
+ if (rear_lat_dev_mgs < -4.5) //鑻ュ悗鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸宸�
+ {
+ rear_motor_steering_dir = 1; //鍒欏悗鏂硅浆鍚戠數鏈烘杞紙鍗冲悜鍙筹級
+ cmd->set_rear_steering_switch(Chassis::STEERINGPOSITIVE);
++ cmd->set_rear_wheel_target(10.0);
+ } else if (rear_lat_dev_mgs > 4.5) //鑻ュ悗鏂圭瀵艰埅妫�娴嬪嚭杞﹀亸鍙�
+ {
+ rear_motor_steering_dir = 2; //鍒欏悗鏂硅浆鍚戠數鏈哄弽杞紙鍗冲悜宸︼級
+ cmd->set_rear_steering_switch(Chassis::STEERINGNEGATIVE);
++ cmd->set_rear_wheel_target(-10.0);
+ } else {
+ if (rear_wheel_angle_realtime >= 0.5) // 褰撳墠鍚庤疆杞涓烘锛屽悜鍙冲亸
+ {
+ rear_motor_steering_dir = 2; // 鍚庢柟杞悜鐢垫満鍙嶈浆锛堝悜宸︼級
++ cmd->set_rear_wheel_target(-10.0);
+ cmd->set_rear_steering_switch(Chassis::STEERINGNEGATIVE);
+ } else if ((rear_wheel_angle_realtime > -0.5) &&
+ (rear_wheel_angle_realtime < 0.5)) {
+ rear_motor_steering_dir = 0; // 鍚庢柟杞悜鐢垫満鍋滆浆
+ cmd->set_rear_steering_switch(Chassis::STEERINGSTOP);
+ } else // 褰撳墠鍚庤疆杞涓鸿礋锛屽悜宸﹀亸
+ {
+ rear_motor_steering_dir = 1; // 鍚庢柟杞悜鐢垫満姝ｈ浆锛堝悜鍙筹級
++ cmd->set_rear_wheel_target(10.0);
+ cmd->set_rear_steering_switch(Chassis::STEERINGPOSITIVE);
+ }
+ }
+ } else { // 鑻ュ嚭鐜板紓甯�
+ // auto motor_torque = pid_speed(veh_spd, 0, speed_motor_deadzone);
+ front_motor_steering_dir = 0; // 鍋滄
+ rear_motor_steering_dir = 0; // 鍋滄
+ cmd->set_front_steering_switch(Chassis::STEERINGSTOP);
+ cmd->set_rear_steering_switch(Chassis::STEERINGSTOP);
+ }
+
+ // 鏇存柊杞儙杞鍜岀紪鐮佸櫒搴︽暟
+ front_wheel_angle_realtime = update_wheel_angle(
+ front_wheel_angle_previous, front_encoder_angle_previous,
+ front_encoder_angle_realtime, encoder2wheel_gear_ratio);
+ rear_wheel_angle_realtime = update_wheel_angle(
+ rear_wheel_angle_previous, rear_encoder_angle_previous,
+ rear_encoder_angle_realtime, encoder2wheel_gear_ratio);
+
+ front_wheel_angle_previous = front_wheel_angle_realtime;
+ rear_wheel_angle_previous = rear_wheel_angle_realtime;
+
+ front_encoder_angle_previous = front_encoder_angle_realtime;
+ rear_encoder_angle_previous = rear_encoder_angle_realtime;
++
++ chassis_wheel_angle->set_front_wheel_angle(front_wheel_angle_realtime);
++ chassis_wheel_angle->set_rear_wheel_angle(rear_wheel_angle_realtime);
++
++ chassis_writer_->Write(chassis_wheel_angle);
+
+ control_cmd_writer_->Write(cmd);
+ rate.Sleep();
+ }
}
-ControlComponent::~ControlComponent()
-{
- // back chassis handle
- async_action_.wait();
+ControlComponent::~ControlComponent() {
+ // back chassis handle
+ async_action_.wait();
}
} // namespace control
diff --cc modules/control/diamondauto_control_pid.h
index 261b667,53f1d84..0000000
mode 100644,100644..100755
--- a/modules/control/diamondauto_control_pid.h
+++ b/modules/control/diamondauto_control_pid.h
@@@ -1,11 -1,9 +1,11 @@@
- 锘�#pragma once
-#ifndef _DIAMONDAUTO_CONTROL_H_
-#define _DIAMONDAUTO_CONTROL_H_
++#pragma once
#include <stdlib.h>
-#include <iostream>
#include <cmath>
+#include <iostream>
+
+namespace apollo {
+namespace control {
using namespace std;
@@@ -31,39 -29,213 +31,33 @@@ static float veh_spd = 0
static float speed_motor_deadzone = 0;
-static float pid_integral = 0; // 纵向pid累积量
-static float pid_error = 0;
+static float pid_integral = 0;
+static float pid_error = 0;
static float pid_error_pre = 0;
-static float fmottq = 0;
-static float canbus_veh_spd = 0;
-static int veh_mode = 0;
-
-// void pid_steering(float & e2, float & e1, float & e); // 转向PID控制，PID输出控制量（转速）给转向电机
-// float steer_motor(float error); // 经过电机响应、车辆运动，输出下一时刻的磁导航偏差
-// void pid_speed(float desire_v); // 纵向速度PID控制，PID输出控制量（转矩）给驱动电机
-
-void Init()
-{
- cout<<"初始化......"<<endl<<endl;
-};
-
-void Self_check()
-{
- cout<<"车辆状态自检......"<<endl<<endl;
-};
-
-
-/*void pid_steering(float & e2, float & e1, float & e)
-{
- cout<<"控制前的磁导航偏差："<<e2<<"、"<<e1<<"、"<<e<<endl;
-
- delta_u_steer = kp_steer*(e-e1)+ki_steer*(e)+kd_steer*(e-2*e1+e2); // pid输出的控制量增量
-
- u_steer = u_pre_steer + delta_u_steer;
-
- cout<<"PID输出给转向电机的控制量："<<u_steer<<endl;
-
- u_pre_steer = u_steer;
-
- // 判断电机正反转
- if (e>0)
- {
- cout<<"转向轮应左转"<<endl;
- steer_direction = 1;
- }
- else if (e<0)
- {
- cout<<"转向轮应右转"<<endl;
- steer_direction = -1;
- }
- else
- {
- cout<<"转向轮应直行"<<endl;
- steer_direction = 0;
- }
-
- e2 = e1;
- e1 = e;
- e = steer_motor(e);
-
- cout<<"经过PID调节，新的磁导航偏差序列："<<e2<<"、"<<e1<<"、"<<e<<endl;
-
-};
-*/
-
-/* float steer_motor(float e)
-{
- if (steer_direction == 1) // 左转
- {
- cout<<"电机左转"<<endl;
- // CAN通讯给转向电机左转命令；
- }
- else if(steer_direction == -1) // 右转
- {
- cout<<"电机右转"<<endl;
- // CAN通讯给转向电机右转命令；
- }
-
-
- // 电机模型：由 pid 控制量 到 转向电机输出转速；
- steer_motor_spd = u_steer;// 随便写的
-
-
- // 减速机构：转向电机输出转速 通过减速机构 到 轮胎转向速度；
- tire_steer_spd = steer_motor_spd/20;
-
-
- // 运动更新：轮胎转向速度 到 轮胎转角(角位移传感器)；车辆横纵向运动，状态更新
- steer_angle = steer_angle + tire_steer_spd;//角位移传感器测量
-
-
- // 新的偏差检测：磁导航传感器检测当前偏差
- cout<<"轮胎转角："<<steer_angle<<endl;
-
- float error_new;
- error_new = e+sin(steer_angle*3.1415926/180)*veh_spd*100*0.05; //0.05s控制一次
- // cout<<"新的偏差："<<error_new<<endl;
- return error_new;
-};
-*/
-
-void simple_steering(float & e)
-{
- if (e>4.5 || e<-4.5)
- {
- // 下发让转向电机开始转的信号
- cout<<"转向电机开始转"<<endl;
- }
- else
- {
- // 下发让转向电机停止转的信号
- cout<<"转向电机停止转"<<endl;
- }
+// static float fmottq = 0;
+// static float canbus_veh_spd = 0;
+// static int veh_mode = 0;
-}
+float pid_speed(float veh_spd, float desire_v, float spd_motor_deadzone) {
- cout << "pid涓殑鏈熸湜閫熷害" << desire_v << endl;
+ // veh_spd = chassis_->speed_mps();
- pid_error = desire_v - veh_spd; // pid杈撳叆涓哄綋鍓嶈溅閫熻宸�
- cout << "绾靛悜閫熷害鍋忓樊" << pid_error << endl;
++ pid_error = desire_v - veh_spd;
+ pid_integral += pid_error;
-void rule_steering(float & e)
-{
- // 判断前面转向电机正反转
- if (e>4.5)
- {
- cout<<"转向轮应左转"<<endl; //根据偏差模块反馈的值进行调整
- steer_direction = 1;
- }
- else if (e<-4.5)
- {
- cout<<"转向轮应右转"<<endl;
- steer_direction = 2;
- }
- else
- {
- cout<<"转向轮应直行"<<endl;
- steer_direction = 0;
- }
-
- // 把正反转信号发给CAN通讯
-
- // 前面转向电机接收CAN给的正反转信号
-
- if (steer_direction == 1) // 左转
- {
- cout<<"电机左转"<<endl;
- // CAN通讯给转向电机左转命令；
-
- if (e>=6)
- {
- steer_motor_spd = 1435; // 数据具体给多少需要算
- }
- else if (e>=3)
- {
- steer_motor_spd = 1435;
- }
- else
- {
- steer_motor_spd = 1435;
- }
- }
- else if(steer_direction == 2) // 右转
- {
- cout<<"电机右转"<<endl;
- // CAN通讯给转向电机右转命令；
-
- if (e<=-6)
- {
- steer_motor_spd = 1435; // 转向电机额定转速1435rpm，目前不可控，正在尝试可控中，额定转速下是70s转1圈，车轮0.857rpm
- }
- else if (e<=-3)
- {
- steer_motor_spd = 1435;
- }
- else
- {
- steer_motor_spd = 1435;
- }
- }
-
- cout<<"转速电机转速为"<<steer_motor_spd<<endl;
-
- e = e/2; // 此处应该是磁导航传感器实时接收的新偏差
-
- cout<<"新的磁导航偏差为："<<e<<endl;
-}
+ u_torque = spd_motor_deadzone + kp_speed * pid_error +
+ ki_speed * pid_integral + kd_speed * (pid_error - pid_error_pre);
- cout << "PID杈撳嚭缁欓┍鍔ㄧ數鏈虹殑鎺у埗閲忥紙杞煩锛夛細" << u_torque << endl;
-float pid_speed(float desire_v) // 目的是用pid求出转矩
-{
- cout<<"pid中的期望速度："<<desire_v<<endl;
- pid_error = desire_v - veh_spd; // pid输入为当前车速误差
- cout<< "纵向速度偏差："<<pid_error<<endl;
- pid_integral += pid_error;
-
- // speed_motor_error应该是一个与期望速度相关的前馈量，给定期望速度可以算出一个该速度匀速下的目标转矩值
- // speed_motor_error应该是整车目标转矩
- u_torque = speed_motor_deadzone + kp_speed*pid_error + ki_speed*pid_integral + kd_speed*(pid_error-pid_error_pre);
+ // veh_spd = canbus_veh_spd;
- cout << "褰撳墠杞﹂��" << veh_spd << endl;
- cout<<"PID输出给驱动电机的控制量(转矩)："<<u_torque<<endl;
+ pid_error_pre = pid_error;
- veh_spd = canbus_veh_spd; // 车速更新；需要根据给驱动电机的控制量计算出车速响应；
-
- cout<<"当前车速："<<veh_spd<<endl;
- pid_error_pre = pid_error;
-
- return u_torque;
-
- // 将u_torque通过CAN通讯发送给驱动电机
+ return u_torque;
- // 灏唘_torque閫氳繃CAN閫氳鍙戦�佺粰椹卞姩鐢垫満
- // u_pre_torque = u_torque;
- // 将CANbus反馈的车速赋给当前车速
-}
+ // u_pre_torque = u_torque;
- // 灏咰ANbus鍙嶉鐨勮溅閫熶粯缁欏綋鍓嶈溅閫�
+}
-#endif
+} // namespace control
+} // namespace apollo
diff --cc modules/control/proto/control_cmd.proto
index 29273df,b2055a6..0000000
--- a/modules/control/proto/control_cmd.proto
+++ b/modules/control/proto/control_cmd.proto
@@@ -32,13 -32,13 +32,14 @@@ message ControlCommand
// target non-directional steering rate, in percentage of full scale per
// second [0, 100]
- optional double steering_rate = 6;
+ optional double steering_rate = 5;
// target front steering angle, in percentage of full scale [-100, 100]
- optional double front_steering_target = 7;
+ optional apollo.canbus.Chassis.SteeringSwitch front_steering_switch = 6;
++ //optional double front_steering_switch = 6;
- // target back steering angle, in percentage of full scale [-100, 100]
- optional double back_steering_target = 7;
+ // target rear steering angle, in percentage of full scale [-100, 100]
+ optional apollo.canbus.Chassis.SteeringSwitch rear_steering_switch = 7;
// parking brake engage. true: engaged
optional bool parking_brake = 8;
@@@ -62,7 -62,7 +63,15 @@@
optional PadMessage pad_msg = 25;
optional apollo.common.EngageAdvice engage_advice = 26;
optional bool is_in_safe_mode = 27 [default = false];
--
++
++ optional double front_throttle = 28;
++ optional double rear_throttle = 29;
++ optional double front_brake = 30;
++ optional double rear_brake = 31;
++
++ optional double front_wheel_target = 32 [default = nan];
++ optional double rear_wheel_target = 33 [default = nan];
++ optional double front_steering_switch_pre = 34 [default=0];
}
message SimpleLongitudinalDebug {
diff --git a/docker/scripts/cyber_start.sh b/docker/scripts/cyber_start.sh
index dfb0701..ded21f3 100755
--- a/docker/scripts/cyber_start.sh
+++ b/docker/scripts/cyber_start.sh
@@ -21,7 +21,8 @@ source "${APOLLO_ROOT_DIR}/scripts/apollo.bashrc"
# CACHE_ROOT_DIR="${APOLLO_ROOT_DIR}/.cache"
VERSION_X86_64="diamond-auto-x86_64-18.04-20200727_1838"
-VERSION_AARCH64="diamond-auto-aarch64-18.04-20200729_1004"
+#VERSION_AARCH64="diamond-auto-aarch64-18.04-20200729_1004"
+VERSION_AARCH64="diamond-auto-aarch64-18.04-20200827_1424"
VERSION_LOCAL_CYBER="local_cyber_dev"
CYBER_CONTAINER="apollo_cyber_${USER}"
CYBER_INSIDE="in_cyber_docker"
diff --git a/modules/canbus/proto/chassis.proto b/modules/canbus/proto/chassis.proto
index 0f40ea9..eec1fc1 100644
--- a/modules/canbus/proto/chassis.proto
+++ b/modules/canbus/proto/chassis.proto
@@ -112,4 +112,8 @@ message Chassis {
optional float bat_percentage = 37 [default = nan];
optional float front_encoder_angle = 38 [default = nan];
optional float rear_encoder_angle = 39 [default = nan];
+
+ optional double front_wheel_angle = 40 [default = nan];
+ optional double rear_wheel_angle = 41 [default = nan];
+
}
diff --git a/modules/canbus/tools/teleop.cc b/modules/canbus/tools/teleop.cc
index d19de77..7324bc1 100644
--- a/modules/canbus/tools/teleop.cc
+++ b/modules/canbus/tools/teleop.cc
@@ -7,8 +7,6 @@
*
* http://www.apache.org/licenses/LICENSE-2.0
*
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
@@ -59,7 +57,7 @@ using apollo::cyber::Writer;
const uint32_t KEYCODE_O = 0x4F; // '0'
-const uint32_t KEYCODE_UP1 = 0x57; // 'w'
+const uint32_t KEYCODE_UP1 = 0x57; // 'W'
const uint32_t KEYCODE_UP2 = 0x77; // 'w'
const uint32_t KEYCODE_DN1 = 0x53; // 'S'
const uint32_t KEYCODE_DN2 = 0x73; // 's'
@@ -67,8 +65,10 @@ const uint32_t KEYCODE_LF1 = 0x41; // 'A'
const uint32_t KEYCODE_LF2 = 0x61; // 'a'
const uint32_t KEYCODE_RT1 = 0x44; // 'D'
const uint32_t KEYCODE_RT2 = 0x64; // 'd'
+//const uint32_t KEYCODE_RT2 = 0x; // 'Z'
+//const uint32_t KEYCODE_RT2 = 0x; // 'z'
-const uint32_t KEYCODE_PKBK = 0x50; // hand brake or parking brake
+const uint32_t KEYCODE_PKBK = 0x50; //'P' hand brake or parking brake
// set throttle, gear, and brake
const uint32_t KEYCODE_SETT1 = 0x54; // 'T'
@@ -78,7 +78,6 @@ const uint32_t KEYCODE_SETG2 = 0x67; // 'g'
const uint32_t KEYCODE_SETB1 = 0x42; // 'B'
const uint32_t KEYCODE_SETB2 = 0x62; // 'b'
const uint32_t KEYCODE_ZERO = 0x30; // '0'
-
const uint32_t KEYCODE_SETQ1 = 0x51; // 'Q'
const uint32_t KEYCODE_SETQ2 = 0x71; // 'q'
@@ -112,7 +111,7 @@ class Teleop {
printf(" 2 GEAR_REVERSE\n");
printf(" 3 GEAR_PARKING\n");
printf(" 4 GEAR_LOW\n");
- printf(" 5 GEAR_INVALID\n");
+ printf(" 5 Gapollo.canbus.Chassis.SteeringSwitchEAR_INVALID\n");
printf(" 6 GEAR_NONE\n");
printf("\n-----------------------------------------------------------\n");
printf("Throttle/Speed up: [%c] | Set Throttle: [%c]+Num\n",
@@ -136,7 +135,8 @@ class Teleop {
double acc = 0;
double dec = 0;
double front_steering = 0;
- double rear_steering = 0;
+ //double front_steering_switch = 0;
+ //double rear_steering = 0;
struct termios cooked_;
struct termios raw_;
int32_t kfd_ = 0;
@@ -233,21 +233,24 @@ class Teleop {
case KEYCODE_LF2:
front_steering = control_command_.front_steering_switch();
front_steering = GetCommand(front_steering, FLAGS_steer_inc_delta);
- rear_steering = control_command_.rear_steering_switch();
- rear_steering = GetCommand(rear_steering, FLAGS_steer_inc_delta);
+ //control_command_.set_front_steering_switch(front_steering);
+ control_command_.set_front_steering_switch_pre(front_steering);
control_command_.set_front_steering_switch(Chassis::STEERINGNEGATIVE);
- // control_command_.set_rear_steering_switch(rear_steering);
AINFO << "Front Steering Target = " << front_steering;
+
break;
case KEYCODE_RT1: // right
case KEYCODE_RT2:
front_steering = control_command_.front_steering_switch();
front_steering = GetCommand(front_steering, -FLAGS_steer_inc_delta);
- rear_steering = control_command_.rear_steering_switch();
- rear_steering = GetCommand(rear_steering, -FLAGS_steer_inc_delta);
- control_command_.set_front_steering_switch(Chassis::STEERINGNEGATIVE);
- // control_command_.set_rear_steering_switch(rear_steering);
- AINFO << "rear Steering Target = " << rear_steering;
+ control_command_.set_front_steering_switch_pre(front_steering);
+ if(front_steering!=0){
+ control_command_.set_front_steering_switch(Chassis::STEERINGPOSITIVE);
+ }else{
+ control_command_.set_front_steering_switch(Chassis::STEERINGSTOP);
+ }
+ //control_command_.set_rear_steering_switch(Chassis::STEERINGPOSITIVE);
+ AINFO << "rear Steering Target = " << front_steering;
break;
case KEYCODE_PKBK: // hand brake
parking_brake = !control_command_.parking_brake();
@@ -289,7 +292,8 @@ class Teleop {
}
level = c - KEYCODE_ZERO;
control_command_.set_throttle(0.0);
- control_command_.set_brake(level * 10.0);
+ control_command_.set_brake(0.0);
+ //control_command_.set_brake(level * 10.0);
AINFO << "Throttle = " << control_command_.throttle()
<< ", Brake = " << control_command_.brake();
break;
@@ -408,12 +412,13 @@ class Teleop {
control_command_.set_brake(0.0);
control_command_.set_steering_rate(0.0);
control_command_.set_front_steering_switch(Chassis::STEERINGSTOP);
- control_command_.set_rear_steering_switch(Chassis::STEERINGSTOP);
+ //control_command_.set_rear_steering_switch(Chassis::STEERINGSTOP);
+ control_command_.set_front_steering_switch_pre(0.0);
control_command_.set_parking_brake(false);
control_command_.set_speed(0.0);
control_command_.set_acceleration(0.0);
control_command_.set_engine_on_off(false);
- // control_command_.set_driving_mode(Chassis::COMPLETE_MANUAL);
+ //control_command_.set_driving_mode(Chassis::COMPLETE_MANUAL);
control_command_.set_gear_location(Chassis::GEAR_INVALID);
control_command_.mutable_signal()->set_turn_signal(
VehicleSignal::TURN_NONE);
diff --git a/modules/canbus/vehicle/diamond/diamond_controller.cc b/modules/canbus/vehicle/diamond/diamond_controller.cc
index 0f5b7f9..a7fb3dc 100644
--- a/modules/canbus/vehicle/diamond/diamond_controller.cc
+++ b/modules/canbus/vehicle/diamond/diamond_controller.cc
@@ -19,6 +19,7 @@
#include "modules/common/proto/vehicle_signal.pb.h"
#include <stdio.h>
+#include <cmath>
#include <cstdio>
#include "cyber/common/log.h"
#include "modules/canbus/vehicle/diamond/diamond_message_manager.h"
@@ -26,6 +27,7 @@
#include "modules/common/time/time.h"
#include "modules/drivers/canbus/can_comm/can_sender.h"
#include "modules/drivers/canbus/can_comm/protocol_data.h"
+
namespace apollo {
namespace canbus {
namespace diamond {
@@ -98,8 +100,14 @@ ErrorCode DiamondController::Init(
AINFO << "DiamondController is initialized.";
// Initialize frequency converter
- device_frequency_converter.SetOpt(9600, 8, 'N',