PrograminFOLO_for_M5Atom.ino

#include "M5AtomS3.h"

// M5Atomとmicro:bitエッジコネクタの対応
#define MB_PIN1 7 
#define MB_PIN2 8
#define MB_PIN12 1 
#define MB_PIN13 5 
#define MB_PIN14 6 
#define MB_PIN15 39 
#define MB_PIN16 38

bool moveMode = false ;
int btnMode = 0 ;

void setup() {
  M5.begin();
  M5.Lcd.begin();
  M5.Lcd.fillScreen(BLACK);           // 画面背景(指定色で画面全体を塗りつぶす。表示を更新する場合にも使用)
  M5.Lcd.setRotation(2);              // 180度回転
  M5.Lcd.setTextFont(4);              // フォント(フォント番号：0,2,4,6,7,8の中から指定)
  M5.Lcd.setCursor(0, 0);             // テキスト表示座標(x座標, y座標)
  M5.Lcd.setTextSize(1);              // テキストサイズ倍率(整数で指定)
  M5.Lcd.setTextColor(YELLOW, BLACK);  // テキスト色(文字色, 文字背景)
  M5.Lcd.println("M5AtomS3");
  M5.Lcd.setTextColor(WHITE, BLACK);  // テキスト色(文字色, 文字背景)

  // IR Senser
  pinMode(MB_PIN12,OUTPUT) ;
  digitalWrite(MB_PIN12,HIGH) ;

  // MOVE MOTER
  pinMode(MB_PIN13,OUTPUT) ;
  pinMode(MB_PIN14,OUTPUT) ;
  digitalWrite(MB_PIN13,LOW) ;
  digitalWrite(MB_PIN14,LOW) ;

  // ROTATE MOTER
  pinMode(MB_PIN15,OUTPUT) ;
  pinMode(MB_PIN16,OUTPUT) ;
  digitalWrite(MB_PIN15,LOW) ;
  digitalWrite(MB_PIN16,LOW) ;
}

void loop() {
  // --- Mode Select
  int readValue = analogRead(G2) ;
  int btn = 0 ;
  if (readValue > 3500) btn = 0 ;
  else if (readValue > 2500) btn = 1 ;
  else if (readValue > 1800) btn = 2 ;
  else btn = 3 ;
  if (btnMode != btn) {
    btnMode = btn ;
    M5.Lcd.setCursor(0, 30);
    M5.Lcd.print("BTN:");
    switch(btn) {
      case 0 : M5.Lcd.print("-        ");
        break ;
      case 1 : M5.Lcd.print("A      ");
        moveMode = true ;
        break ;
      case 2 : M5.Lcd.print("B      ");
        moveMode = false ;
        break ;
      case 3 : M5.Lcd.print("A+B");
        break ;
    }
  }

  // --- IR Senser
  int senserRight = analogRead(MB_PIN1) ;
  int senserLeft  = analogRead(MB_PIN2) ;
  M5.Lcd.setCursor(0, 60);
  M5.Lcd.print("R:");  M5.Lcd.print(senserRight);  M5.Lcd.print("    ");
  M5.Lcd.setCursor(0, 90);
  M5.Lcd.print("L:");  M5.Lcd.print(senserLeft);  M5.Lcd.print("    ");

  // 移動処理
  if (moveMode) {
    if (senserRight > 1000 && senserLeft > 1000) {
      // 障害物が前面にあり右回転
      digitalWrite(MB_PIN13,LOW) ;
      digitalWrite(MB_PIN16,HIGH) ;
      while(senserRight > 600 || senserLeft > 600) {
        senserRight = analogRead(MB_PIN1) ;
        senserLeft  = analogRead(MB_PIN2) ;
      }
      delay(400) ;
      digitalWrite(MB_PIN16,LOW) ;
      digitalWrite(MB_PIN13,HIGH) ;
    } else
    if (senserRight > 1000) {
       // 左に旋回
       digitalWrite(MB_PIN13,LOW) ;
       digitalWrite(MB_PIN15,HIGH) ;
       while(senserRight > 600) {
         senserRight = analogRead(MB_PIN1) ;
       }
       delay(400) ;
       digitalWrite(MB_PIN15,LOW) ;
       digitalWrite(MB_PIN13,HIGH) ;
    } else
    if (senserLeft > 1000) {
       // 右に旋回
       digitalWrite(MB_PIN13,LOW) ;
       digitalWrite(MB_PIN16,HIGH) ;
       while(senserLeft > 600) {
         senserLeft = analogRead(MB_PIN2) ;
       }
       delay(400) ;
       digitalWrite(MB_PIN16,LOW) ;
       digitalWrite(MB_PIN13,HIGH) ;
    } else {
      digitalWrite(MB_PIN13,HIGH) ;
    }
  }

  delay(200) ;
}