• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision426 (tree)
Zeit2019-09-14 15:06:28
Autory04136

Log Message

(empty log message)

Ändern Zusammenfassung

Diff

--- branches/src/hrp2/workspace/yamashita/StartToGoalR.cpp (nonexistent)
+++ branches/src/hrp2/workspace/yamashita/StartToGoalR.cpp (revision 426)
@@ -0,0 +1,321 @@
1+#include "StartToGoalR.h"
2+
3+StartToGoalR::StartToGoalR() {
4+ mode = Straight;
5+}
6+
7+StartToGoalR::~StartToGoalR() {
8+}
9+
10+/*次モードへの移行
11+ 左右モータの総回転角からモードを走行モードを切り替える*/
12+bool StartToGoalR::IsNextCourse()
13+{
14+ // 左右モータ総合回転角平均
15+ double totalAvg = (motor_ang_r + WK + motor_ang_l + WK) / 2;
16+ //forward = 30;
17+
18+ if(totalAvg < 40) {
19+ gyro_offset = -13;
20+ }
21+ else {
22+ gyro_offset = 0;
23+ }
24+
25+
26+ if(totalAvg < 700) {
27+ //スタートからA地点手前まで ストレート
28+ mode = FirstStraight;
29+ }
30+ else if(totalAvg < 800) {
31+ // スピードダウン
32+ mode = SpeedDown;
33+ // 次モードのforward値
34+ minSpeed = 120;
35+ }
36+ else if(totalAvg < 1400) {
37+ //A地点からB地点まで カーブ
38+ mode = Curve1;
39+ }
40+ else if(totalAvg < 1900) {
41+ //B地点からC地点まで ストレート
42+ mode = FirstStraight;
43+ }
44+ else if(totalAvg < 2200) {
45+ // スピードダウン
46+ mode = SpeedDown;
47+ // 次モードのforward値
48+ minSpeed = 90;
49+ }
50+ else if(totalAvg < 5200) {
51+ mode = Curve2;
52+ }
53+ else if(totalAvg < 6100)
54+ {
55+ mode = Curve4;
56+ }
57+ else if(totalAvg < 7100) {
58+ //D地点からE地点まで ストレート
59+ mode = FirstStraight;
60+ }
61+ else if(totalAvg < 7500) {
62+ // スピードダウン
63+ mode = SpeedDown;
64+ // 次モードのforward値
65+ minSpeed = 110;
66+ }
67+ else if(totalAvg < 8500) {
68+ //E地点からF地点まで カーブ
69+ mode = Curve3;
70+ }
71+ else if(totalAvg < 9000) {
72+
73+ mode = Curve2;
74+ }
75+ else if(totalAvg < 10500) {
76+
77+ mode = Curve3;
78+ }
79+ else if(totalAvg < 11000) {
80+ //G地点からH地点まで カーブ
81+ mode = Curve2;
82+ }
83+ else if(totalAvg < 11200) {
84+ mode = SpeedDown;
85+ // 次モードのforward値
86+ minSpeed = 30;
87+ }
88+
89+
90+
91+
92+// if(totalAvg < 700) {
93+// //スタートからA地点手前まで ストレート
94+// mode = FirstStraight;
95+// }
96+// else if(totalAvg < 800) {
97+// // スピードダウン
98+// mode = SpeedDown;
99+// // 次モードのforward値
100+// minSpeed = 120;
101+// }
102+// else if(totalAvg < 1400) {
103+// //A地点からB地点まで カーブ
104+// mode = Curve1;
105+// }
106+// else if(totalAvg < 1900) {
107+// //B地点からC地点まで ストレート
108+// mode = FirstStraight;
109+// }
110+// else if(totalAvg < 2000) {
111+// // スピードダウン
112+// mode = SpeedDown;
113+// // 次モードのforward値
114+// minSpeed = 110;
115+// }
116+// else if(totalAvg < 3800) {
117+// mode = Curve1;
118+// }
119+// else if(totalAvg < 5200) {
120+// mode = Curve2;
121+// }
122+// else if(totalAvg < 6300)
123+// {
124+// mode = Curve4;
125+// }
126+// else if(totalAvg < 7100) {
127+// //I地点からJ地点まで ストレート
128+// mode = FirstStraight;
129+// }
130+// else if(totalAvg < 7500) {
131+// // スピードダウン
132+// mode = SpeedDown;
133+// // 次モードのforward値
134+// minSpeed = 110;
135+// }
136+// else if(totalAvg < 8500) {
137+// //E地点からF地点まで カーブ
138+// mode = Curve3;
139+// }
140+// else if(totalAvg < 9000) {
141+//
142+// mode = Curve2;
143+// }
144+// else if(totalAvg < 10500) {
145+//
146+// mode = Curve3;
147+// }
148+// else if(totalAvg < 11000) {
149+// //G地点からH地点まで カーブ
150+// mode = Curve2;
151+// }
152+// else if(totalAvg < 11200) {
153+// mode = SpeedDown;
154+// // 次モードのforward値
155+// minSpeed = 30;
156+// }
157+
158+
159+
160+// if(totalAvg < 700) {
161+// //スタートからA地点手前まで ストレート
162+// mode = FirstStraight;
163+// }
164+// else if(totalAvg < 800) {
165+// // スピードダウン
166+// mode = SpeedDown;
167+// // 次モードのforward値
168+// minSpeed = 120;
169+// }
170+// else if(totalAvg < 1400) {
171+// //A地点からB地点まで カーブ
172+// mode = Curve1;
173+// }
174+// else if(totalAvg < 1900) {
175+// //B地点からC地点まで ストレート
176+// mode = FirstStraight;
177+// }
178+// else if(totalAvg < 2200) {
179+// // スピードダウン
180+// mode = SpeedDown;
181+// // 次モードのforward値
182+// minSpeed = 90;
183+// }
184+// else if(totalAvg < 5200) {
185+// mode = Curve2;
186+// }
187+// else if(totalAvg < 6500)
188+// {
189+// mode = Curve4;
190+// }
191+// else if(totalAvg < 7100) {
192+// //D地点からE地点まで ストレート
193+// mode = FirstStraight;
194+// }
195+// else if(totalAvg < 7500) {
196+// // スピードダウン
197+// mode = SpeedDown;
198+// // 次モードのforward値
199+// minSpeed = 110;
200+// }
201+// else if(totalAvg < 8500) {
202+// //E地点からF地点まで カーブ
203+// mode = Curve3;
204+// }
205+// else if(totalAvg < 9000) {
206+//
207+// mode = Curve2;
208+// }
209+// else if(totalAvg < 10500) {
210+//
211+// mode = Curve3;
212+// }
213+// else if(totalAvg < 11000) {
214+// //G地点からH地点まで カーブ
215+// mode = Curve2;
216+// }
217+// else if(totalAvg < 11200) {
218+// mode = SpeedDown;
219+// // 次モードのforward値
220+// minSpeed = 30;
221+// }
222+
223+ else {
224+ // ゴール通過後 次の走行クラス(難所)へ切り替え
225+ return true;
226+ }
227+
228+ // ログ出力 コメントアウト
229+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
230+ int touchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
231+// if(touchSensor == 1){
232+ log("%f, %d, %f, %d, %d, %d\r\n", totalAvg, mode, forward, colorSensor, volt, touchSensor);
233+// }
234+ return false;
235+}
236+
237+void StartToGoalR::updatePidValue() {
238+
239+ const signed int LIGHT_GRAY2 = 30;
240+ const signed int LIGHT_WHITE2 = 48;
241+ const signed int LIGHT_BLACK2 = 0;
242+
243+ switch (mode)
244+ {
245+ case AfterGoal:
246+ // グレーと白の中間を走るPID制御を実施
247+ PIDMgr->SetColor(LIGHT_GRAY2, LIGHT_WHITE2);
248+ turn = PIDMgr->GetTurnValue() * (-1) ;
249+ break;
250+ default:
251+ // 黒と白の中間を走るPID制御を実施
252+ PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
253+ turn = PIDMgr->GetTurnValue() * (-1);
254+ break;
255+
256+ }
257+}
258+
259+/*PID制御処理呼び出し
260+ modeによる適正値を設定し、PID制御呼び出し*/
261+void StartToGoalR::SetPIDData() {
262+ if(mode == Straight || mode == SpeedDown || mode == FirstStraight || mode == Straight2) {
263+ PIDMgr->SetFixedData(0.4, 0, 0.03);
264+ }
265+ else if(mode == Curve1){
266+ //forward = 120
267+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
268+ }
269+ else if(mode == Curve2){
270+ //forward = 90
271+// PIDMgr->SetFixedData(0.5, 0.3, 0.05);
272+ PIDMgr->SetFixedData(0.9, 0.6, 0.05);
273+ }
274+ else if(mode == Curve3){
275+ //forward = 110
276+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
277+ }
278+ else if(mode == Curve4){
279+ //forward = 100
280+ PIDMgr->SetFixedData(0.9, 0.4, 0.075);
281+ }
282+ else if(mode == AfterGoal){
283+ PIDMgr->SetFixedData(1.2, 0.6, 0.05);
284+ }
285+}
286+
287+/*forward値設定
288+ モードからforward値を設定*/
289+void StartToGoalR::setForward() {
290+ if (mode == FirstStraight) {
291+ SpeedUp(FIRST_STRAIGHT_SPEED);
292+ }
293+ else if(mode == Straight) {
294+ SpeedUp(STRAIGHT_SPEED);
295+ }
296+ else if(mode == Straight2) {
297+ SpeedUp(120);
298+ }
299+ else if(mode == Straight3) {
300+ SpeedUp(110);
301+ }
302+ else if(mode == SpeedDown) {
303+ BaseCourse::SpeedDown(minSpeed);
304+ }
305+ else if(mode == Curve1) {
306+ forward = 120;
307+ }
308+ else if(mode == Curve2) {
309+ forward = 100;
310+ }
311+ else if(mode == Curve3) {
312+ forward = 120;
313+ }
314+ else if(mode == Curve4) {
315+ forward = 100;
316+ }
317+ else if(mode == AfterGoal) {
318+ forward = 30;
319+ }
320+}
321+
Show on old repository browser