• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision421 (tree)
Zeit2019-07-08 21:22:51
Autormizutani-f

Log Message

試走会用
暫定対応:StartToGoalLとStartToGoalRはトレースを反転しただけ。
       Garageは昨年のコードを右側トレースとしただけ。

Ändern Zusammenfassung

Diff

--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Garage.cpp (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Garage.cpp (revision 421)
@@ -18,6 +18,16 @@
1818 {
1919 }
2020
21+void Garage::updatePidValue() {
22+
23+ //const signed int LIGHT_WHITE2 = 48;
24+ //const signed int LIGHT_BLACK2 = 0;
25+
26+ // 黒と白の中間を走るPID制御を実施
27+ //PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
28+ turn = PIDMgr->GetTurnValue() * (-1); // 右側トレース
29+}
30+
2131 void Garage::SetPIDData()
2232 {
2333 // PID制御に使用する定数を設定する
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Garage.h (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Garage.h (revision 421)
@@ -24,7 +24,8 @@
2424 bool AngleCheck();
2525 int p_courseType;
2626 double GetDistance();
27-
27+ void updatePidValue();
28+
2829 private:
2930 enum GarageState
3031 {
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalL.cpp (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalL.cpp (revision 421)
@@ -14,61 +14,77 @@
1414 // 左右モータ総合回転角平均
1515 double totalAvg = (motor_ang_r + motor_ang_l) / 2;
1616 //forward = 20;
17-
18- if(totalAvg < 40) {
17+
18+ if(totalAvg < 40) {
1919 gyro_offset = -12;
2020 }
2121 else {
2222 gyro_offset = 0;
2323 }
24-
25- if(totalAvg < 2600) {
24+
25+ if(totalAvg < 700) {
2626 //スタートからA地点手前まで ストレート
2727 mode = FirstStraight;
2828 }
29- else if(totalAvg < 2800) {
29+ else if(totalAvg < 800) {
3030 // スピードダウン
3131 mode = SpeedDown;
3232 // 次モードのforward値
3333 minSpeed = 120;
3434 }
35- else if(totalAvg < 4200) {
35+ else if(totalAvg < 1400) {
3636 //A地点からB地点まで カーブ
37- mode = Curve;
37+ mode = Curve1;
3838 }
39- else if(totalAvg < 6400) {
39+ else if(totalAvg < 1900) {
4040 //B地点からC地点まで ストレート
41- mode = Straight;
41+ mode = FirstStraight;
4242 }
43- else if(totalAvg < 7100) {
44- //C地点からD地点まで カーブ
43+ else if(totalAvg < 2200) {
44+ // スピードダウン
45+ mode = SpeedDown;
46+ // 次モードのforward値
47+ minSpeed = 90;
48+ }
49+ else if(totalAvg < 5200) {
4550 mode = Curve2;
4651 }
47- // 要速度調整
48- else if(totalAvg < 7650) {
52+ else if(totalAvg < 6500)
53+ {
54+ mode = Curve4;
55+ }
56+ else if(totalAvg < 7100) {
4957 //D地点からE地点まで ストレート
50- mode = Curve2_3;
58+ mode = FirstStraight;
5159 }
52- // 要PID制御調整
53- else if(totalAvg < 8600) {
60+ else if(totalAvg < 7500) {
61+ // スピードダウン
62+ mode = SpeedDown;
63+ // 次モードのforward値
64+ minSpeed = 110;
65+ }
66+ else if(totalAvg < 8500) {
5467 //E地点からF地点まで カーブ
5568 mode = Curve3;
5669 }
57- else if(totalAvg < 11150) {
58- //F地点からG地点手前まで ストレート
59- mode = Straight;
70+ else if(totalAvg < 9000) {
71+
72+ mode = Curve2;
6073 }
61- else if(totalAvg < 11500) {
62- // スピードダウン
74+ else if(totalAvg < 10500) {
75+
76+ mode = Curve3;
77+ }
78+ else if(totalAvg < 11000) {
79+ //G地点からH地点まで カーブ
80+ mode = Curve2;
81+ }
82+ else if(totalAvg < 11200) {
6383 mode = SpeedDown;
6484 // 次モードのforward値
65- minSpeed = 120;
85+ minSpeed = 30;
6686 }
67- // 要PID制御調整
68- else if(totalAvg < 12000) {
69- //G地点からH地点まで カーブ
70- mode = Curve4;
71- }
87+
7288 else {
7389 // ゴール通過後 次の走行クラス(難所)へ切り替え
7490 return true;
@@ -78,20 +94,20 @@
7894 int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
7995 int touchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
8096 //if(touchSensor == 1){
81- log("%f, %d, %f, %d, %d, %d, %f\r\n", totalAvg, mode, forward, colorSensor, volt, touchSensor, turn);
82- //}
97+ log("%f, %d, %f, %d, %d, %d\r\n", totalAvg, mode, forward, colorSensor, volt, touchSensor);
98+ //}
8399 return false;
84100 }
85101
86102 void StartToGoalL::updatePidValue() {
87103
88- const signed int LIGHT_GRAY2 = 25;
89- const signed int LIGHT_WHITE2 = 54;
104+ const signed int LIGHT_GRAY2 = 30;
105+ const signed int LIGHT_WHITE2 = 48;
90106 const signed int LIGHT_BLACK2 = 0;
91107
92108 switch (mode)
93109 {
94- case SpeedDown2:
110+ case Curve4:
95111 // グレーと白の中間を走るPID制御を実施
96112 PIDMgr->SetColor(LIGHT_GRAY2, LIGHT_WHITE2);
97113 turn = PIDMgr->GetTurnValue();
@@ -109,24 +125,28 @@
109125 /*PID制御処理呼び出し
110126 modeによる適正値を設定し、PID制御呼び出し*/
111127 void StartToGoalL::SetPIDData() {
112- if(mode == Straight || mode == FirstStraight || mode == SpeedDown || mode == Curve2_3) {
113- PIDMgr->SetFixedData(0.5, 0, 0.03);
128+ if(mode == Straight || mode == SpeedDown || mode == FirstStraight || mode == Straight2) {
129+ PIDMgr->SetFixedData(0.4, 0, 0.03);
114130 }
115- else if(mode == SpeedDown2) {
116- PIDMgr->SetFixedData(1, 0, 0.03);
131+ else if(mode == Curve1){
132+ //forward = 120
133+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
117134 }
118- else if(mode == Curve){
119- PIDMgr->SetFixedData(0.5, 0.1, 0.03);
120- }
121135 else if(mode == Curve2){
122- PIDMgr->SetFixedData(1, 0.8, 0.06);
136+ //forward = 90
137+ PIDMgr->SetFixedData(0.9, 0.6, 0.05);
138+ }
139+ else if(mode == Curve3){
140+ //forward = 110
141+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
123142 }
124- else if(mode == Curve3 || mode == Curve4){
125- PIDMgr->SetFixedData(1.25, 1, 0.09);
143+ else if(mode == Curve4){
144+ //forward = 100
145+ PIDMgr->SetFixedData(0.9, 0.55, 0.06);
126146 }
127- else{
128- PIDMgr->SetFixedData(1, 0.3, 0.06);
129- }
147+ else if(mode == AfterGoal){
148+ PIDMgr->SetFixedData(1.2, 0.6, 0.05);
149+ }
130150 }
131151
132152 /*forward値設定
@@ -139,25 +159,28 @@
139159 else if(mode == Straight) {
140160 SpeedUp(STRAIGHT_SPEED);
141161 }
162+ else if(mode == Straight2) {
163+ SpeedUp(120);
164+ }
165+ else if(mode == Straight3) {
166+ SpeedUp(110);
167+ }
142168 else if(mode == SpeedDown) {
143169 BaseCourse::SpeedDown(minSpeed);
144170 }
145- else if(mode == SpeedDown2) {
146- BaseCourse::SpeedDown(minSpeed);
171+ else if(mode == Curve1) {
172+ forward = 120;
147173 }
148- else if(mode == Curve) {
149- forward = 150;
150- }
151174 else if(mode == Curve2) {
152- forward = 120;
175+ forward = 90;
153176 }
154- else if(mode == Curve2_3){
155- forward = 100;
156- }
157177 else if(mode == Curve3) {
158178 forward = 110;
159179 }
160180 else if(mode == Curve4) {
181+ forward = 100;
182+ }
183+ else if(mode == AfterGoal) {
161184 forward = 30;
162185 }
163186 }
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalL.h (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalL.h (revision 421)
@@ -18,13 +18,14 @@
1818 enum RunMode {
1919 FirstStraight,
2020 Straight,
21+ Straight2,
22+ Straight3,
2123 SpeedDown,
22- SpeedDown2,
23- Curve,
24+ Curve1,
2425 Curve2,
25- Curve2_3,
2626 Curve3,
27- Curve4
27+ Curve4,
28+ AfterGoal
2829 };
2930 };
3031
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalR.cpp (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalR.cpp (revision 421)
@@ -13,7 +13,7 @@
1313 {
1414 // 左右モータ総合回転角平均
1515 double totalAvg = (motor_ang_r + WK + motor_ang_l + WK) / 2;
16-// forward = 30;
16+ //forward = 30;
1717
1818 if(totalAvg < 40) {
1919 gyro_offset = -12;
@@ -22,50 +22,68 @@
2222 gyro_offset = 0;
2323 }
2424
25- if(totalAvg < 2100) {
25+ if(totalAvg < 700) {
2626 //スタートからA地点手前まで ストレート
2727 mode = FirstStraight;
2828 }
29- else if(totalAvg < 2400) {
29+ else if(totalAvg < 800) {
3030 // スピードダウン
3131 mode = SpeedDown;
3232 // 次モードのforward値
33- minSpeed = 90;
33+ minSpeed = 120;
3434 }
35- else if(totalAvg < 4100) {
35+ else if(totalAvg < 1400) {
3636 //A地点からB地点まで カーブ
37- mode = FirstCurve;
37+ mode = Curve1;
3838 }
39- else if(totalAvg < 4800) {
39+ else if(totalAvg < 1900) {
4040 //B地点からC地点まで ストレート
41- mode = Straight2;
41+ mode = FirstStraight;
4242 }
43- else if(totalAvg < 6100) {
44- //C地点からD地点まで カーブ
45- mode = SecondCurve;
43+ else if(totalAvg < 2200) {
44+ // スピードダウン
45+ mode = SpeedDown;
46+ // 次モードのforward値
47+ minSpeed = 90;
4648 }
47- else if(totalAvg < 7000) {
49+ else if(totalAvg < 5200) {
50+ mode = Curve2;
51+ }
52+ else if(totalAvg < 6500)
53+ {
54+ mode = Curve4;
55+ }
56+ else if(totalAvg < 7100) {
4857 //D地点からE地点まで ストレート
49- mode = Straight2;
58+ mode = FirstStraight;
5059 }
51- else if(totalAvg < 8300) {
60+ else if(totalAvg < 7500) {
61+ // スピードダウン
62+ mode = SpeedDown;
63+ // 次モードのforward値
64+ minSpeed = 110;
65+ }
66+ else if(totalAvg < 8500) {
5267 //E地点からF地点まで カーブ
53- mode = ThirdCurve;
68+ mode = Curve3;
5469 }
55- else if(totalAvg < 10800) {
56- //F地点からG地点手前まで ストレート
57- mode = Straight;
70+ else if(totalAvg < 9000) {
71+
72+ mode = Curve2;
5873 }
59- else if(totalAvg < 11900) {
60- // スピードダウン
74+ else if(totalAvg < 10500) {
75+
76+ mode = Curve3;
77+ }
78+ else if(totalAvg < 11000) {
79+ //G地点からH地点まで カーブ
80+ mode = Curve2;
81+ }
82+ else if(totalAvg < 11200) {
6183 mode = SpeedDown;
6284 // 次モードのforward値
6385 minSpeed = 30;
6486 }
65- else if(totalAvg < 12200) {
66- //G地点からH地点まで カーブ
67- mode = AfterGoal;
68- }
6987
7088 else {
7189 // ゴール通過後 次の走行クラス(難所)へ切り替え
@@ -92,12 +110,12 @@
92110 case AfterGoal:
93111 // グレーと白の中間を走るPID制御を実施
94112 PIDMgr->SetColor(LIGHT_GRAY2, LIGHT_WHITE2);
95- turn = PIDMgr->GetTurnValue();
113+ turn = PIDMgr->GetTurnValue() * (-1) ;
96114 break;
97115 default:
98116 // 黒と白の中間を走るPID制御を実施
99117 PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
100- turn = PIDMgr->GetTurnValue();
118+ turn = PIDMgr->GetTurnValue() * (-1);
101119 break;
102120
103121 }
@@ -109,16 +127,22 @@
109127 if(mode == Straight || mode == SpeedDown || mode == FirstStraight || mode == Straight2) {
110128 PIDMgr->SetFixedData(0.4, 0, 0.03);
111129 }
112- else if(mode == FirstCurve){
113- //PIDMgr->SetFixedData(1.3, 1, 0.05);
114- PIDMgr->SetFixedData(0.9, 0.5, 0.05);
130+ else if(mode == Curve1){
131+ //forward = 120
132+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
115133 }
116- else if(mode == SecondCurve){
117- PIDMgr->SetFixedData(1.2, 0.6, 0.05);
134+ else if(mode == Curve2){
135+ //forward = 90
136+ PIDMgr->SetFixedData(0.9, 0.6, 0.05);
118137 }
119- else if(mode == ThirdCurve){
138+ else if(mode == Curve3){
139+ //forward = 110
120140 PIDMgr->SetFixedData(0.9, 0.5, 0.07);
121141 }
142+ else if(mode == Curve4){
143+ //forward = 100
144+ PIDMgr->SetFixedData(0.9, 0.55, 0.06);
145+ }
122146 else if(mode == AfterGoal){
123147 PIDMgr->SetFixedData(1.2, 0.6, 0.05);
124148 }
@@ -134,20 +158,26 @@
134158 SpeedUp(STRAIGHT_SPEED);
135159 }
136160 else if(mode == Straight2) {
137- forward = 120;
161+ SpeedUp(120);
138162 }
163+ else if(mode == Straight3) {
164+ SpeedUp(110);
165+ }
139166 else if(mode == SpeedDown) {
140167 BaseCourse::SpeedDown(minSpeed);
141168 }
142- else if(mode == FirstCurve) {
143- forward = 125;//110
169+ else if(mode == Curve1) {
170+ forward = 120;
144171 }
145- else if(mode == SecondCurve) {
146- forward = 110;//90
172+ else if(mode == Curve2) {
173+ forward = 90;
147174 }
148- else if(mode == ThirdCurve) {
149- forward = 120;
175+ else if(mode == Curve3) {
176+ forward = 110;
150177 }
178+ else if(mode == Curve4) {
179+ forward = 100;
180+ }
151181 else if(mode == AfterGoal) {
152182 forward = 30;
153183 }
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalR.h (revision 420)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/StartToGoalR.h (revision 421)
@@ -20,10 +20,12 @@
2020 FirstStraight,
2121 Straight,
2222 Straight2,
23+ Straight3,
2324 SpeedDown,
24- FirstCurve,
25- SecondCurve,
26- ThirdCurve,
25+ Curve1,
26+ Curve2,
27+ Curve3,
28+ Curve4,
2729 AfterGoal
2830 };
2931 };
Show on old repository browser