Revision | 428 (tree) |
---|---|
Zeit | 2019-09-15 13:56:46 |
Autor | ncxx-watanabe |
(empty log message)
@@ -20,7 +20,7 @@ | ||
20 | 20 | const signed int interval1stTailAngle = 78; |
21 | 21 | const signed int interval2ndTailAngle = 72; |
22 | 22 | //const signed int interval3ndTailAngle = 68; |
23 | -const signed int throughGateTailAngle = 66; | |
23 | +const signed int throughGateTailAngle = 65; | |
24 | 24 | |
25 | 25 | void LookupGate::Run() { |
26 | 26 | // 左右モータ総合回転角平均 |
@@ -142,7 +142,7 @@ | ||
142 | 142 | // ライントレース(84度想定) |
143 | 143 | tail_control(standTailAngle); |
144 | 144 | LineTraceWithTail(); |
145 | - if (125 < ++waitCount) { | |
145 | + if (200 < ++waitCount) { | |
146 | 146 | waitCount = 0; |
147 | 147 | lookupGateState = LookupGateStateThroughGateForwardAjustment; |
148 | 148 | log("To SpinRight\r\n"); |
@@ -256,10 +256,10 @@ | ||
256 | 256 | case LookupGateStateThroughGateReady8th: |
257 | 257 | break; |
258 | 258 | case LookupGateStateThroughGateForward: |
259 | - forward = 12; | |
259 | + forward = 24; | |
260 | 260 | break; |
261 | 261 | case LookupGateStateThroughGateForwardAjustment: |
262 | - forward = 8; | |
262 | + forward = 20; | |
263 | 263 | break; |
264 | 264 | case LookupGateStateBodyDown1st: |
265 | 265 | case LookupGateStateBodyDown2nd: |
@@ -325,7 +325,7 @@ | ||
325 | 325 | void LookupGate::throughGateReady2nd() { |
326 | 326 | runMode = run66; |
327 | 327 | BaseCourse::Run(); |
328 | - if (++counter >= 250) { | |
328 | + if (++counter >= 50) { | |
329 | 329 | log("LookupGateStateThroughGateReady3rd\r\n"); |
330 | 330 | lookupGateState = LookupGateStateThroughGateReady3rd; |
331 | 331 | counter = 0; |
@@ -408,7 +408,7 @@ | ||
408 | 408 | forwardMotorAngL = motor_ang_l; |
409 | 409 | } |
410 | 410 | } else { |
411 | - if (350 < motor_ang_l - forwardMotorAngL) { | |
411 | + if (450 < motor_ang_l - forwardMotorAngL) { | |
412 | 412 | log("LookupGateStateThroughGateStop\r\n"); |
413 | 413 | lookupGateState = LookupGateStateThroughGateStop; |
414 | 414 | } |
@@ -468,7 +468,7 @@ | ||
468 | 468 | BaseCourse::Run(); |
469 | 469 | if(tail_control(standTailAngle)) { |
470 | 470 | UpridhtBodyCount++; |
471 | - if(UpridhtBodyCount != 5){ | |
471 | + if(UpridhtBodyCount != 3){ | |
472 | 472 | log("LookupGateStateBeforeSpin\r\n"); |
473 | 473 | lookupGateState = LookupGateStateBeforeSpin; |
474 | 474 | } else { |
@@ -509,14 +509,17 @@ | ||
509 | 509 | // const int LightGray = 3; |
510 | 510 | const int LightBlack = 0; |
511 | 511 | const int targetColor = (LightWhite + LightBlack) / 2; |
512 | - const int defaultPower = 5; | |
513 | - const int additionalPower = 5; | |
512 | + const int defaultPower = 10; | |
513 | + const int additionalPower = 10; | |
514 | 514 | if (forward > 0) { |
515 | 515 | int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor); |
516 | - pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
517 | - pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
518 | - //pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
519 | - //pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
516 | + if(colorDiff <= 10){ | |
517 | + pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
518 | + pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
519 | + }else { | |
520 | + pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
521 | + pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
522 | + } | |
520 | 523 | } else if(forward == 0) { |
521 | 524 | pwm_L = 0; |
522 | 525 | pwm_R = 0; |
@@ -525,18 +528,21 @@ | ||
525 | 528 | pwm_R = forward; |
526 | 529 | } |
527 | 530 | } else if(runMode == run84) { |
528 | - const int LightWhite = 16; | |
531 | + const int LightWhite = 12; | |
529 | 532 | // const int LightGray = 9; |
530 | 533 | const int LightBlack = 0; |
531 | 534 | const int targetColor = (LightWhite + LightBlack) / 2; |
532 | - const int defaultPower = 5; | |
533 | - const int additionalPower = 5; | |
535 | + const int defaultPower = 10; | |
536 | + const int additionalPower = 10; | |
534 | 537 | if (forward > 0) { |
535 | 538 | int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor); |
536 | - //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
537 | - //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
538 | - pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
539 | - pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
539 | + if(colorDiff <= 10){ | |
540 | + pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
541 | + pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
542 | + }else { | |
543 | + pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0); | |
544 | + pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0); | |
545 | + } | |
540 | 546 | } else if(forward == 0) { |
541 | 547 | pwm_L = 0; |
542 | 548 | pwm_R = 0; |
@@ -549,7 +555,7 @@ | ||
549 | 555 | void LookupGate::GraySerchRun() { |
550 | 556 | runMode = run84; |
551 | 557 | BaseCourse::Run(); |
552 | - if (340 < (motor_ang_l - forwardMotorAngL)) { | |
558 | + if (512 < (motor_ang_l - forwardMotorAngL)) { | |
553 | 559 | log("LookupGateStateChangeGarage\r\n"); |
554 | 560 | lookupGateState = LookupGateStateChangeGarage; |
555 | 561 | } |
@@ -570,6 +576,7 @@ | ||
570 | 576 | // グレーサーチモードで、 |
571 | 577 | if (lookupGateState == LookupGateStateChangeGarage) { |
572 | 578 | ev3_motor_stop(tail_motor, true); |
579 | + log("LookupGateStateChangeGarage_STOP\r\n"); | |
573 | 580 | result = true; |
574 | 581 | } |
575 | 582 | return result; |
@@ -603,13 +610,13 @@ | ||
603 | 610 | bool LookupGate::SpinRight() { |
604 | 611 | int32_t motorAngle = ev3_motor_get_counts(right_motor); |
605 | 612 | if (targetSpinCount == 0) { |
606 | - targetSpinCount = motorAngle + 300; | |
613 | + targetSpinCount = motorAngle + 370; | |
607 | 614 | //targetSpinCount = motorAngle - 330; |
608 | 615 | ev3_motor_stop(left_motor, true); |
609 | 616 | } |
610 | 617 | if (motorAngle < targetSpinCount) { |
611 | 618 | // log("target:%d current:%d\r\n", targetSpinCount, motorAngle); |
612 | - ev3_motor_set_power(right_motor, 10); | |
619 | + ev3_motor_set_power(right_motor, 20); | |
613 | 620 | return false; |
614 | 621 | } else { |
615 | 622 | ev3_motor_stop(right_motor, true); |
@@ -621,7 +628,7 @@ | ||
621 | 628 | bool LookupGate::SpinLeft() { |
622 | 629 | int32_t motorAngle = ev3_motor_get_counts(left_motor); |
623 | 630 | if (targetSpinCount == 0) { |
624 | - targetSpinCount = motorAngle - 360; | |
631 | + targetSpinCount = motorAngle - 300; | |
625 | 632 | // targetSpinCountBack = motorAngle - 600; |
626 | 633 | //targetSpinCount = motorAngle + 390; |
627 | 634 | ev3_motor_stop(right_motor, true); |
@@ -628,7 +635,7 @@ | ||
628 | 635 | } |
629 | 636 | if (targetSpinCount < motorAngle) { |
630 | 637 | // log("target:%d current:%d\r\n", targetSpinCount, motorAngle); |
631 | - ev3_motor_set_power(left_motor, -10); | |
638 | + ev3_motor_set_power(left_motor, -20); | |
632 | 639 | return false; |
633 | 640 | } else { |
634 | 641 | // if(targetSpinCountBack < motorAngle){ |