• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision427 (tree)
Zeit2019-09-14 16:18:11
Autormizutani-f

Log Message

シーソー本番用(阿部さん作成分)

Ändern Zusammenfassung

Diff

--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Seesaw.cpp (revision 426)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Seesaw.cpp (revision 427)
@@ -45,26 +45,25 @@
4545
4646 //デバック時少し動く
4747 case SeesawState0stStepDetection:
48- log("StepDetection start volt %d\r\n" ,volt);
48+ // log("StepDetection (48) start volt %d\r\n" ,volt);
49+ gyro_offset = NORMAL_GYRO_OFFSET;
4950 StartForward(MORTOR_ANG_START_FORWARD);
5051 break;
5152
5253 //段差検知モード
5354 case SeesawState1stStepDetection:
54- log("StepDetection start\r\n" );
55+ // log("StepDetection (54) start\r\n" );
5556 StepDetection();
5657 break;
5758
5859 //バック走行
5960 case SeesawState1stBackward:
60- //case SeesawState2ndBackward:
61- log("Backward start\r\n" );
61+ // log("Backward (60) start\r\n" );
6262 Backward(MORTOR_ANG_BACKWARD);
6363 break;
6464 //昇段モード
6565 case SeesawState1stStepUp:
66- //case StairsState2ndStepUp:
67- log("statestepup start\r\n" );
66+ //log("statestepup (65) start\r\n" );
6867 gyro_offset = STEP_UP_GYRO_OFFSET;
6968 StepUp(MORTOR_ANG_STEP_UP);
7069 break;
@@ -72,23 +71,31 @@
7271 // 一回目さかのぼり
7372 case SeesawState1stSlopeUp:
7473 //距離
74+ // log("SeesawState1stSlopeUp (73) start\r\n" );
7575 gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
7676 SlopeUp(MORTOR_ANG_SLOPE_UP);
7777 break;
78+
79+ // 一回目さかのぼり中間
80+ case SeesawState1stCenterSlopeUp:
81+ //距離
82+ // log("SeesawState1stCenterSlopeUp (81) start\r\n" );
83+ gyro_offset = 2; //ジャイロオフセット0
84+ SlopeUp(MORTOR_ANG_SLOPE_1ST_UP);
85+ break;
7886
7987 // 一回目さかくだり
8088 case SeesawState1stSlopeDown:
81- // 音デバック用
82- //ev3_speaker_play_tone(NOTE_C4, 500);
83- //距離
89+ //log("SeesawState1stSlopeDown (92) start\r\n" );
8490 gyro_offset = SLOPE_DOWN_GYRO_OFFSET;
8591 SlopeDown(MORTOR_ANG_SLOPE_DOWN);
8692 break;
8793
88- // 2回目さかのぼりバック
94+ // 2回目さかのぼりバック:
8995 case SeesawState2ndSlopeUpReverse:
9096 //距離
91- gyro_offset = SLOPE_DOWN_2ND_GYRO_OFFSET; //ジャイロオフセット0
97+ //log("SeesawState2ndSlopeUpReverse (100) start\r\n" );
98+ gyro_offset = 10; //ジャイロオフセット
9299 SlopeUpBack(MORTOR_ANG_SLOPE_UP_BACK);
93100 break;
94101
@@ -95,16 +102,16 @@
95102 // 2回目さかのぼり中間バック
96103 case SeesawState2ndCenterSlopeUpReverse:
97104 //距離
98- gyro_offset = -1; //ジャイロオフセット0
105+ //log("SeesawState2ndCenterSlopeUpReverse (108) start\r\n" );
106+ gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
99107 SlopeUpBack(MORTOR_ANG_SLOPE_CENTER_BACK);
100108 break;
101109
102110 // 2回目さかくだりバック
103111 case SeesawState2ndSlopeDownReverse:
104- // 音デバック用
105- //ev3_speaker_play_tone(NOTE_C4, 500);
106112 //距離
107- gyro_offset = SLOPE_UP_2ND_GYRO_OFFSET;
113+ //log("SeesawState2ndSlopeDownReverse (120) start\r\n" );
114+ gyro_offset = SLOPE_UP_2ND_GYRO_OFFSET;//ジャイロオフセット-20
108115 SlopeDownBack(MORTOR_ANG_SLOPE_DOWN_BACK);
109116 break;
110117
@@ -111,15 +118,23 @@
111118 // 3回目さかのぼり
112119 case SeesawState3rdSlopeUp:
113120 //距離
114- gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
121+ //log("SeesawState3rdSlopeUp (128) start\r\n" );
122+ gyro_offset = -10;
115123 SlopeUp3rd(MORTOR_ANG_SLOPE_UP_3RD);
116124 break;
117-
125+
126+ // 3回目さかのぼり中間
127+ case SeesawState3rdCenterSlopeUp:
128+ //距離
129+ //log("SeesawState3rdCenterSlopeUp (128) start\r\n" );
130+ gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
131+ SlopeUp3rd(MORTOR_ANG_SLOPE_UP_CENTER_3RD);
132+ break;
133+
118134 // 3回目さかくだり
119135 case SeesawState3rdSlopeDown:
120- // 音デバック用
121- //ev3_speaker_play_tone(NOTE_C4, 500);
122136 //距離
137+ //log("SeesawState3rdSlopeDown (138) start\r\n" );
123138 gyro_offset = SLOPE_DOWN_3RD_GYRO_OFFSET;
124139 SlopeDown3rd(MORTOR_ANG_SLOPE_DOWN_3RD);
125140 break;
@@ -127,7 +142,7 @@
127142 // グレーライントレースモード
128143 case SeesawStateGraySearch:
129144 GraySearch();
130- log("ddddd \r\n");
145+ //log("GraySearch (139) \r\n");
131146 break;
132147 }
133148 }
@@ -156,24 +171,6 @@
156171 {
157172 switch(seesawState)
158173 {
159-// case SeesawStateLineEdgeChangeForward0:
160-// modeUpdatePidValue = GrayLineTraceMode;
161-// modeNormalizeTurnValue = LineTraceTurnMode;
162-// modeSetForward = AdjustLineForwardMode;
163-// modeSetPIDData = OtherPIDMode;
164-// break;
165-// case SeesawStateLineEdgeChangeForward1:
166-// modeUpdatePidValue = GrayLineTraceMode;
167-// modeNormalizeTurnValue = LineOverMode1;
168-// modeSetForward = BackwardMode;
169-// modeSetPIDData = OtherPIDMode;
170-// break;
171-// case SeesawStateLineEdgeChangeForward2:
172- // modeUpdatePidValue = GrayLineTraceMode;
173-// modeNormalizeTurnValue = LineOverMode2;
174-// modeSetForward = BackwardMode;
175-// modeSetPIDData = OtherPIDMode;
176-// break;
177174 case SeesawState0stStepDetection: // 0 デバック用 少し動く 段差検知モード
178175 modeUpdatePidValue = GrayLineTraceMode;
179176 modeNormalizeTurnValue = LineTraceTurnMode;
@@ -180,76 +177,90 @@
180177 modeSetForward = StepDetectionMode;
181178 modeSetPIDData = OtherPIDMode;
182179 break;
183- case SeesawState1stStepDetection: // 0 1段目 段差検知モード
180+ case SeesawState1stStepDetection: // 1 1段目 段差検知モード
184181 modeUpdatePidValue = GrayLineTraceMode;
185182 modeNormalizeTurnValue = LineTraceTurnMode;
186183 modeSetForward = StepDetectionMode;
187184 modeSetPIDData = OtherPIDMode;
188185 break;
189- case SeesawState1stBackward: // 1 1段目 段差検知後バック走行モード
186+ case SeesawState1stBackward: // 2 1段目 段差検知後バック走行モード
190187 modeUpdatePidValue = GrayLineTraceMode;
191- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
192- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
188+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
189+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
193190 modeSetForward = BackwardMode;
194191 modeSetPIDData = OtherPIDMode;
195192 break;
196- case SeesawState1stStepUp: // 2 昇段モード
193+ case SeesawState1stStepUp: // 3 昇段モード
197194 modeUpdatePidValue = GrayLineTraceMode;
198- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
199- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
195+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
196+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
200197 modeSetForward = StepUpMode;
201198 modeSetPIDData = StepDetectionPIDMode;
202199 break;
203- case SeesawState1stSlopeUp: // 3 一回目さかのぼりモード
200+ case SeesawState1stSlopeUp: // 4 一回目さかのぼりモード
204201 modeUpdatePidValue = GrayLineTraceMode;
205- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
206- modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
202+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
203+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
207204 modeSetForward = SlopeUpMode;
208205 modeSetPIDData = StepDetectionPIDMode;
209206 break;
210- case SeesawState1stSlopeDown: // 4 一回目さかくだりモード
207+ case SeesawState1stCenterSlopeUp: // 5 一回目 さかのぼり中間
211208 modeUpdatePidValue = GrayLineTraceMode;
212- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
213- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
209+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
210+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
211+ modeSetForward = SlopeCenterUpMode;
212+ modeSetPIDData = StepDetectionPIDMode;
213+ break;
214+ case SeesawState1stSlopeDown: // 6 一回目 さかくだり
215+ modeUpdatePidValue = GrayLineTraceMode;
216+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
217+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
214218 modeSetForward = SlopeDownReverseMode;
215219 modeSetPIDData = StepDetectionPIDMode;
216220 break;
217- case SeesawState2ndSlopeUpReverse: // 5 二回目さかのぼりバックモード
221+ case SeesawState2ndSlopeUpReverse: // 7 二回目 さかのぼり(バック)
218222 modeUpdatePidValue = GrayLineTraceMode;
219- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
220- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
223+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
224+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
221225 modeSetForward = SlopeDownReverse2ndMode;
222226 modeSetPIDData = StepDetectionPIDMode;
223227 break;
224- case SeesawState2ndCenterSlopeUpReverse: // 5 二回目さかのぼりバックモード
228+ case SeesawState2ndCenterSlopeUpReverse: // 8 二回目 さかのぼり中間(バック)
225229 modeUpdatePidValue = GrayLineTraceMode;
226- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
227- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
230+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
231+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
228232 modeSetForward = SlopeDownReverseMode;
229233 modeSetPIDData = StepDetectionPIDMode;
230234 break;
231- case SeesawState2ndSlopeDownReverse: // 6 二回目さかくだりバックモード
235+ case SeesawState2ndSlopeDownReverse: // 9 二回目 さかくだり(バック)
232236 modeUpdatePidValue = GrayLineTraceMode;
233- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
234- //void updatePidValue();modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
237+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
238+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
235239 modeSetForward = SlopeDownReverseMode;
236240 modeSetPIDData = StepDetectionPIDMode;
237241 break;
238- case SeesawState3rdSlopeUp: // 7 三回目さかのぼり
242+ case SeesawState3rdSlopeUp: // 10 三回目 さかのぼり
239243 modeUpdatePidValue = GrayLineTraceMode;
240244 //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
241245 modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
242246 modeSetForward = SlopeUp3rdMode;
243247 modeSetPIDData = StepDetectionPIDMode;
244- break;
245- case SeesawState3rdSlopeDown: // 8 三回目さかくだり
248+ break;
249+ case SeesawState3rdCenterSlopeUp: // 10 三回目 さかのぼり
246250 modeUpdatePidValue = GrayLineTraceMode;
247- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
248- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
251+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
252+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
253+ modeSetForward = SlopeUp3rdCenterMode;
254+ modeSetPIDData = StepDetectionPIDMode;
255+ break;
256+ case SeesawState3rdSlopeDown: // 11 三回目 さかくだり
257+ modeUpdatePidValue = GrayLineTraceMode;
258+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
259+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
249260 modeSetForward = SlopeDown3rdMode;
250261 modeSetPIDData = StepDetectionPIDMode;
251262 break;
252- case SeesawStateGraySearch: // 38 グレーライン検知モード
263+ case SeesawStateGraySearch: // 12 グレーライン検知モード
253264 modeUpdatePidValue = GrayLineTraceMode;
254265 modeNormalizeTurnValue = LineTraceTurnMode;
255266 modeSetForward = StepDetectionMode;
@@ -333,7 +344,6 @@
333344 */
334345 void Seesaw::GraySearch()
335346 {
336- log("ssss\r\n");
337347 BaseCourse::Run();
338348 }
339349
@@ -413,7 +423,11 @@
413423 case SlopeUpMode:
414424 forward = FORWARD_SLOPE_UP;
415425 break;
416-
426+
427+ case SlopeCenterUpMode:
428+ forward = FORWARD_SLOPE_CENTER_UP;
429+ break;
430+
417431 case SlopeDownMode:
418432 forward = FORWARD_SLOPE_DOWN;
419433 break;
@@ -434,6 +448,10 @@
434448 forward = FORWARD_SLOPE_UP_3RD;
435449 break;
436450
451+ case SlopeUp3rdCenterMode:
452+ forward = FORWARD_SLOPE_UP_CENTER_3RD;
453+ break;
454+
437455 case SlopeDown3rdMode:
438456 forward = FORWARD_SLOPE_DOWN_3RD;
439457 break;
@@ -471,7 +489,6 @@
471489 break;
472490
473491 case GraySearchMode:
474- log("aaaaaa \r\n");
475492 forward = FORWARD_GRAY_SEARCH;
476493 break;
477494
@@ -505,13 +522,9 @@
505522
506523 switch(seesawState)
507524 {
508-
509- //case SeesawStateLineEdgeChangeForward1:
510- //case SeesawStateLineEdgeChangeForward2:
511- break;
525+ break;
512526 default:
513- //turn = turn * -1;
514- break;
527+ break;
515528 }
516529 }
517530
@@ -518,7 +531,7 @@
518531 void Seesaw::setMotorPower() {
519532 // モーター出力の設定
520533 //log("setMotorPower a %d, %d, %d, %d \r\r\n", (int)motor_ang_l, (int)wk_motor_ang_l, (int)(motor_ang_r), (int)(wk_motor_ang_r));
521- //log("setMotorPower b %d, %d, %d, %d \r\r\n", volt, (int)pwm_L, (int)(pwm_R), colorSensor);
534+ log("setMotorPower b %d, %d, %d, %d \r\r\n", volt, (int)pwm_L, (int)(pwm_R), colorSensor);
522535
523536 BaseCourse::setMotorPower();
524537
@@ -614,7 +627,7 @@
614627 BaseCourse::Run();
615628 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
616629 {
617- log("StairsState1stStepUp end\r\n");
630+ //log("StairsState1stStepUp (631) end\r\n");
618631 changeNextState();
619632 }
620633 }
@@ -628,7 +641,7 @@
628641 void Seesaw::StepDetection()
629642 {
630643 BaseCourse::Run();
631- log("StepDetection %d \r\n", gyro);
644+ //log("StepDetection (645) %d \r\n", gyro);
632645 if(STEP_DETECTION_GYRO < std::abs(gyro))
633646 {
634647 changeNextState();
@@ -643,11 +656,11 @@
643656 void Seesaw::StartForward(int motor_ang)
644657 {
645658 BaseCourse::Run();
646- log("start motor_ang %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
659+ //log("start motor_ang (660) %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
647660 //if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
648661 if(motor_ang < (motor_ang_l + motor_ang_r) /2)
649662 {
650- log("StartForward end\r\n");
663+ //log("StartForward (664) end\r\n");
651664 changeNextState();
652665 }
653666 }
@@ -661,12 +674,12 @@
661674 void Seesaw::SlopeUp(int motor_ang)
662675 {
663676 BaseCourse::Run();
664- //log("UP start motor_ang %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
665- log("UP1st start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
666- if(1100 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR && 1110 > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR){
667- // 音デバック用
668- ev3_speaker_play_tone(NOTE_C4, 100);
669- }
677+ //log("UP1st (679) start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
678+ log("UP1st (679) start gyro STEP_DETECTION_GYRO %f, %d,forward %d\r\n",STEP_DETECTION_GYRO, gyro , (motor_ang_l + motor_ang_r) - wk_motor_ang_LR );
679+ //if(350 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR){
680+ // 音デバック用
681+ //ev3_speaker_play_tone(NOTE_C4, 100);
682+ //}
670683 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
671684 {
672685 //log("SlopeUp end\r\n");
@@ -683,11 +696,11 @@
683696 void Seesaw::SlopeDown(int motor_ang)
684697 {
685698 BaseCourse::Run();
686- log("SeesawState1stSlopeDown start Gyro %f, %d motor_ang %d\r\n ",STEP_DETECTION_GYRO, gyro ,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
699+ //log("SeesawState1stSlopeDown (700) start Gyro %f, %d motor_ang %d\r\n ",STEP_DETECTION_GYRO, gyro ,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
687700
688701 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
689702 {
690- log("slopedown end\r\n");
703+ log("slopedown (704) end\r\n");
691704 changeNextState();
692705 }
693706 }
@@ -703,13 +716,13 @@
703716 BaseCourse::Run();
704717 //log("UP start motor_ang %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
705718 //log("UP2nd start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
706- if(1100 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR && 1110 > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR){
707- // 音デバック用
708- ev3_speaker_play_tone(NOTE_C4, 100);
719+ if(1100 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR){
720+ // 音デバック用
721+ //ev3_speaker_play_tone(NOTE_C4, 100);
709722 }
710723 if(motor_ang > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
711724 {
712- log("22SlopeUp end\r\n");
725+ log("22SlopeUp (726) end\r\n");
713726 changeNextState();
714727 }
715728 }
@@ -727,7 +740,7 @@
727740
728741 if(motor_ang > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
729742 {
730- log("2222slopedown end\r\n");
743+ //log("2222slopedown (744) end\r\n");
731744 changeNextState();
732745 }
733746 }
@@ -742,10 +755,10 @@
742755 {
743756 BaseCourse::Run();
744757 //log("UP start motor_ang %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
745- log("UP3rd start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
758+ //log("UP3rd (759) start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
746759 if(1100 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR && 1110 > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR){
747- // 音デバック用
748- ev3_speaker_play_tone(NOTE_C4, 100);
760+ // 音デバック用
761+ // ev3_speaker_play_tone(NOTE_C4, 100);
749762 }
750763 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
751764 {
@@ -763,11 +776,11 @@
763776 void Seesaw::SlopeDown3rd(int motor_ang)
764777 {
765778 BaseCourse::Run();
766- log("SeesawState3stSlopeDown start motor_ang %d,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
779+ //log("SeesawState3stSlopeDown (780) start motor_ang %d,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
767780
768781 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
769782 {
770- log("slopedown end\r\n");
783+ //log("slopedown (784) end\r\n");
771784 changeNextState();
772785 }
773786 }
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Seesaw.h (revision 426)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/Seesaw.h (revision 427)
@@ -65,33 +65,31 @@
6565
6666 // ジャイロ値関連
6767 const float STEP_DETECTION_GYRO = 115; // 段差検知とするジャイロ値
68- //const float STEP_UP_GYRO_OFFSET = -7; // 段差上昇時のジャイロオフセット値
69- const float STEP_UP_GYRO_OFFSET = -7; // 段差上昇時のジャイロオフセット値
70- const float SLOPE_DOWN_GYRO_OFFSET = 9; // さかかえしのジャイロオフセット値
68+ const float STEP_UP_GYRO_OFFSET = -9; // 段差上昇時のジャイロオフセット値
69+ const float SLOPE_DOWN_GYRO_OFFSET = 20; // さかかえしのジャイロオフセット値
7170 const float SLOPE_UP_GYRO_OFFSET = 2; // さかかえしのジャイロオフセット値
7271 const float SLOPE_DOWN_2ND_GYRO_OFFSET = 0; // さかかえしのジャイロオフセット値
73- const float SLOPE_UP_2ND_GYRO_OFFSET = -4; // さかのぼりのジャイロオフセット値
72+ const float SLOPE_UP_2ND_GYRO_OFFSET = -10; // さかのぼりのジャイロオフセット値
7473 const float SLOPE_DOWN_3RD_GYRO_OFFSET = 2; // さかのぼりのジャイロオフセット値
7574 const float NORMAL_GYRO_OFFSET = 0; // 通常時のジャイロオフセット値
76-// const float SPIN_GYRO_OFFSET = 0.00; // スピン時のジャイロオフセット値
7775
7876 // カラーセンサー値関連
79- const int LIGHT_GRAY = 30; // グレー色の光センサ値
77+ const int LIGHT_GRAY = 30; // グレー色の光センサ値
8078
8179 // モーター回転角度関連
8280 const int MORTOR_ANG_START_FORWARD = 50; // (デバック用)スタート時のライントレース距離(左右モーターの合計回転角度)
8381 const int MORTOR_ANG_BEFORE_STEP_DOWN_LINE_TRACE = 90; // 段差を降りる前のライントレース距離(左右モーターの合計回転角度)
84- const int MORTOR_ANG_AFTER_STEP_DOWN_LINE_TRACE = 1670; // 段差を降りた後のライントレース距離(左右モーターの合計回転角度)
85- //const int MORTOR_ANG_BACKWARD = -350; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
86- const int MORTOR_ANG_BACKWARD = -200; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
87- const int MORTOR_ANG_STEP_UP = 250; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
88- const int MORTOR_ANG_SLOPE_UP = 960; // 坂を上る距離(左右モーターの合計回転角度)(一回目)
89- const int MORTOR_ANG_SLOPE_DOWN = 105; // 坂を下る距離(左右モーターの合計回転角度)(一回目)
90- const int MORTOR_ANG_SLOPE_UP_BACK = -70; // 坂を上る距離(左右モーターの合計回転角度)(二回目バック)
91- const int MORTOR_ANG_SLOPE_CENTER_BACK = -800; // 坂を上る距離(左右モーターの合計回転角度)(二回目バック)
92- const int MORTOR_ANG_SLOPE_DOWN_BACK = -100; // 坂を下る距離(左右モーターの合計回転角度)(二回目バック)
93- const int MORTOR_ANG_SLOPE_UP_3RD = 150; // 坂を上る距離(左右モーターの合計回転角度)(三回目)
94- const int MORTOR_ANG_SLOPE_DOWN_3RD = 500; // 坂を下る距離(左右モーターの合計回転角度)(三回目)
82+ const int MORTOR_ANG_BACKWARD = -220; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
83+ const int MORTOR_ANG_STEP_UP = 240; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
84+ const int MORTOR_ANG_SLOPE_UP = 650; // 一回目さかのぼり距離(左右モーターの合計回転角度)(一回目)
85+ const int MORTOR_ANG_SLOPE_1ST_UP = 140; // 一回目さかのぼり中間の距離(左右モーターの合計回転角度)(一回目)
86+ const int MORTOR_ANG_SLOPE_DOWN = 20; // 坂を下る距離(左右モーターの合計回転角度)(一回目)
87+ const int MORTOR_ANG_SLOPE_UP_BACK = -50; // 坂を上る距離(左右モーターの合計回転角度)(二回目バック)
88+ const int MORTOR_ANG_SLOPE_CENTER_BACK = -10; // 坂を上る距離(左右モーターの合計回転角度)(二回目バック)
89+ const int MORTOR_ANG_SLOPE_DOWN_BACK = -20; // 坂を下る距離(左右モーターの合計回転角度)(二回目バック)
90+ const int MORTOR_ANG_SLOPE_UP_3RD = 100; // 坂を上る距離(左右モーターの合計回転角度)(三回目)
91+ const int MORTOR_ANG_SLOPE_UP_CENTER_3RD = 100; // 坂を上る距離(左右モーターの合計回転角度)(三回目中間)
92+ const int MORTOR_ANG_SLOPE_DOWN_3RD = 500; // 坂を下る距離(左右モーターの合計回転角度)(三回目)
9593 const int MORTOR_ANG_STOP = 400; // 段差を昇ったあとから停止する位置の距離(左右モーターの合計回転角度)
9694 const int MORTOR_ANG_STOP_RANGE = 90; // 段差を昇ったあとから停止する位置の誤差許容範囲(左右モーターの合計回転角度)
9795 const int MORTOR_ANG_SEARCH_LINE_FORWARD = 90; // ライン探索時の前進距離(左右モーターの合計回転角度)
@@ -106,25 +104,27 @@
106104 const int MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD2 = 60;
107105
108106 // TURN値関連
109- const int TURN_NONE = 0; // ライントレース無し直線走行時の指定値1111
110- const int TURN_SPIN = -60; // スピン時の指定値
111- const int TURN_CHANGE_ANGLE = -20; // ライン探索時の車体方向角度変更時の指定値
112- const int TURN_MAX = 60; // TURN値のMAX値(絶対値)
107+ const int TURN_NONE = 0; // ライントレース無し直線走行時の指定値1111
108+ const int TURN_SPIN = -60; // スピン時の指定値
109+ const int TURN_CHANGE_ANGLE = -20; // ライン探索時の車体方向角度変更時の指定値
110+ const int TURN_MAX = 60; // TURN値のMAX値(絶対値)
113111 const int TURN_LINE_OVER1 = -10; // ラインをまたぐ際の値
114- const int TURN_LINE_OVER2 = 10; // ラインをまたぐ際の値
112+ const int TURN_LINE_OVER2 = 10; // ラインをまたぐ際の値
115113
116114
117115 // FORWARD(spped)値関連
118116 const int FORWARD_STEP_DETECTION = 30; // 段差検知時の指定値
119- const int FORWARD_BACKWARD = -60; // 段差を昇るための助走距離分バック走行時指定値
120- const int FORWARD_STEP_UP = 130; // 段差を昇る時の指定値
121- const int FORWARD_SLOPE_UP = 40; // aaaaaaaaaaaaaaaaaさかを上る時の指定値(一回目上り)
122- const int FORWARD_SLOPE_DOWN = 20; // aaaaaaaaaaaaaaaaaさかを下る時の指定値(一回目下り)
123- const int FORWARD_SLOPE_UP_BACK = -270; // aaaaaaaaaaaaaaaaaさかを上る時の指定値(二回目バック上り)
124- const int FORWARD_SLOPE_DOWN_BACK = -20; // aaaaaaaaaaaaaaaaaさかを下る時の指定値(二回目バック下り)
125- const int FORWARD_SLOPE_2ND_DOWN_BACK = -120;
126- const int FORWARD_SLOPE_UP_3RD = 200; // aaaaaaaaaaaaaaaaaさかを上る時の指定値(三回目上り)
127- const int FORWARD_SLOPE_DOWN_3RD = 20; // aaaaaaaaaaaaaaaaaさかを下る時の指定値(三回目下り)
117+ const int FORWARD_BACKWARD = -40; // 段差を昇るための助走距離分バック走行時指定値
118+ const int FORWARD_STEP_UP = 140; // 段差を昇る時の指定値
119+ const int FORWARD_SLOPE_UP = 110; // さかを上る時の指定値(一回目上り)
120+ const int FORWARD_SLOPE_CENTER_UP = 60; // さかを上る時の指定値(一回目上り中間)
121+ const int FORWARD_SLOPE_DOWN = 20; // さかを下る時の指定値(一回目下り)
122+ const int FORWARD_SLOPE_UP_BACK = -100; // さかを上る時の指定値(二回目バック上り)
123+ const int FORWARD_SLOPE_DOWN_BACK = 50; // さかを下る時の指定値(二回目バック下り)
124+ const int FORWARD_SLOPE_2ND_DOWN_BACK = 50; // さかを下る時の指定値(二回目バック下り)
125+ const int FORWARD_SLOPE_UP_3RD = 100; // さかを上る時の指定値(三回目上り)
126+ const int FORWARD_SLOPE_UP_CENTER_3RD = 400; // さかを上る時の指定値(三回目上り中間)
127+ const int FORWARD_SLOPE_DOWN_3RD = 20; // さかを下る時の指定値(三回目下り)
128128 const int FORWARD_MAX_FOR_STOP = 30; // 指定停止位置に移動してする際のMAX値
129129 const int FORWARD_SEARCH_LINE_FORWARD = 30; // ライン探索前進時の指定値
130130 const int FORWARD_SEARCH_LINE_BACKWARD = -30; // ライン探索後進時の指定値
@@ -134,12 +134,12 @@
134134 const int FORWARD_GRAY_SEARCH = 30; // グレーマーカー探索時の指定値
135135 const int FORWARD_ADJUST_LINE_FORWARD = 10; // ライントレース前進による位置調整時の指定値
136136 const int FORWARD_ADJUST_LINE_BACKWARD = -10; // ライントレース位置調整後の後進時の指定値
137- const int FORWARD_STOP = 0; //
137+ const int FORWARD_STOP = 0; // とまる
138138
139139 // 階段走行時のPID関連
140- const float P_VALUE_STAIRS_NORMAL = 1.5;
140+ const float P_VALUE_STAIRS_NORMAL = 0.4;
141141 const float I_VALUE_STAIRS_NORMAL = 0;
142- const float D_VALUE_STAIRS_NORMAL = 0.1;
142+ const float D_VALUE_STAIRS_NORMAL = 0.03;
143143
144144 // 階段走行時のPID関連
145145 const float P_VALUE_STAIRS_DETECTION = 0.4;
@@ -153,7 +153,7 @@
153153 // 階段フル基準電圧
154154 const float STAIRS_FULL_VOLT = 9200.0;
155155 // 階段通常時基準電圧
156- const float STAIRS_NORMAL_VOLT = 9200.0;
156+ const float STAIRS_NORMAL_VOLT = 9200.0; //9200
157157
158158 GrayChecker *grayChecker;
159159 /**
@@ -162,21 +162,20 @@
162162 */
163163 enum SeesawStates
164164 {
165-// SeesawStateLineEdgeChangeForward0, // 0
166-// SeesawStateLineEdgeChangeForward1, // 1
167-// SeesawStateLineEdgeChangeForward2, // 2
168- SeesawState0stStepDetection, // 0 スタート時に少しの距離動く
169- SeesawState1stStepDetection, // 3 1段目 段差検知モード
170- SeesawState1stBackward, // 4 1段目 段差検知後バック走行モード
171- SeesawState1stStepUp, // 5 1段目 昇段モード
172- SeesawState1stSlopeUp, // 6 一回目 さかのぼり
173- SeesawState1stSlopeDown, // 7 一回目 さかくだり
174- SeesawState2ndSlopeUpReverse, // 8 二回目 さかのぼり(バック)
175- SeesawState2ndCenterSlopeUpReverse, // 9 二回目 さかのぼり中間(バック)
176- SeesawState2ndSlopeDownReverse, // 10 二回目 さかくだり(バック)
177- SeesawState3rdSlopeUp, // 11 三回目 さかのぼり
178- SeesawState3rdSlopeDown, // 12 三回目 さかくだり
179- SeesawStateGraySearch, // 39 グレーライン検知モード
165+ SeesawState0stStepDetection, // 0 スタート時に少しの距離動く
166+ SeesawState1stStepDetection, // 1 1段目 段差検知モード
167+ SeesawState1stBackward, // 2 1段目 段差検知後バック走行モード
168+ SeesawState1stStepUp, // 3 1段目 昇段モード
169+ SeesawState1stSlopeUp, // 4 一回目 さかのぼり
170+ SeesawState1stCenterSlopeUp, // 5 一回目 さかのぼり中間
171+ SeesawState1stSlopeDown, // 6 一回目 さかくだり
172+ SeesawState2ndSlopeUpReverse, // 7 二回目 さかのぼり(バック)
173+ SeesawState2ndCenterSlopeUpReverse, // 8 二回目 さかのぼり中間(バック)
174+ SeesawState2ndSlopeDownReverse, // 9 二回目 さかくだり(バック)
175+ SeesawState3rdSlopeUp, // 10 三回目 さかのぼり
176+ SeesawState3rdCenterSlopeUp, // 11 三回目 さかのぼり中間
177+ SeesawState3rdSlopeDown, // 12 三回目 さかくだり
178+ SeesawStateGraySearch, // 13 グレーライン検知モード
180179
181180 };
182181 SeesawStates seesawState; // 階段走行のステート
@@ -217,11 +216,13 @@
217216 BackwardMode,
218217 StepUpMode,
219218 SlopeUpMode,
220- SlopeDownMode,
219+ SlopeCenterUpMode,
220+ SlopeDownMode,
221221 SlopeUpReverseMode,
222222 SlopeDownReverse2ndMode,
223223 SlopeDownReverseMode,
224224 SlopeUp3rdMode,
225+ SlopeUp3rdCenterMode,
225226 SlopeDown3rdMode,
226227 AdjustPostionMode,
227228 // StopMode,
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/SingleSeesaw.cpp (revision 426)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/SingleSeesaw.cpp (revision 427)
@@ -45,29 +45,27 @@
4545
4646 //デバック時少し動く
4747 case SingleSeesawState0stStepDetection:
48- log("StepDetection start volt %d\r\n" ,volt);
48+ gyro_offset = NORMAL_GYRO_OFFSET;
4949 StartForward(MORTOR_ANG_START_FORWARD);
5050 break;
5151
5252 //段差検知モード
5353 case SingleSeesawState1stStepDetection:
54- log("StepDetection start\r\n" );
5554 StepDetection();
5655 break;
5756
5857 //バック走行
5958 case SingleSeesawState1stBackward:
60- log("Backward start\r\n" );
6159 Backward(MORTOR_ANG_BACKWARD);
6260 break;
61+
6362 //昇段モード
6463 case SingleSeesawState1stStepUp:
65- log("statestepup start\r\n" );
6664 gyro_offset = STEP_UP_GYRO_OFFSET;
6765 StepUp(MORTOR_ANG_STEP_UP);
6866 break;
6967
70- // 一回目さかのぼり
68+ // さかのぼり
7169 case SingleSeesawState1stSlopeUp:
7270 //距離
7371 gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
@@ -74,67 +72,41 @@
7472 SlopeUp(MORTOR_ANG_SLOPE_UP);
7573 break;
7674
77- // 一回目さかのぼり中間
75+ // さかのぼり2
7876 case SingleSeesawState1stCenterSlopeUp:
7977 //距離
80- //gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
78+ gyro_offset = 2; //ジャイロオフセット0
8179 SlopeUp(MORTOR_ANG_SLOPE_1ST_UP);
8280 break;
8381
84- // 一回目さかくだり
82+ // さかくだり
8583 case SingleSeesawState1stSlopeDown:
8684 gyro_offset = SLOPE_DOWN_GYRO_OFFSET;
8785 SlopeDown(MORTOR_ANG_SLOPE_DOWN);
8886 break;
89-
90- // 2回目さかのぼりバック
91- case SingleSeesawState2ndSlopeUpReverse:
92- gyro_offset = 2; //ジャイロオフセット0
93- SlopeDown(MORTOR_ANG_SLOPE_UP_3RD);
87+
88+ // さかくだり2
89+ case SingleSeesawState1stCenterSlopeDown:
90+ //距離
91+ gyro_offset = SLOPE_DOWN_GYRO_OFFSET2; //ジャイロオフセット
92+ SlopeDown(MORTOR_ANG_SLOPE_UP_BACK);
9493 break;
95-
96- // 坂降りるときのジャイロ操作用
94+
95+ // さかおりる
9796 case SingleSeesawStateSlopeOut:
98- //ev3_speaker_play_tone(NOTE_C4, -1);
99- //gyro_offset = SLOPE_OUT_GYRO_OFFSET; //ジャイロオフセット 20
100- gyro_offset = 10;
101- SlopeDown(MORTOR_ANG_SLOPE_UP_BACK);
97+ gyro_offset = SLOPE_DOWN_GYRO_OFFSET3;
98+ SlopeDown(MORTOR_ANG_STOP);
10299 break;
103100
104- // 坂降りるときにすすむよう
101+ // さかおりる2
105102 case SingleSeesawStateSlopeOut2nd:
106- //log("bbbbbbbbbbbbbbbbbbbbbbbbbbbbb\r\n" );
107- //ev3_speaker_stop();
108- gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
109- SlopeDown(MORTOR_ANG_SLOPE_DOWN_3RD);
103+ gyro_offset = NORMAL_GYRO_OFFSET; //ジャイロオフセット0
104+ SlopeDown(MORTOR_ANG_STEP_DOWN);
110105 break;
111- // しっぽの角度を固定するまで進む
112- case SingleSeesawBeforeStop:
113- log("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa\r\n" );
114- gyro_offset = 1;
115- CommonApp::tail_control(82, 5);
116- //SlopeDown(MORTOR_ANG_SLOPE_DOWN_3RD);
117- AngleCheck();
118- break;
119106
120- // 停止
121- case SingleSeesawStop:
122- CommonApp::tail_control(82, 5);
123- //ev3_motor_stop(tail_motor, true);
124-
125- stopCount++;
126- if(stopCount > 300){
127- gyro_offset = 2; //ジャイロオフセット0
128- ev3_motor_stop(right_motor, true);
129- ev3_motor_stop(left_motor, true);
130- }
131- //Stop(MORTOR_ANG_SLOPE_UP_BACK);
132- break;
133-
134- // グレーライントレースモード
107+ // グレーライントレースモード
135108 case SingleSeesawStateGraySearch:
136109 GraySearch();
137- log("ddddd \r\n");
138110 break;
139111 }
140112 }
@@ -163,96 +135,83 @@
163135 {
164136 switch(seesawState)
165137 {
166- case SingleSeesawState0stStepDetection: // 0 デバック用 少し動く 段差検知モード
167- modeUpdatePidValue = GrayLineTraceMode;
138+ case SingleSeesawState0stStepDetection: // 0 デバック用 少し動く 段差検知モード
139+ modeUpdatePidValue = GrayLineTraceMode;
168140 modeNormalizeTurnValue = LineTraceTurnMode;
169- modeSetForward = StepDetectionMode;
170- modeSetPIDData = OtherPIDMode;
141+ modeSetForward = StepDetectionMode;
142+ modeSetPIDData = OtherPIDMode;
171143 break;
172- case SingleSeesawState1stStepDetection: // 0 1段目 段差検知モード
173- modeUpdatePidValue = GrayLineTraceMode;
144+ case SingleSeesawState1stStepDetection: // 1 1段目 段差検知モード
145+ modeUpdatePidValue = GrayLineTraceMode;
174146 modeNormalizeTurnValue = LineTraceTurnMode;
175- modeSetForward = StepDetectionMode;
176- modeSetPIDData = OtherPIDMode;
147+ modeSetForward = StepDetectionMode;
148+ modeSetPIDData = OtherPIDMode;
177149 break;
178- case SingleSeesawState1stBackward: // 1 1段目 段差検知後バック走行モード
179- modeUpdatePidValue = GrayLineTraceMode;
180- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
181- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
182- modeSetForward = BackwardMode;
183- modeSetPIDData = OtherPIDMode;
150+ case SingleSeesawState1stBackward: // 2 1段目 段差検知後バック走行モード
151+ modeUpdatePidValue = GrayLineTraceMode;
152+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
153+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
154+ modeSetForward = BackwardMode;
155+ modeSetPIDData = OtherPIDMode;
184156 break;
185- case SingleSeesawState1stStepUp: // 2 昇段モード
186- modeUpdatePidValue = GrayLineTraceMode;
187- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
188- modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
189- modeSetForward = StepUpMode;
190- modeSetPIDData = StepDetectionPIDMode;
157+ case SingleSeesawState1stStepUp: // 3 昇段モード
158+ modeUpdatePidValue = GrayLineTraceMode;
159+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
160+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
161+ modeSetForward = StepUpMode;
162+ modeSetPIDData = StepDetectionPIDMode;
191163 break;
192- case SingleSeesawState1stSlopeUp: // 3 一回目さかのぼりモード
193- modeUpdatePidValue = GrayLineTraceMode;
194- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
195- modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
196- modeSetForward = SlopeUpMode;
197- modeSetPIDData = StepDetectionPIDMode;
198- break;
199- case SingleSeesawState1stCenterSlopeUp: // 2 一回目さかのぼりモード中間
200- modeUpdatePidValue = GrayLineTraceMode;
201- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
202- modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
203- modeSetForward = SlopeUpMode;
204- modeSetPIDData = StepDetectionPIDMode;
164+ case SingleSeesawState1stSlopeUp: // 4 一回目さかのぼりモード
165+ modeUpdatePidValue = GrayLineTraceMode;
166+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
167+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
168+ modeSetForward = SlopeUpMode;
169+ modeSetPIDData = StepDetectionPIDMode;
205170 break;
206- case SingleSeesawState1stSlopeDown: // 4 一回目さかくだりモード
207- modeUpdatePidValue = GrayLineTraceMode;
208- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
209- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
210- modeSetForward = SlopeDownReverseMode;
211- modeSetPIDData = StepDetectionPIDMode;
171+ case SingleSeesawState1stCenterSlopeUp: // 5 一回目さかのぼりモード中間
172+ modeUpdatePidValue = GrayLineTraceMode;
173+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
174+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
175+ modeSetForward = SlopeUpCenterMode;
176+ modeSetPIDData = StepDetectionPIDMode;
212177 break;
213- case SingleSeesawState2ndSlopeUpReverse: // 5 二回目さかのぼりバックモード
214- modeUpdatePidValue = GrayLineTraceMode;
215- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
216- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
217- modeSetForward = SlopeDownReverse2ndMode;
218- modeSetPIDData = StepDetectionPIDMode;
219- break;
220- case SingleSeesawStateSlopeOut: // さか降り前ジャイロ用
221- modeUpdatePidValue = GrayLineTraceMode;
222- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
223- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
224- modeSetForward = SlopeDown2ndMode;
225- modeSetPIDData = StepDetectionPIDMode;
226- break;
227- case SingleSeesawStateSlopeOut2nd: // さかおり
228- modeUpdatePidValue = GrayLineTraceMode;
229- modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
230- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
231- modeSetForward = SlopeDownReverseMode;
232- modeSetPIDData = StepDetectionPIDMode;
233- break;
234- case SingleSeesawBeforeStop: // 5 停止モード
235- modeUpdatePidValue = GrayLineTraceMode;
236- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
237- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
238- modeSetForward = BeforeStopMode;
239- modeSetPIDData = StepDetectionPIDMode;
240- break;
241- case SingleSeesawStop: // 5 停止モード
242- modeUpdatePidValue = GrayLineTraceMode;
243- //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
244- //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
245- modeSetForward = StopMode;
246- modeSetPIDData = StepDetectionPIDMode;
247- break;
248- case SingleSeesawStateGraySearch: // 38 グレーライン検知モード
249- modeUpdatePidValue = GrayLineTraceMode;
178+ case SingleSeesawState1stSlopeDown: // 6 一回目さかくだりモード
179+ modeUpdatePidValue = GrayLineTraceMode;
180+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
181+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
182+ modeSetForward = SlopeDownReverseMode;
183+ modeSetPIDData = StepDetectionPIDMode;
184+ break;
185+ case SingleSeesawState1stCenterSlopeDown: // 7 二回目さかのぼりバックモード
186+ modeUpdatePidValue = GrayLineTraceMode;
187+ modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
188+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
189+ modeSetForward = SlopeDownReverse2ndMode;
190+ modeSetPIDData = StepDetectionPIDMode;
191+ break;
192+ case SingleSeesawStateSlopeOut: // 8 さか降り前ジャイロ用
193+ modeUpdatePidValue = GrayLineTraceMode;
194+ //modeNormalizeTurnValue = NoneTurnMode; //ライントレースなし
195+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
196+ modeSetForward = SlopeDownReverse2ndMode;
197+ modeSetPIDData = StepDetectionPIDMode;
198+ break;
199+ case SingleSeesawStateSlopeOut2nd: // 9 さかおり
200+ modeUpdatePidValue = GrayLineTraceMode;
201+ modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースなし
202+ //modeNormalizeTurnValue = LineTraceTurnMode; //ライントレースあり
203+ modeSetForward = SlopeDownReverseMode;
204+ modeSetPIDData = StepDetectionPIDMode;
205+ break;
206+ case SingleSeesawStateGraySearch: //10 グレーライン検知モード
207+ modeUpdatePidValue = GrayLineTraceMode;
250208 modeNormalizeTurnValue = LineTraceTurnMode;
251- modeSetForward = StepDetectionMode;
252- //modeSetForward = GraySearchMode;
253- modeSetPIDData = OtherPIDMode;
209+ modeSetForward = StepDetectionMode;
210+ //modeSetForward = GraySearchMode;
211+ modeSetPIDData = OtherPIDMode;
254212 break;
255213
214+
256215 }
257216 }
258217
@@ -329,7 +288,6 @@
329288 */
330289 void SingleSeesaw::GraySearch()
331290 {
332- log("ssss\r\n");
333291 BaseCourse::Run();
334292 }
335293
@@ -353,7 +311,7 @@
353311 PIDMgr->SetFixedData(P_VALUE_STAIRS_NORMAL, I_VALUE_STAIRS_NORMAL, D_VALUE_STAIRS_NORMAL);
354312 m_pFixVal = P_VALUE_STAIRS_NORMAL;
355313 m_iFixVal = I_VALUE_STAIRS_NORMAL;
356- m_dFixVal = D_VALUE_STAIRS_NORMAL;
314+ m_dFixVal = D_VALUE_STAIRS_NORMAL;
357315 break;
358316 }
359317
@@ -373,11 +331,7 @@
373331 // グレーサーチモードで、
374332 if (seesawState == SingleSeesawStateGraySearch)
375333 {
376-// // さらにグレーマーカー⇒黒ラインを感知したら次コース処理に遷移
377-// if(grayChecker->IsGrayLineEnd(colorSensor))
378-// {
379- result = true;
380-// }
334+ result = true;
381335 }
382336 return result;
383337 }
@@ -410,6 +364,10 @@
410364 forward = FORWARD_SLOPE_UP;
411365 break;
412366
367+ case SlopeUpCenterMode:
368+ forward = FORWARD_SLOPE_CENTER_UP;
369+ break;
370+
413371 case SlopeDownMode:
414372 forward = FORWARD_SLOPE_DOWN;
415373 break;
@@ -421,26 +379,7 @@
421379 case SlopeDownReverse2ndMode:
422380 forward = FORWARD_SLOPE_2ND_DOWN_BACK;
423381 break;
424- case SlopeDown2ndMode:
425- forward = 70;
426- break;
427382
428- case AdjustPostionMode:
429- forward = CalcForward((motor_ang_l + motor_ang_r) - wk_motor_ang_LR, MORTOR_ANG_STOP, FORWARD_MAX_FOR_STOP, MORTOR_ANG_FOR_STOP_DIFF_MAX);
430- break;
431-
432- case SearchLineForwardMode:
433- forward = FORWARD_SEARCH_LINE_FORWARD;
434- break;
435-
436- case SearchLineBackwardMode:
437- forward = FORWARD_SEARCH_LINE_BACKWARD;
438- break;
439-
440- case SearchLineChangeAngleMode:
441- forward = FORWARD_SEARCH_LINE_CHANGE_ANGLE;
442- break;
443-
444383 case LineTraceMode:
445384 forward = FORWARD_LINE_TRACE;
446385 break;
@@ -496,12 +435,8 @@
496435
497436 switch(seesawState)
498437 {
499-
500- //case SingleSeesawStateLineEdgeChangeForward1:
501- //case SingleSeesawStateLineEdgeChangeForward2:
502438 break;
503439 default:
504- //turn = turn * -1;
505440 break;
506441 }
507442 }
@@ -509,7 +444,7 @@
509444 void SingleSeesaw::setMotorPower() {
510445 // モーター出力の設定
511446 //log("setMotorPower a %d, %d, %d, %d \r\r\n", (int)motor_ang_l, (int)wk_motor_ang_l, (int)(motor_ang_r), (int)(wk_motor_ang_r));
512- //log("setMotorPower b %d, %d, %d, %d \r\r\n", volt, (int)pwm_L, (int)(pwm_R), colorSensor);
447+ log("setMotorPower b %d, %d, %d, %d \r\r\n", volt, (int)pwm_L, (int)(pwm_R), colorSensor);
513448
514449 BaseCourse::setMotorPower();
515450
@@ -541,13 +476,6 @@
541476 turn = TURN_MAX * (turn > 0) - (turn < 0);
542477 }
543478 break;
544-
545- case LineOverMode1:
546- turn = TURN_LINE_OVER1;
547- break;
548- case LineOverMode2:
549- turn = TURN_LINE_OVER2;
550- break;
551479 }
552480 }
553481
@@ -558,15 +486,12 @@
558486 */
559487 float SingleSeesaw::CalcForward(int currentMotorAng, int targetMortorAng, int maxForward, int maxDiffMortorAng) {
560488
561-// log("CalcForward %d, %d, %d, %d \r\r\n", currentMotorAng, targetMortorAng, maxForward, maxDiffMortorAng);
562489 float ret = 0;
563490 // 係数の算出 係数=最大差分(モーター角度)/最大FORWARD
564491 float coefficient = (float)maxDiffMortorAng / (float)maxForward;
565-// log("CalcForward coefficient %f \r\r\n", coefficient);
566492
567493 // 戻り値 = (現在の両輪モーター回転角度 - 目標位置の両輪モーター回転角度) * 係数
568494 ret = (float)(targetMortorAng - currentMotorAng) * coefficient;
569-// log("CalcForward ret %f \r\r\n", ret);
570495
571496 // 戻り値が最大Forward値を超えていたら最大Forward値に設定
572497 if (std::abs(ret) > maxForward)
@@ -574,7 +499,6 @@
574499 // maxForward * retの符号
575500 ret = maxForward * ((ret > 0) - (ret < 0));
576501 }
577-// log("CalcForward ret %f \r\r\n", ret);
578502 return ret;
579503 }
580504
@@ -587,7 +511,6 @@
587511 void SingleSeesaw::Backward(int motor_ang)
588512 {
589513 BaseCourse::Run();
590- //log("Backward %d, %d, %d, %d \r\r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
591514 if(motor_ang > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
592515 {
593516 changeNextState();
@@ -619,7 +542,6 @@
619542 void SingleSeesaw::StepDetection()
620543 {
621544 BaseCourse::Run();
622- log("StepDetection %d \r\n", gyro);
623545 if(STEP_DETECTION_GYRO < std::abs(gyro))
624546 {
625547 changeNextState();
@@ -634,8 +556,6 @@
634556 void SingleSeesaw::StartForward(int motor_ang)
635557 {
636558 BaseCourse::Run();
637- //log("start motor_ang %d ,%d,%d,%d,%d\r\n ",motor_ang ,motor_ang_l,motor_ang_r,wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
638- //if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
639559 if(motor_ang < (motor_ang_l + motor_ang_r) /2)
640560 {
641561 log("StartForward end\r\n");
@@ -652,10 +572,8 @@
652572 void SingleSeesaw::SlopeUp(int motor_ang)
653573 {
654574 BaseCourse::Run();
655- log("UP1st start gyro STEP_DETECTION_GYRO %f, %d,forward %d + %d - %d = %d\r\n",STEP_DETECTION_GYRO, gyro , motor_ang_l,motor_ang_r, wk_motor_ang_LR,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
656575 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
657- {
658- //log("SlopeUp end\r\n");
576+ {
659577 changeNextState();
660578 }
661579 }
@@ -669,7 +587,6 @@
669587 void SingleSeesaw::SlopeDown(int motor_ang)
670588 {
671589 BaseCourse::Run();
672- //log("SingleSeesawState1stSlopeDown start Gyro %f, %d motor_ang %d\r\n ",STEP_DETECTION_GYRO, gyro ,(motor_ang_l + motor_ang_r) - wk_motor_ang_LR);
673590
674591 if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
675592 {
@@ -679,19 +596,6 @@
679596 }
680597
681598 /**
682- * @brief しっぽの角度を判定する
683- * @note しっぽの角度
684- */
685-void SingleSeesaw::AngleCheck()
686-{
687- log("tail %d\r\n",ev3_motor_get_counts(tail_motor));
688- if ( 80 < ev3_motor_get_counts(tail_motor) ) {
689- log(" BeforeStopend\r\n");
690- changeNextState();
691- }
692-}
693-
694-/**
695599 * @brief 停止
696600 * @note 停止
697601 */
@@ -699,8 +603,6 @@
699603 {
700604 if(motor_ang > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
701605 {
702- log("22SlopeUp end\r\n");
703606 changeNextState();
704- //return true;
705607 }
706608 }
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/SingleSeesaw.h (revision 426)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/SingleSeesaw.h (revision 427)
@@ -64,14 +64,10 @@
6464
6565 // ジャイロ値関連
6666 const float STEP_DETECTION_GYRO = 115; // 段差検知とするジャイロ値
67- //const float STEP_UP_GYRO_OFFSET = -7; // 段差上昇時のジャイロオフセット値
68- const float STEP_UP_GYRO_OFFSET = -5; // 段差上昇時のジャイロオフセット値
69- const float SLOPE_DOWN_GYRO_OFFSET = 35; // さかかえしのジャイロオフセット値
70- const float SLOPE_UP_GYRO_OFFSET = 2; // さかかえしのジャイロオフセット値
71- const float SLOPE_OUT_GYRO_OFFSET = 10; // さかから降りるときのジャイロ値
72- const float SLOPE_DOWN_2ND_GYRO_OFFSET = 0; // さかかえしのジャイロオフセット値
73- const float SLOPE_UP_2ND_GYRO_OFFSET = -10; // さかのぼりのジャイロオフセット値
74- const float SLOPE_DOWN_3RD_GYRO_OFFSET = 2; // さかのぼりのジャイロオフセット値
67+ const float STEP_UP_GYRO_OFFSET = -9; // 段差上昇時のジャイロオフセット値
68+ const float SLOPE_DOWN_GYRO_OFFSET = 10; // さかくだりのジャイロオフセット値
69+ const float SLOPE_DOWN_GYRO_OFFSET2 = 10; // さかくだり2のジャイロオフセット値
70+ const float SLOPE_DOWN_GYRO_OFFSET3 = 2; // さかくだり3のジャイロオフセット値
7571 const float NORMAL_GYRO_OFFSET = 0; // 通常時のジャイロオフセット値
7672
7773 // カラーセンサー値関連
@@ -80,47 +76,36 @@
8076 // モーター回転角度関連
8177 const int MORTOR_ANG_START_FORWARD = 50; // (デバック用)スタート時のライントレース距離(左右モーターの合計回転角度)
8278 const int MORTOR_ANG_BEFORE_STEP_DOWN_LINE_TRACE = 90; // 段差を降りる前のライントレース距離(左右モーターの合計回転角度)
83- const int MORTOR_ANG_AFTER_STEP_DOWN_LINE_TRACE = 1670; // 段差を降りた後のライントレース距離(左右モーターの合計回転角度)
84- //const int MORTOR_ANG_BACKWARD = -350; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
85- const int MORTOR_ANG_BACKWARD = -200; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
86- const int MORTOR_ANG_STEP_UP = 230; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
87- const int MORTOR_ANG_SLOPE_UP = 520; // 坂を上る距離(左右モーターの合計回転角度)(一回目)
88- const int MORTOR_ANG_SLOPE_1ST_UP = 570;
89- const int MORTOR_ANG_SLOPE_DOWN = 105; // 坂を下る距離(左右モーターの合計回転角度)(一回目)
90- const int MORTOR_ANG_SLOPE_UP_BACK = 140; // 坂を上る距離(左右モーターの合計回転角度)(二回目バック)
91- const int MORTOR_ANG_SLOPE_UP_3RD = 420; // 坂を上る距離(左右モーターの合計回転角度)(三回目)
92- const int MORTOR_ANG_SLOPE_DOWN_3RD = 10; // 坂を下る距離(左右モーターの合計回転角度)(三回目)
93- const int MORTOR_ANG_STOP = 400; // 段差を昇ったあとから停止する位置の距離(左右モーターの合計回転角度)
94- const int MORTOR_ANG_STOP_RANGE = 90; // 段差を昇ったあとから停止する位置の誤差許容範囲(左右モーターの合計回転角度)
95- const int MORTOR_ANG_SEARCH_LINE_FORWARD = 90; // ライン探索時の前進距離(左右モーターの合計回転角度)
96- const int MORTOR_ANG_SEARCH_LINE_BACKWARD = -60; // ライン探索時の後進距離(左右モーターの合計回転角度)
97- const int MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_RIGHT = 30; // スピン時の距離(右モーターの回転角度)
98- const int MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_LEFT = -30; // スピン時の距離(左モーターの回転角度)
99- const int MORTOR_ANG_STEP_DOWN = 300; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
79+ const int MORTOR_ANG_BACKWARD = -220; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
80+ const int MORTOR_ANG_STEP_UP = 240; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
81+ const int MORTOR_ANG_SLOPE_UP = 650; // さかのぼり距離(左右モーターの合計回転角度)
82+ const int MORTOR_ANG_SLOPE_1ST_UP = 140; // さかのぼり中間の距離(左右モーターの合計回転角度)
83+ const int MORTOR_ANG_SLOPE_DOWN = 20; // さかくだり距離(左右モーターの合計回転角度)
84+ const int MORTOR_ANG_SLOPE_UP_BACK = 40; // さかくだり中間の距離(左右モーターの合計回転角度)
85+ const int MORTOR_ANG_STOP = 170; // 段差を昇ったあとから停止する位置の距離(左右モーターの合計回転角度)
86+ const int MORTOR_ANG_STEP_DOWN = 520; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
10087 const int MORTOR_ANG_FOR_STOP_DIFF_MAX = 30; // 段差上昇後の停止位置に移動する際、目標位置までの差分距離のMAX値。(これ以上は同じForward値となるよという閾値)
10188 const int MORTOR_ANG_ADJUST_LINE_FORWARD = 100; // ライントレース走行による位置調整時の前進距離(左右モーターの合計回転角度)
10289 const int MORTOR_ANG_ADJUST_LINE_BACKWARD = -80; // ライントレース走行による位置調整時の後進距離(左右モーターの合計回転角度)
103- const int MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD1 = 120;
104- const int MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD2 = 60;
10590
10691 // TURN値関連
107- const int TURN_NONE = 0; // ライントレース無し直線走行時の指定値1111
108- const int TURN_SPIN = -60; // スピン時の指定値
109- const int TURN_CHANGE_ANGLE = -20; // ライン探索時の車体方向角度変更時の指定値
110- const int TURN_MAX = 60; // TURN値のMAX値(絶対値)
92+ const int TURN_NONE = 0; // ライントレース無し直線走行時の指定値1111
93+ const int TURN_SPIN = -60; // スピン時の指定値
94+ const int TURN_CHANGE_ANGLE = -20; // ライン探索時の車体方向角度変更時の指定値
95+ const int TURN_MAX = 60; // TURN値のMAX値(絶対値)
11196 const int TURN_LINE_OVER1 = -10; // ラインをまたぐ際の値
112- const int TURN_LINE_OVER2 = 10; // ラインをまたぐ際の値
97+ const int TURN_LINE_OVER2 = 10; // ラインをまたぐ際の値
11398
11499
115100 // FORWARD(spped)値関連
116101 const int FORWARD_STEP_DETECTION = 30; // 段差検知時の指定値
117- const int FORWARD_BACKWARD = -60; // 段差を昇るための助走距離分バック走行時指定値
118- const int FORWARD_STEP_UP = 130; // 段差を昇る時の指定値
119- const int FORWARD_SLOPE_UP = 20; // aaaaaaaaaaaaaaaaaさかを上る時の指定値(一回目上り)
120- const int FORWARD_SLOPE_DOWN = 20; // aaaaaaaaaaaaaaaaaさかを下る時の指定値(一回目下り)
121- //const int FORWARD_SLOPE_UP_BACK = -100; // aaaaaaaaaaaaaaaaaさかを上る時の指定値(二回目バック上り)
122- const int FORWARD_SLOPE_DOWN_BACK = -150; // aaaaaaaaaaaaaaaaaさかを下る時の指定値(二回目バック下り)
123- const int FORWARD_SLOPE_2ND_DOWN_BACK = 20;
102+ const int FORWARD_BACKWARD = -40; // 段差を昇るための助走距離分バック走行時指定値
103+ const int FORWARD_STEP_UP = 140; // 段差を昇る時の指定値
104+ const int FORWARD_SLOPE_UP = 110; // さかを上る時の指定値(一回目上り)
105+ const int FORWARD_SLOPE_CENTER_UP = 60; // さかを上る時の指定値(一回目上り中間)
106+ const int FORWARD_SLOPE_DOWN = 20; // さかを下る時の指定値(一回目下り)
107+ const int FORWARD_SLOPE_DOWN_BACK = 50; // さかを下る時の指定値(二回目バック下り)
108+ const int FORWARD_SLOPE_2ND_DOWN_BACK = 50; // さかを下る時の指定値(二回目バック下り)
124109 const int FORWARD_MAX_FOR_STOP = 30; // 指定停止位置に移動してする際のMAX値
125110 const int FORWARD_SEARCH_LINE_FORWARD = 30; // ライン探索前進時の指定値
126111 const int FORWARD_SEARCH_LINE_BACKWARD = -30; // ライン探索後進時の指定値
@@ -130,7 +115,7 @@
130115 const int FORWARD_GRAY_SEARCH = 30; // グレーマーカー探索時の指定値
131116 const int FORWARD_ADJUST_LINE_FORWARD = 10; // ライントレース前進による位置調整時の指定値
132117 const int FORWARD_ADJUST_LINE_BACKWARD = -10; // ライントレース位置調整後の後進時の指定値
133- const int FORWARD_BEFORE_STOP = 10; // 停止前に尻尾を下すときの速度
118+ const int FORWARD_BEFORE_STOP = 10; // 停止前に尻尾を下すときの速度
134119 const int FORWARD_STOP = 0; //
135120
136121 //シッポ
@@ -163,22 +148,17 @@
163148 */
164149 enum SingleSeesawStates
165150 {
166-// SingleSeesawStateLineEdgeChangeForward0, // 0
167-// SingleSeesawStateLineEdgeChangeForward1, // 1
168-// SingleSeesawStateLineEdgeChangeForward2, // 2
169151 SingleSeesawState0stStepDetection, // 0 スタート時に少しの距離動く
170- SingleSeesawState1stStepDetection, // 3 1段目 段差検知モード
171- SingleSeesawState1stBackward, // 4 1段目 段差検知後バック走行モード
172- SingleSeesawState1stStepUp, // 5 1段目 昇段モード
173- SingleSeesawState1stSlopeUp, // 6 一回目 さかのぼり
174- SingleSeesawState1stCenterSlopeUp, // 中間
175- SingleSeesawState1stSlopeDown, // 7 一回目 さかくだり
176- SingleSeesawState2ndSlopeUpReverse, // 8 二回目 さかのぼり(バック)
177- SingleSeesawStateSlopeOut, // 坂降りるジャイロ
178- SingleSeesawStateSlopeOut2nd, // 坂おりる
179- SingleSeesawBeforeStop, // 9 しっぽが下りるまで動く
180- SingleSeesawStop, // 9 停止
181- SingleSeesawStateGraySearch, // 39 グレーライン検知モード
152+ SingleSeesawState1stStepDetection, // 1 1段目 段差検知モード
153+ SingleSeesawState1stBackward, // 2 1段目 段差検知後バック走行モード
154+ SingleSeesawState1stStepUp, // 3 1段目 昇段モード
155+ SingleSeesawState1stSlopeUp, // 4 さかのぼり
156+ SingleSeesawState1stCenterSlopeUp, // 5 さかのぼり2
157+ SingleSeesawState1stSlopeDown, // 6 さかくだり
158+ SingleSeesawState1stCenterSlopeDown, // 7 さかくだり2
159+ SingleSeesawStateSlopeOut, // 8 坂おりる
160+ SingleSeesawStateSlopeOut2nd, // 9 坂おりる2
161+ SingleSeesawStateGraySearch, // 10 グレーライン検知モード
182162
183163 };
184164 SingleSeesawStates seesawState; // 階段走行のステート
@@ -203,9 +183,7 @@
203183 {
204184 NoneTurnMode,
205185 ChangeAngleTurnMode,
206- LineTraceTurnMode,
207- LineOverMode1,
208- LineOverMode2,
186+ LineTraceTurnMode
209187 };
210188 ModeNormalizeTurnValues modeNormalizeTurnValue; // Turn値変更時の詳細モード
211189
@@ -219,17 +197,12 @@
219197 BackwardMode,
220198 StepUpMode,
221199 SlopeUpMode,
200+ SlopeUpCenterMode,
222201 SlopeDownMode,
223202 SlopeDownReverseMode,
224203 SlopeDownReverse2ndMode,
225- SlopeDown2ndMode,
226204 StopMode,
227205 BeforeStopMode,
228- AdjustPostionMode,
229-// StopMode,
230- SearchLineForwardMode,
231- SearchLineBackwardMode,
232- SearchLineChangeAngleMode,
233206 LineTraceMode,
234207 StepDownMode,
235208 GraySearchMode,
Show on old repository browser