• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision424 (tree)
Zeit2019-08-31 17:15:52
Autorncxx-watanabe

Log Message

(empty log message)

Ändern Zusammenfassung

Diff

--- trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/LookupGate.cpp (revision 423)
+++ trunk/src/hrp2_beta7/sdk/workspace/sample_cpp2/course/LookupGate.cpp (revision 424)
@@ -23,6 +23,16 @@
2323 const signed int throughGateTailAngle = 66;
2424
2525 void LookupGate::Run() {
26+ // 左右モータ総合回転角平均
27+ // double totalAvg = (motor_ang_r + motor_ang_l) / 2;
28+ //forward = 20;
29+
30+ //if(totalAvg < 40) {
31+ // gyro_offset = -12;
32+ //}
33+ //else {
34+ // gyro_offset = 0;
35+ //}
2636 sonar_alert();
2737 switch(lookupGateState) {
2838 case LookupGateStateRun:
@@ -125,7 +135,7 @@
125135 else if (125 < ++waitCount) {
126136 waitCount = 0;
127137 lookupGateState = LookupGateStateAfterSpin;
128- log("To SpinLeft\r\n");
138+ log("To LookupGateStateAfterSpin\r\n");
129139 }
130140 break;
131141 case LookupGateStateAfterSpin:
@@ -384,7 +394,7 @@
384394 void LookupGate::ForwardRunAjustment() {
385395 runMode = run84;
386396 BaseCourse::Run();
387- if ((distance <= 5) && (distance >= 0)) {
397+ if ((distance <= 30) && (distance >= 0)) {
388398 log("LookupGateStateBodyDown1st\r\n");
389399 lookupGateState = LookupGateStateBodyDown1st;
390400 }
@@ -398,7 +408,7 @@
398408 forwardMotorAngL = motor_ang_l;
399409 }
400410 } else {
401- if (170 < motor_ang_l - forwardMotorAngL) {
411+ if (350 < motor_ang_l - forwardMotorAngL) {
402412 log("LookupGateStateThroughGateStop\r\n");
403413 lookupGateState = LookupGateStateThroughGateStop;
404414 }
@@ -408,7 +418,7 @@
408418 void LookupGate::RunStop() {
409419 runMode = run66;
410420 BaseCourse::Run();
411- if (++counter >= 100) {
421+ if (++counter >= 150) {
412422 log("LookupGateStateUpridhtBody1st\r\n");
413423 lookupGateState = LookupGateStateUpridhtBody1st;
414424 counter = 0;
@@ -458,7 +468,7 @@
458468 BaseCourse::Run();
459469 if(tail_control(standTailAngle)) {
460470 UpridhtBodyCount++;
461- if(UpridhtBodyCount != 3){
471+ if(UpridhtBodyCount != 5){
462472 log("LookupGateStateBeforeSpin\r\n");
463473 lookupGateState = LookupGateStateBeforeSpin;
464474 } else {
@@ -469,21 +479,6 @@
469479 }
470480 }
471481
472-void LookupGate::updatePidValue() {
473-
474- const signed int LIGHT_WHITE2 = 48;
475- const signed int LIGHT_BLACK2 = 0;
476-
477- // 黒と白の中間を走るPID制御を実施
478- PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
479- turn = PIDMgr->GetTurnValue() * (-1);
480-}
481-
482-void LookupGate::SetPIDData() {
483- // PID制御に使用する定数を設定する
484- PIDMgr->SetFixedData(1.4, 1, 0.03);
485-}
486-
487482 bool LookupGate::tail_control(signed int angle) {
488483 if (angle < 50) {
489484 angle = 50;
@@ -510,7 +505,7 @@
510505 if(runMode == normalRun) {
511506 BaseCourse::calcMotorPower();
512507 } else if(runMode == run66) {
513- const int LightWhite = 12;
508+ const int LightWhite = 3;
514509 // const int LightGray = 3;
515510 const int LightBlack = 0;
516511 const int targetColor = (LightWhite + LightBlack) / 2;
@@ -518,10 +513,10 @@
518513 const int additionalPower = 5;
519514 if (forward > 0) {
520515 int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
521- //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
522- //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
523- pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
524- pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
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);
525520 } else if(forward == 0) {
526521 pwm_L = 0;
527522 pwm_R = 0;
@@ -591,7 +586,7 @@
591586 static int alert = 0;
592587 /* 約40msec周期毎に障害物検知 */
593588 if (++counter == 40/4) {
594-// log("distance : %d\r\n", distance);
589+ log("distance : %d\r\n", distance);
595590 distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
596591 if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0)) {
597592 alert = 1; /* 障害物を検知 */
@@ -651,4 +646,19 @@
651646 void LookupGate::LineTraceWithTail() {
652647 runMode = run84;
653648 BaseCourse::Run();
654-}
\ No newline at end of file
649+}
650+
651+void LookupGate::updatePidValue() {
652+
653+ const signed int LIGHT_WHITE2 = 48;
654+ const signed int LIGHT_BLACK2 = 0;
655+
656+ // 黒と白の中間を走るPID制御を実施
657+ PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
658+ turn = PIDMgr->GetTurnValue() * (-1);
659+}
660+
661+void LookupGate::SetPIDData() {
662+ // PID制御に使用する定数を設定する
663+ PIDMgr->SetFixedData(1.4, 1, 0.03);
664+}
Show on old repository browser