FOLO-01.ino

// 改造FOLO(M5Samp PICO)用 ベーススケッチ
// コレをベースに機能を付け足していく予定

int mode = 0 ;
const int BTN = 36 ;

// 目玉用LED
const int LED_R = 3 ;
const int LED_L = 1 ;

// 赤外線レーダー
const int SCN_R = 33 ;
const int SCN_L = 32 ;

// モータードライバ
const int OUT_AEN = 25 ; // 前後
const int OUT_APH = 19 ; // HL:前 LH:後 HH:停止
const int OUT_BEN = 26 ; // 左右
const int OUT_BPH = 18 ; // HL:右 LH:左 HH:停止

const int SPK_PIN = 0 ;
// 動作モード切り替えアナウンス用のメロディーデータ
static int melody[][2] = {
{262,450},{0,50}, {262,450},{0,50}, {392,450},{0,50},
{392,450},{0,50}, {440,450},{0,50}, {440,450},{0,50}, 
{392,1000}, {349,450},{0,50}, {349,450},{0,50},
{330,450},{0,50}, {330,450},{0,50}, {294,450},{0,50}, 
{294,450},{0,50}, {262,1000}, {0,0} } ;

void setup() {
  pinMode(BTN,INPUT);
  pinMode(SPK_PIN, OUTPUT);

  pinMode(LED_R,OUTPUT);
  pinMode(LED_L,OUTPUT);
  digitalWrite(LED_R,HIGH) ;
  digitalWrite(LED_L,HIGH) ;

  pinMode(OUT_APH,OUTPUT) ;
  pinMode(OUT_AEN,OUTPUT) ;
  pinMode(OUT_BPH,OUTPUT) ;
  pinMode(OUT_BEN,OUTPUT) ;
  digitalWrite(OUT_APH,HIGH) ;
  digitalWrite(OUT_AEN,HIGH) ;
  digitalWrite(OUT_BPH,HIGH) ;
  digitalWrite(OUT_BEN,HIGH) ;
}

void loop() {
  int M = digitalRead(BTN) ;
  if (M == HIGH) {
    mode = (mode +1) % 2 ;
    // 動作モード
    //   0:停止
    //   1:移動
    if (mode == 1) {
      // 移動モードに移行した際に目玉を点滅＆メロディー演奏
      for (int i=0;i<3;i++) {
        digitalWrite(LED_R,HIGH) ;
        digitalWrite(LED_L,HIGH) ;
        delay(500) ;
        digitalWrite(LED_R,LOW) ;
        digitalWrite(LED_L,LOW) ;
        delay(500) ;
      }
      melodyPlay( ) ;
    } else {
      // 停止モード移行した際に目玉を点灯＆モーター停止
      digitalWrite(LED_R,HIGH) ;
      digitalWrite(LED_L,HIGH) ;

      digitalWrite(OUT_APH,HIGH) ;
      digitalWrite(OUT_AEN,HIGH) ;

      digitalWrite(OUT_BPH,HIGH) ;
      digitalWrite(OUT_BEN,HIGH) ;
    }
  }

  if (mode == 1) {
    // 移動モードの処理
    int R = analogRead(SCN_R) ;
    int L = analogRead(SCN_L) ;

    if (R == 0 && L == 0) {
      // 停止
      digitalWrite(OUT_APH,HIGH) ;
      digitalWrite(OUT_AEN,HIGH) ;
    } else {
      // 前進
      digitalWrite(OUT_APH,HIGH) ;
      digitalWrite(OUT_AEN,LOW) ;
    }

    if (R >3000 && L > 3000) {
      // 旋回停止
      digitalWrite(LED_R,HIGH) ;
      digitalWrite(LED_L,HIGH) ;
      digitalWrite(OUT_BPH,HIGH) ;
      digitalWrite(OUT_BEN,HIGH) ;
      
    } else
    if (R < 3000) {
      // 左旋回
      digitalWrite(LED_R,LOW) ;
      digitalWrite(LED_L,HIGH) ;
      digitalWrite(OUT_BPH,LOW) ;
      digitalWrite(OUT_BEN,HIGH) ;
    } else
    if (L < 3000) {
      // 右旋回
      digitalWrite(LED_R,HIGH) ;
      digitalWrite(LED_L,LOW) ;
      digitalWrite(OUT_BPH,HIGH) ;
      digitalWrite(OUT_BEN,LOW) ;
    }
  }
  delay(200) ;
}

// メロディー演奏
void melodyPlay( ) {
    int i ;
    for (i=0;melody[i][0] != 0 || melody[i][1] != 0;i++) {
      if (melody[i][0] == 0) {
        noTone(SPK_PIN);
      } else {
        tone(SPK_PIN, melody[i][0]);
      }
      delay(melody[i][1]);
    }
    noTone(SPK_PIN);
}