ソース部分作成3 (1-9.ソース：共通関数) [ロボット制作(tankun-mk1)]

おはようぼざいます。結局、遅くなりました

前回、中途半端になっていた各関数を紹介していきます。

各関数を作成しているのは色んな箇所で似たような処理をする部分を
一か所にまとめることで単純に書くソース量を減らすのと、
変更があった場合に修正箇所を少なくできるためです

[モータ操作]
まずはキャタピラ部分のモータ操作。
呼び元からは「前進」「右旋回」など動く方向だけを与えて、
関数の中で両モータの前進・後退を制御します。

void motor_move( int mode ) {
  //                   GO    STOP  LEFT  RIGHT  BACK
  int motor_0_0[6] = { HIGH, HIGH, LOW,  HIGH,  LOW};
  int motor_0_1[6] = { LOW,  HIGH, HIGH, LOW,   HIGH};
  int motor_1_0[6] = { HIGH, HIGH, HIGH, LOW,   LOW};
  int motor_1_1[6] = { LOW,  HIGH, LOW,  HIGH,  HIGH};
  short delay_time[6] = { 800, 1000, 1100, 1100, 1000 };

  digitalWrite( PIN_MTR_LEFT_GO,    motor_0_0[mode] );
  digitalWrite( PIN_MTR_LEFT_BACK,  motor_0_1[mode] );
  digitalWrite( PIN_MTR_RIGHT_GO,   motor_1_0[mode] );
  digitalWrite( PIN_MTR_RIGHT_BACK, motor_1_1[mode] );
  
  delay( delay_time[mode] );
  return;
}

[超音波センサの距離取得]
超音波センサから距離を取得する関数と、
前方に障害物があるかチェックする関数です。
チェックは引数でエラー距離を受ける事で汎用的にしています。

int check_sonic_sensor_dst( int err_dst, int mode) {
  float Distance;

  // ----------------------------------------------
  // 距離を取得
  // ----------------------------------------------
  Distance = get_sonic_sensor_dst();
  if ( Distance == 3 ) {
    Distance = get_sonic_sensor_dst();
  }

  if ( ( Distance  >= 5 ) && ( Distance <= err_dst ) ) {
    motor_move(MTR_SET_MODE_STOP);
    // 引数で受けたエラー距離を超えた
    Serial.print("kyo'ri  senti',me-'toru");
    delay(3500);

    return 1;
  }

//  if( mode == 1 ) {
//    Serial.print("kyo'ri  senti',me-'toru");
//    delay(3000);
//  }
  
  return 0;
}

int get_sonic_sensor_dst() {
  int Duration;
  float Distance;

  int pin_trig=PIN_SENSOR_FRONT_TRIG;
  int pin_echo=PIN_SENSOR_FRONT_ECHO;

  // 距離を求めるおまじない  
  digitalWrite( pin_trig, LOW );
  delayMicroseconds(1);
  digitalWrite( pin_trig, HIGH );
  delayMicroseconds(1);
  digitalWrite( pin_trig, LOW );

  // 距離を取得
  Duration = pulseIn( pin_echo, HIGH );
//    Serial.print(Duration);
//    Serial.println("Duration");
  if( Duration > 0 ) {
    // 距離をcmに変換
    Distance = Duration /2;
    Distance = Distance*340*100/1000000;

//    Serial.print(Distance);
//    Serial.println("Distance");

    return Distance;
  }else {
    return 3;
  }
}

[サーボの制御]
引数で受けた角度にサーボを回転させて、一定時間のディレイをいれてます

void exec_sensor_servo_ungle(int ungle) {
  servo_front.write(ungle);
  delay(SERVO_UNGLE_DELAY_TIME);

  return;
}

[次の行き先チェック]
この関数は
　１．進みたい方向にサーボを回転させて
　２．超音波センサで、その方向に障害物があるかチェックして
　３．障害物がなければ進む
という動きです。
先に右をチェックして、障害物があれば左をチェックして、
さらに障害物がある場合は少し下がっています。

void set_next_away() {
  int Dst_Check=0;
  int Mode=MTR_SET_MODE_RIGHT;

  exec_sensor_servo_ungle(SERVO_UNGLE_RIGHT);
  delay(1000);
  Dst_Check=check_sonic_sensor_dst(SENSOR_SIDE_ERROR_DST, 1);
  if ( Dst_Check == 0 ) {
    delay(500);
    Serial.println("mi'gini;susu'mi,masu'");
    delay(3000);
  }else {
    delay(100);
    exec_sensor_servo_ungle(SERVO_UNGLE_FRONT);
    delay(100);
    exec_sensor_servo_ungle(SERVO_UNGLE_LEFT);
    delay(500);
    Dst_Check=check_sonic_sensor_dst(SENSOR_SIDE_ERROR_DST, 1);
    if ( Dst_Check == 0 ) {
      Mode=MTR_SET_MODE_LEFT;
      delay(500);
//      Serial.println("hida'rini;maga'ri,ma'-su");
      Serial.println("hida'rini;susu'mi,masu'");
      delay(3000);
    }
  }

    delay(100);
  exec_sensor_servo_ungle(SERVO_UNGLE_FRONT);
    delay(100);

  if ( Dst_Check == 0 ) {
    motor_move(Mode);
    motor_move(MTR_SET_MODE_STOP);
    delay(100);
    motor_move(MTR_SET_MODE_STOP);
    motor_move(MTR_SET_MODE_GO);
    
  }else {
    motor_move(MTR_SET_MODE_BACK);
  }

  return;
}

かなり長くなりましたがこんな感じです。
各関数の説明が雑なので、折を見て補足していきますね。

では次回は、、、ついに完成かも