• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision404 (tree)
Time2018-09-01 17:28:20
Authorncxxsuzuki

Log Message

(empty log message)

Change Summary

Incremental Difference

--- trunk/src/hrp2_beta7/sdk/workspace/20180901kitoh_bk/LookupGate.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/20180901kitoh_bk/LookupGate.cpp (revision 404)
@@ -0,0 +1,660 @@
1+/*
2+ * File: LookupGate.cpp
3+ * Author: h.kitoh
4+ *
5+ * Created on 2016/08/14, 10:39
6+ */
7+
8+#include "LookupGate.h"
9+#include "GrayChecker.h"
10+
11+#include "CommonApp.h"
12+
13+LookupGate::LookupGate():lookupGateState(LookupGateStateBeforeSonorDetected), runMode(normalRun) {
14+ grayChecker = new GrayChecker();
15+}
16+
17+LookupGate::~LookupGate() {
18+}
19+const signed int standTailAngle = 84;
20+const signed int interval1stTailAngle = 78;
21+const signed int interval2ndTailAngle = 72;
22+//const signed int interval3ndTailAngle = 68;
23+const signed int throughGateTailAngle = 66;
24+
25+void LookupGate::Run() {
26+ sonar_alert();
27+ switch(lookupGateState) {
28+ case LookupGateStateRun:
29+ BaseCourse::Run();
30+ // 少し倒立振子走行してから尻尾を下す
31+ if (200 < motor_ang_l) {
32+ if (stopper.DoStop()) {
33+ lookupGateState = LookupGateStateBeforeSpin;
34+ log("To BeforeSpin\r\n");
35+ }
36+ }
37+ break;
38+ case LookupGateStateBeforeSonorDetected:
39+ PIDMgr->SetColor(20, 40);
40+ runUntilSonorDetected();
41+ break;
42+ case LookupGateStateAfterSonorDetected:
43+ runSonorDetected();
44+ break;
45+ case LookupGateStateThroughGateReady1st:
46+ throughGateReady1st();
47+ break;
48+ case LookupGateStateThroughGateReady2nd:
49+ tail_control(standTailAngle, 100);
50+ throughGateReady2nd();
51+ break;
52+ case LookupGateStateThroughGateReady3rd:
53+ throughGateReady3rd();
54+ break;
55+ case LookupGateStateThroughGateReady4th:
56+ tail_control(interval1stTailAngle, 100);
57+ throughGateReady4th();
58+ break;
59+ case LookupGateStateThroughGateReady5th:
60+ throughGateReady5th();
61+ break;
62+ case LookupGateStateThroughGateReady6th:
63+ tail_control(interval2ndTailAngle, 100);
64+ throughGateReady6th();
65+ break;
66+ case LookupGateStateThroughGateReady7th:
67+ throughGateReady7th();
68+ break;
69+ case LookupGateStateThroughGateReady8th:
70+ tail_control(throughGateTailAngle, 100);
71+ throughGateReady8th();
72+ break;
73+ case LookupGateStateThroughGateForward:
74+ tail_control(throughGateTailAngle, 100);
75+ ForwardRun();
76+ break;
77+ case LookupGateStateUpridhtBody1st:
78+ UpridhtBody1st();
79+ break;
80+ case LookupGateStateUpridhtBody2nd:
81+ tail_control(interval2ndTailAngle, 200);
82+ UpridhtBody2nd();
83+ break;
84+ case LookupGateStateUpridhtBody3rd:
85+ UpridhtBody3rd();
86+ break;
87+ case LookupGateStateUpridhtBody4th:
88+ tail_control(interval1stTailAngle, 200);
89+ UpridhtBody4th();
90+ break;
91+ case LookupGateStateUpridhtBody5th:
92+ tail_control(standTailAngle, 200);
93+ UpridhtBody5th();
94+ break;
95+ case LookupGateStateBeforeSpin:
96+// tail_control(standTailAngle, 100);
97+ tail_control(throughGateTailAngle, 100);
98+ // 0.5秒止まる
99+ if(125 < ++waitCount){
100+ waitCount = 0;
101+ lookupGateState = LookupGateStateSpinRightWheel;
102+ log("To SpinRight\r\n");
103+ }
104+ break;
105+ case LookupGateStateSpinRightWheel:
106+// tail_control(standTailAngle, 100);
107+ tail_control(throughGateTailAngle, 100);
108+ // 右ホイールを前進させ0.5秒止まる
109+ if (waitCount == 0) {
110+ if (SpinRight()) {
111+ ++waitCount;
112+ }
113+ } else if(125 < ++waitCount) {
114+ waitCount = 0;
115+ lookupGateState = LookupGateStateSpinLeftWheel;
116+ log("To SpinLeft\r\n");
117+ }
118+ break;
119+ case LookupGateStateSpinLeftWheel:
120+// tail_control(standTailAngle, 100);
121+ tail_control(throughGateTailAngle, 100);
122+ // 左ホイールを後退させ0.5秒止まる
123+ if (waitCount == 0) {
124+ if (SpinLeft()) {
125+ ++waitCount;
126+ }
127+ }
128+ else if (125 < ++waitCount) {
129+ waitCount = 0;
130+ lookupGateState = LookupGateStateAfterSpin;
131+ log("To SpinLeft\r\n");
132+ }
133+ break;
134+ case LookupGateStateAfterSpin:
135+ // ライントレース(84度想定)
136+// tail_control(standTailAngle, 100);
137+ tail_control(throughGateTailAngle, 100);
138+ LineTraceWithTail();
139+ if (125 < ++waitCount) {
140+ waitCount = 0;
141+ lookupGateState = LookupGateStateThroughGateForwardAjustment;
142+ log("To SpinRight\r\n");
143+ log("To LookupGateStateThroughGateForwardAjustment\r\n");
144+ }
145+ break;
146+ case LookupGateStateThroughGateForwardAjustment:
147+// runMode = run84;
148+ runMode = run66;
149+ tail_control(standTailAngle, 100);
150+ ForwardRunAjustment();
151+ break;
152+ case LookupGateStateBodyDown1st:
153+ runMode = run66;
154+ BaseCourse::Run();
155+ if (tail_control(interval1stTailAngle, 100)) {
156+ log("LookupGateStateBodyDown2nd\r\n");
157+ lookupGateState = LookupGateStateBodyDown2nd;
158+ }
159+ break;
160+ case LookupGateStateBodyDown2nd:
161+ tail_control(interval1stTailAngle, 100);
162+ runMode = run66;
163+ BaseCourse::Run();
164+ if (++counter >= 100) {
165+ log("LookupGateStateBodyDown3rd\r\n");
166+ lookupGateState = LookupGateStateBodyDown3rd;
167+ counter = 0;
168+ }
169+ break;
170+ case LookupGateStateBodyDown3rd:
171+ runMode = run66;
172+ BaseCourse::Run();
173+ if (tail_control(interval2ndTailAngle, 100)) {
174+ log("LookupGateStateBodyDown4th\r\n");
175+ lookupGateState = LookupGateStateBodyDown4th;
176+ }
177+ break;
178+ case LookupGateStateBodyDown4th:
179+ tail_control(interval2ndTailAngle, 100);
180+ runMode = run66;
181+ BaseCourse::Run();
182+ if (++counter >= 100) {
183+ log("LookupGateStateBodyDown5th\r\n");
184+ lookupGateState = LookupGateStateBodyDown5th;
185+ counter = 0;
186+ }
187+ break;
188+ case LookupGateStateBodyDown5th:
189+ runMode = run66;
190+ BaseCourse::Run();
191+ if (tail_control(throughGateTailAngle, 100)) {
192+ log("LookupGateStateBodyDown6th\r\n");
193+ lookupGateState = LookupGateStateBodyDown6th;
194+ }
195+ break;
196+ case LookupGateStateBodyDown6th:
197+ tail_control(throughGateTailAngle, 100);
198+ runMode = run66;
199+ BaseCourse::Run();
200+ if (++counter >= 300) {
201+ forwardMotorAngL = 0;
202+ log("LookupGateStateThroughGateForward\r\n");
203+ lookupGateState = LookupGateStateThroughGateForward;
204+ counter = 0;
205+ }
206+ break;
207+ case LookupGateStateThroughGateStop:
208+ tail_control(throughGateTailAngle, 100);
209+ RunStop();
210+ break;
211+ case LookupGateStateGraySerchRun:
212+ tail_control(standTailAngle, 100);
213+ GraySerchRun();
214+ break;
215+ case LookupGateStateChangeGarage:
216+ tail_control(standTailAngle, 100);
217+ ChangeGarage();
218+ break;
219+ }
220+}
221+
222+void LookupGate::setForward()
223+{
224+ forward = 0;
225+ switch(lookupGateState)
226+ {
227+ case LookupGateStateRun:
228+ forward = 20;
229+ break;
230+ case LookupGateStateBeforeSpin:
231+ case LookupGateStateSpinRightWheel:
232+ case LookupGateStateSpinLeftWheel:
233+ case LookupGateStateAfterSpin:
234+ break;
235+ case LookupGateStateBeforeSonorDetected:
236+ forward = 30;
237+ break;
238+ case LookupGateStateAfterSonorDetected:
239+ case LookupGateStateThroughGateReady1st:
240+ break;
241+ case LookupGateStateThroughGateReady2nd:
242+ forward = 5;
243+ break;
244+ case LookupGateStateThroughGateReady3rd:
245+ forward = 2;
246+ break;
247+ case LookupGateStateThroughGateReady4th:
248+ case LookupGateStateThroughGateReady5th:
249+ case LookupGateStateThroughGateReady6th:
250+ case LookupGateStateThroughGateReady7th:
251+ case LookupGateStateThroughGateReady8th:
252+ break;
253+ case LookupGateStateThroughGateForward:
254+ forward = 12;
255+ break;
256+ case LookupGateStateThroughGateForwardAjustment:
257+ forward = 8;
258+ break;
259+ case LookupGateStateBodyDown1st:
260+ case LookupGateStateBodyDown2nd:
261+ case LookupGateStateBodyDown3rd:
262+ case LookupGateStateBodyDown4th:
263+ case LookupGateStateBodyDown5th:
264+ case LookupGateStateBodyDown6th:
265+ case LookupGateStateThroughGateStop:
266+ break;
267+ case LookupGateStateUpridhtBody1st:
268+ forward = -2;
269+ break;
270+ case LookupGateStateUpridhtBody2nd:
271+ break;
272+ case LookupGateStateUpridhtBody3rd:
273+ forward = -2;
274+ break;
275+ case LookupGateStateUpridhtBody4th:
276+ break;
277+ case LookupGateStateUpridhtBody5th:
278+ forward = -2;
279+ break;
280+ case LookupGateStateGraySerchRun:
281+ forward = 10;
282+ break;
283+ case LookupGateStateChangeGarage:
284+ forward = 10;
285+ break;
286+ }
287+}
288+
289+void LookupGate::runUntilSonorDetected()
290+{
291+ runMode = normalRun;
292+ BaseCourse::Run();
293+ if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0))
294+ {
295+ log("LookupGateStateAfterSonorDetected\r\n");
296+ lookupGateState = LookupGateStateAfterSonorDetected;
297+ counter = 0;
298+ }
299+}
300+
301+void LookupGate::runSonorDetected() {
302+ runMode = normalRun;
303+ BaseCourse::Run();
304+ if (++counter >= 250) {
305+ log("LookupGateStateThroughGateReady1st\r\n");
306+ lookupGateState = LookupGateStateThroughGateReady1st;
307+ counter = 0;
308+ }
309+}
310+
311+void LookupGate::throughGateReady1st() {
312+ runMode = normalRun;
313+ BaseCourse::Run();
314+ if (tail_control(standTailAngle, 100)) {
315+ log("LookupGateStateThroughGateReady2nd\r\n");
316+ lookupGateState = LookupGateStateThroughGateReady2nd;
317+ }
318+}
319+
320+void LookupGate::throughGateReady2nd() {
321+ runMode = run66;
322+ BaseCourse::Run();
323+ if (++counter >= 250) {
324+ log("LookupGateStateThroughGateReady3rd\r\n");
325+ lookupGateState = LookupGateStateThroughGateReady3rd;
326+ counter = 0;
327+ }
328+}
329+
330+void LookupGate::throughGateReady3rd() {
331+ runMode = run66;
332+ BaseCourse::Run();
333+ if(tail_control(interval1stTailAngle, 100)) {
334+ log("LookupGateStateThroughGateReady4th\r\n");
335+ lookupGateState = LookupGateStateThroughGateReady4th;
336+ }
337+}
338+
339+void LookupGate::throughGateReady4th() {
340+ runMode = run66;
341+ BaseCourse::Run();
342+ if (++counter >= 200) {
343+ log("LookupGateStateThroughGateReady5th\r\n");
344+ lookupGateState = LookupGateStateThroughGateReady5th;
345+ counter = 0;
346+ }
347+}
348+
349+void LookupGate::throughGateReady5th() {
350+ runMode = run66;
351+ BaseCourse::Run();
352+ if (tail_control(interval2ndTailAngle, 100)) {
353+ log("LookupGateStateThroughGateReady6th\r\n");
354+ lookupGateState = LookupGateStateThroughGateReady6th;
355+ }
356+}
357+
358+void LookupGate::throughGateReady6th()
359+{
360+ runMode = run66;
361+ BaseCourse::Run();
362+ if (++counter >= 200) {
363+ log("LookupGateStateThroughGateReady7th\r\n");
364+ lookupGateState = LookupGateStateThroughGateReady7th;
365+ counter = 0;
366+ }
367+}
368+
369+void LookupGate::throughGateReady7th() {
370+ runMode = run66;
371+ BaseCourse::Run();
372+ if(tail_control(throughGateTailAngle, 100)) {
373+ log("LookupGateStateThroughGateReady8th\r\n");
374+ lookupGateState = LookupGateStateThroughGateReady8th;
375+ }
376+}
377+
378+void LookupGate::throughGateReady8th() {
379+ runMode = run66;
380+ BaseCourse::Run();
381+ if (++counter >= 400) {
382+ forwardMotorAngL = 0;
383+ log("LookupGateStateThroughGateForward\r\n");
384+ lookupGateState = LookupGateStateThroughGateForward;
385+ counter = 0;
386+ }
387+}
388+
389+void LookupGate::ForwardRunAjustment() {
390+// runMode = run84;
391+ runMode = run66;
392+ BaseCourse::Run();
393+// if ((distance <= 10) && (distance >= 0)) {
394+ log("DistanseDiff : %d\r\n", preDistanse - distance);
395+ if (preDistanse != 0 && preDistanse - distance > 200) {
396+// log("LookupGateStateBodyDown1st\r\n");
397+// lookupGateState = LookupGateStateBodyDown1st;
398+ log("To LookupGateStateBodyDown6th\r\n");
399+ lookupGateState = LookupGateStateBodyDown6th;
400+ }
401+ preDistanse = distance;
402+}
403+
404+void LookupGate::ForwardRun() {
405+ runMode = run66;
406+ BaseCourse::Run();
407+ if (forwardMotorAngL == 0) {
408+ if ((distance >= 30) || (distance < 0)) {
409+ forwardMotorAngL = motor_ang_l;
410+ }
411+ } else {
412+ if (210 < motor_ang_l - forwardMotorAngL) {
413+ log("To LookupGateStateThroughGateStop\r\n");
414+ lookupGateState = LookupGateStateThroughGateStop;
415+ }
416+ }
417+}
418+
419+void LookupGate::RunStop() {
420+ runMode = run66;
421+ BaseCourse::Run();
422+ if (++counter >= 100) {
423+// log("LookupGateStateUpridhtBody1st\r\n");
424+// lookupGateState = LookupGateStateUpridhtBody1st;
425+ log(" To LookupGateStateUpridhtBody5th;\r\n");
426+ lookupGateState = LookupGateStateUpridhtBody5th;
427+ counter = 0;
428+ }
429+}
430+
431+void LookupGate::UpridhtBody1st() {
432+ runMode = run66;
433+ BaseCourse::Run();
434+ if (tail_control(interval2ndTailAngle, 100)) {
435+ log("LookupGateStateUpridhtBody2nd\r\n");
436+ lookupGateState = LookupGateStateUpridhtBody2nd;
437+ }
438+}
439+
440+void LookupGate::UpridhtBody2nd() {
441+ runMode = run66;
442+ BaseCourse::Run();
443+ if (++counter >= 300) {
444+ log("LookupGateStateUpridhtBody3rd\r\n");
445+ lookupGateState = LookupGateStateUpridhtBody3rd;
446+ counter = 0;
447+ }
448+}
449+
450+void LookupGate::UpridhtBody3rd() {
451+ runMode = run66;
452+ BaseCourse::Run();
453+ if (tail_control(interval1stTailAngle, 100)) {
454+ log("LookupGateStateUpridhtBody4th\r\n");
455+ lookupGateState = LookupGateStateUpridhtBody4th;
456+ }
457+}
458+
459+void LookupGate::UpridhtBody4th() {
460+ runMode = run66;
461+ BaseCourse::Run();
462+ if (++counter >= 300) {
463+ log("LookupGateStateUpridhtBody5th\r\n");
464+ lookupGateState = LookupGateStateUpridhtBody5th;
465+ counter = 0;
466+ }
467+}
468+
469+void LookupGate::UpridhtBody5th() {
470+ runMode = run66;
471+ BaseCourse::Run();
472+// if(tail_control(standTailAngle, 100)) {
473+// if(tail_control(standTailAn, 100)) {
474+ UpridhtBodyCount++;
475+ if(UpridhtBodyCount != 3){
476+ log("LookupGateStateBeforeSpin\r\n");
477+ lookupGateState = LookupGateStateBeforeSpin;
478+ } else {
479+ log("LookupGateStateGraySerchRun\r\n");
480+ forwardMotorAngL = motor_ang_l;
481+ lookupGateState = LookupGateStateGraySerchRun;
482+ }
483+// }
484+}
485+
486+void LookupGate::SetPIDData() {
487+ // PID制御に使用する定数を設定する
488+ PIDMgr->SetFixedData(1.4, 1, 0.03);
489+}
490+
491+bool LookupGate::tail_control(signed int angle, int powerMax) {
492+ if (angle < 50) {
493+ angle = 50;
494+ }
495+ float pwm = (float)(angle - ev3_motor_get_counts(tail_motor)) * P_GAIN_LOOKUP_GATE; /* 比例制御 */
496+ /* PWM出力飽和処理 */
497+ if (pwm > powerMax) {
498+ pwm = powerMax;
499+ } else if (pwm < -powerMax) {
500+ pwm = -powerMax;
501+ }
502+
503+ if (pwm == 0) {
504+ ev3_motor_stop(tail_motor, true);
505+ return true;
506+ } else {
507+ ev3_motor_set_power(tail_motor, (signed char)pwm);
508+ return false;
509+ }
510+}
511+
512+void LookupGate::calcMotorPower() {
513+// log("%d,%d,\r\n", colorSensor,runMode);
514+// log("colorSensor : %d\r\n", colorSensor);
515+ if(runMode == normalRun) {
516+ BaseCourse::calcMotorPower();
517+ } else if(runMode == run66) {
518+ const int LightWhite = 5;
519+ const int LightBlack = 0;
520+ const int targetColor = (LightWhite + LightBlack) / 2;
521+ const int defaultPower = 5;
522+ const int additionalPower = 5;
523+ if (forward > 0) {
524+ int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
525+ //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
526+ //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
527+ pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
528+ pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
529+ } else if(forward == 0) {
530+ pwm_L = 0;
531+ pwm_R = 0;
532+ } else if(forward < 0) {
533+ pwm_L = forward;
534+ pwm_R = forward;
535+ }
536+ } else if(runMode == run84) {
537+ const int LightWhite = 15;
538+ const int LightBlack = 4;
539+ const int targetColor = (LightWhite + LightBlack) / 2;
540+ const int defaultPower = 5;
541+ const int additionalPower = 5;
542+ if (forward > 0) {
543+ int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
544+ //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
545+ //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
546+ pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
547+ pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
548+ } else if(forward == 0) {
549+ pwm_L = 0;
550+ pwm_R = 0;
551+ } else if(forward < 0) {
552+ pwm_L = forward;
553+ pwm_R = forward;
554+ }
555+ }
556+}
557+void LookupGate::GraySerchRun() {
558+// runMode = run84;
559+ runMode = run66;
560+ BaseCourse::Run();
561+ if (300 < (motor_ang_l - forwardMotorAngL)) {
562+ log("LookupGateStateChangeGarage\r\n");
563+ lookupGateState = LookupGateStateChangeGarage;
564+ }
565+}
566+void LookupGate::ChangeGarage() {
567+// runMode = run84;
568+ runMode = run66;
569+ BaseCourse::Run();
570+}
571+
572+/**
573+ * @brief コースモード移行判定(オーバーライド)
574+ * @note
575+ * 自コースの走行クラスにモードを移行していいかを判定
576+ *
577+ */
578+bool LookupGate::IsNextCourse() {
579+ bool result = false;
580+ // グレーサーチモードで、
581+ if (lookupGateState == LookupGateStateChangeGarage) {
582+ ev3_motor_stop(tail_motor, true);
583+ result = true;
584+ }
585+ return result;
586+}
587+
588+//*****************************************************************************
589+// 関数名 : sonar_alert
590+// 引数 : 無し
591+// 返り値 : 1(障害物あり)/0(障害物無し)
592+// 概要 : 超音波センサによる障害物検知
593+//*****************************************************************************
594+int LookupGate::sonar_alert(void) {
595+ static unsigned int counter = 0;
596+ static int alert = 0;
597+ /* 約40msec周期毎に障害物検知 */
598+ if (++counter == 40/4) {
599+// log("distance : %d\r\n", distance);
600+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
601+ if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0)) {
602+ alert = 1; /* 障害物を検知 */
603+ } else {
604+ alert = 0; /* 障害物無し */
605+ }
606+ counter = 0;
607+ } else {
608+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
609+ }
610+ return alert;
611+}
612+
613+bool LookupGate::SpinRight() {
614+ int32_t motorAngle = ev3_motor_get_counts(right_motor);
615+ if (targetSpinCount == 0) {
616+ targetSpinCount = motorAngle + 300;
617+ //targetSpinCount = motorAngle - 330;
618+ ev3_motor_stop(left_motor, true);
619+ }
620+ if (motorAngle < targetSpinCount) {
621+// log("target:%d current:%d\r\n", targetSpinCount, motorAngle);
622+ ev3_motor_set_power(right_motor, 20);
623+ return false;
624+ } else {
625+ ev3_motor_stop(right_motor, true);
626+ targetSpinCount = 0;
627+ return true;
628+ }
629+}
630+
631+bool LookupGate::SpinLeft() {
632+ int32_t motorAngle = ev3_motor_get_counts(left_motor);
633+ if (targetSpinCount == 0) {
634+ targetSpinCount = motorAngle - 360;
635+// targetSpinCountBack = motorAngle - 600;
636+ //targetSpinCount = motorAngle + 390;
637+ ev3_motor_stop(right_motor, true);
638+ }
639+ if (targetSpinCount < motorAngle) {
640+// log("target:%d current:%d\r\n", targetSpinCount, motorAngle);
641+ ev3_motor_set_power(left_motor, -20);
642+ return false;
643+ } else {
644+// if(targetSpinCountBack < motorAngle){
645+// ev3_motor_set_power(left_motor, -10);
646+// ev3_motor_set_power(right_motor, -10);
647+// return false;
648+// }
649+ ev3_motor_stop(left_motor, true);
650+ targetSpinCount = 0;
651+ return true;
652+ }
653+}
654+
655+// 尻尾走行ライントレース(84度想定)
656+void LookupGate::LineTraceWithTail() {
657+// runMode = run84;
658+ runMode = run66;
659+ BaseCourse::Run();
660+}
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/20180901kitoh_bk/LookupGate.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/20180901kitoh_bk/LookupGate.h (revision 404)
@@ -0,0 +1,152 @@
1+/*
2+ * File: LookupGate.h
3+ * Author: h.kitoh
4+ *
5+ * Created on 2016/08/14, 10:39
6+ */
7+
8+#ifndef LOOKUPGATE_H
9+#define LOOKUPGATE_H
10+
11+#define P_GAIN_LOOKUP_GATE 2.5F /* 完全停止用モータ制御比例係数 */
12+#define PWM_ABS_MAX_LOOKUP_GATE 20 /* 完全停止用モータ制御PWM絶対最大値 */
13+#define SONAR_ALERT_DISTANCE 20/* 超音波センサによる障害物検知距離[cm] */
14+#define LIGHT_WHITE_LOOKUP_GATE 12 /* 白色の光センサ値 */
15+#define LIGHT_GRAY_LOOKUP_GATE 6 /* 灰色の光センサ値 */
16+#define LIGHT_BLACK_LOOKUP_GATE 1 /* 黒色の光センサ値 */
17+#define COLOR_LIST_COUNT_SEARCHLINE_LOOKUP_GATE 3
18+#define COLOR_LIST_COUNT_SEARCHLINEEDGE_LOOKUP_GATE 5
19+#define KP 1.0 /* P制御用係数*/
20+#define KI 1.5 /* I制御用係数*/
21+#define KD 1.5 /* D制御用係数*/
22+
23+#include "BaseCourse.h"
24+#include "CommonApp.h"
25+#include "ev3api.h"
26+
27+//////////////////////////////////
28+#include "Stopper.h"
29+//////////////////////////////////
30+
31+class GrayChecker;
32+
33+class LookupGate : public BaseCourse {
34+ public:
35+ LookupGate();
36+ virtual ~LookupGate();
37+
38+ void setForward();
39+ void runUntilSonorDetected();
40+ void runSonorDetected();
41+ void Run();
42+ void ForwardRun();
43+ void ForwardRunAjustment();
44+ void throughGateReady1st();
45+ void throughGateReady2nd();
46+ void throughGateReady3rd();
47+ void throughGateReady4th();
48+ void throughGateReady5th();
49+ void throughGateReady6th();
50+ void throughGateReady7th();
51+ void throughGateReady8th();
52+ void RunStop();
53+ void UpridhtBody1st();
54+ void UpridhtBody2nd();
55+ void UpridhtBody3rd();
56+ void UpridhtBody4th();
57+ void UpridhtBody5th();
58+ void ChangeGarage();
59+ void GraySerchRun();
60+ void SetPIDData();
61+ void calcMotorPower();
62+ bool IsNextCourse();
63+ bool tail_control(signed int angle, int powerMax);
64+ virtual int sonar_alert(void);
65+ private:
66+ LookupGate(const LookupGate& orig){}
67+ bool m_stop;
68+
69+ bool SpinRight();
70+ bool SpinLeft();
71+ void LineTraceWithTail();
72+
73+ private:
74+ enum LookupGateState
75+ {
76+ LookupGateStateBeforeSonorDetected,
77+ LookupGateStateAfterSonorDetected,
78+ LookupGateStateThroughGateReady1st,
79+ LookupGateStateThroughGateReady2nd,
80+ LookupGateStateThroughGateReady3rd,
81+ LookupGateStateThroughGateReady4th,
82+ LookupGateStateThroughGateReady5th,
83+ LookupGateStateThroughGateReady6th,
84+ LookupGateStateThroughGateReady7th,
85+ LookupGateStateThroughGateReady8th,
86+ LookupGateStateThroughGateForward,
87+ LookupGateStateThroughGateForwardAjustment,
88+ LookupGateStateThroughGateStop,
89+ LookupGateStateUpridhtBody1st,
90+ LookupGateStateUpridhtBody2nd,
91+ LookupGateStateUpridhtBody3rd,
92+ LookupGateStateUpridhtBody4th,
93+ LookupGateStateUpridhtBody5th,
94+ LookupGateStateBodyDown1st,
95+ LookupGateStateBodyDown2nd,
96+ LookupGateStateBodyDown3rd,
97+ LookupGateStateBodyDown4th,
98+ LookupGateStateBodyDown5th,
99+ LookupGateStateBodyDown6th,
100+ LookupGateStateGraySerchRun,
101+ LookupGateStateChangeGarage,
102+ LookupGateStateRun,
103+ LookupGateStateBeforeSpin,
104+ LookupGateStateSpinRightWheel,
105+ LookupGateStateSpinLeftWheel,
106+ LookupGateStateAfterSpin,
107+ };
108+ LookupGateState lookupGateState;
109+ enum RunMode
110+ {
111+ normalRun,
112+ run84,
113+ run66
114+ };
115+ RunMode runMode;
116+ GrayChecker *grayChecker;
117+
118+ unsigned int distance = 0;
119+ unsigned int preDistanse = 0;
120+ signed char pwm = 0;
121+ int32_t distanceWork;
122+ int32_t distanceRWork;
123+ int32_t distanceLWork;
124+ int ForwardCounter = 0;
125+ int ForwardCompareCounter = 0;
126+ int counter;
127+ int forwardMotorAngL = 0;
128+ int UpridhtBodyCount = 0;
129+ int32_t targetSpinCount = 0;
130+ int32_t targetSpinCountBack = 0;
131+ const int COLOR_LIST_COUNT_SEARCHLINE = 3; // ライン探索時のカラー値累積カウント
132+ // カラーセンサー値関連
133+ const int LIGHT_GRAY = 9; // グレー色の光センサ値
134+ std::vector<int> m_colorCensorList;
135+ const sensor_port_t
136+ touch_sensor = EV3_PORT_1,
137+ sonar_sensor = EV3_PORT_2,
138+ color_sensor = EV3_PORT_3,
139+ gyro_sensor = EV3_PORT_4;
140+
141+ const motor_port_t
142+ left_motor = EV3_PORT_C,
143+ right_motor = EV3_PORT_B,
144+ tail_motor = EV3_PORT_A;
145+
146+//////////////////////////////////
147+ Stopper stopper;
148+ int waitCount = 0;
149+//////////////////////////////////
150+};
151+
152+#endif /* LOOKUPGATE_H */
\ No newline at end of file
Show on old repository browser