TAMIYA-M5AtomS3.ino

// TAMIYA マイコンロボット工作セット (クローラータイプ)
// M5AtomS3 でコントロールするサンプル

#include "M5AtomS3.h"
#include "Wire.h"
const uint8_t SonnerAddr = 0x2C ; 
const int PWMCH_R = 0; // 右モーターのPWMチャンネル
const int PWMCH_L = 1; // 左モーターのPWMチャンネル
const int PIN_R = 5;   // 右モーター
const int PIN_L = 6;   // 左モーター
const int PIN_RB = 7;  // 右ブレーキ
const int PIN_LB = 8;  // 左ブレーキ
const int N_LEV = 511 ;// ニュートラル時の出力レベル
const int F_LEV = 1023;// 前進時の出力レベル
const int R_LEV = 0 ;// 後退時の出力レベル
int moveMode = 0 ;  // 移動モード 0:停止 1:移動 2:テスト
int turnMode = 0 ;  // 回転モード 0:直進 1:左旋回 2:右旋回

const int SPK_PIN = G1 ;
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 melodyPlay( ) {
  int i ;
  moveMode = 3 ;
  dispMoveMode( ) ;
  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);
  moveMode = 0 ;
  dispMoveMode( ) ;
}

// == 超音波距離センサー
// 連続して呼び出すとReadyが返ってこなくなる200mS位の間隔を開けると良いみたい
int sonicSonerRead() {
  uint8_t readData ;
  int senserData ;
  unsigned long time;
  // --- Ready Check
  Wire.beginTransmission(SonnerAddr);
  Wire.write(51);
  Wire.endTransmission();
  // -- Result --
  Wire.requestFrom(SonnerAddr, 1);
  readData = 0 ;
  time = millis();
  while(millis() - time < 1000){ // データが読み出せなくなることがあるようなのでタイムアウトを設定している。
    if (Wire.available() >= 1) {
      readData = Wire.read();
      break ;
    }
  }
  if (readData != 1) return 0 ;

  // -- センサーデータ --
  Wire.beginTransmission(SonnerAddr);
  Wire.write(14);
  Wire.endTransmission();

  // 2Byte Read
  Wire.requestFrom(SonnerAddr, 2);
  readData = 0 ;
  bool timeOut = true ;
  time = millis();
  while(millis() - time < 1000){ // データが読み出せなくなることがあるようなのでタイムアウトを設定している。
    if (Wire.available() >= 2) {
      readData = Wire.read();
      senserData = Wire.read();
      timeOut = false ;
      break ;
    }
  }
  if (timeOut) return 0 ;

  senserData = senserData << 8 | readData ;
  return senserData ;
}

// == 往復時間から距離(cm)に変換
float distanceCalculation(int data) {
  if (data <= 0) return 0.0 ;
  float dist = (int)((data-160) / 2 * 0.315) / 10. ;
  return dist ;
}

// ==　モーター
// moveMoter(data) 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
void moveMoter(int data) {
  int rLev = N_LEV ;
  int lLev = N_LEV ;
  bool BLK = false ;
  switch(data) {
    case 0: BLK = true ; break ;
    case 1: rLev = F_LEV ; lLev = F_LEV ; break ;
    case 2: rLev = R_LEV ; lLev = R_LEV ; break ;
    case 3: rLev = F_LEV ; lLev = R_LEV ; break ;
    case 4: rLev = R_LEV ; lLev = F_LEV ; break ;
    case 5: rLev = N_LEV ; lLev = N_LEV ; break ;
  }
  ledcWrite(PWMCH_R,rLev) ;
  ledcWrite(PWMCH_L,lLev) ;
  digitalWrite(PIN_RB,BLK) ;
  digitalWrite(PIN_LB,BLK) ;
}

// ----- MOVE TEST
void moveTest( ) {
  moveMode = 2 ;
  dispMoveMode( ) ;

  // 2秒直進
  moveMoter(1); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
  delay(2000) ;
  // 1.5秒右旋回
  moveMoter(4); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
  delay(1500) ;
  // 2秒直進
  moveMoter(1); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
  delay(2000) ;
  // 1.5秒左旋回
  moveMoter(3); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
  delay(1500) ;
  // 2秒後退
  moveMoter(2); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
  delay(2000) ;
  // 停止
  moveMoter(0); // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL

  moveMode = 0 ;
  dispMoveMode( ) ;
}

// -- LCD表示
// 動作モードを表示
void dispMoveMode() {
  M5.Lcd.setCursor(0, 30);
  switch(moveMode) {
    case 0: M5.Lcd.print("STOP    "); break ;
    case 1: M5.Lcd.print("MOVE    "); break ;
    case 2: M5.Lcd.print("TEST    "); break ;
    case 3: M5.Lcd.print("PLAY    "); break ;
  }
}

// センサー読み取り値を表示
void dispSenser(int dist) {
  M5.Lcd.setCursor(0, 60);
  M5.Lcd.print("SS:");
  M5.Lcd.print(dist);
  M5.Lcd.print("   ");
}

// 障害物との距離を表示
void dispDist(float dist) {
  M5.Lcd.setCursor(0, 90);
  M5.Lcd.print(dist);
  M5.Lcd.print("cm   ");
}

// ----- 初期化
void setup() {
  M5.begin();
  M5.Lcd.begin();
  M5.Lcd.fillScreen(BLACK);           // 画面背景(指定色で画面全体を塗りつぶす。表示を更新する場合にも使用)
  M5.Lcd.setRotation(3);              // 90度回転
  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);  // テキスト色(文字色, 文字背景)

  Wire.begin(39,38);    // I2C SDA-39,SCL-38 :: AtomS3

  // ブレーキ 
  pinMode(PIN_RB, OUTPUT);
  pinMode(PIN_LB, OUTPUT);
  digitalWrite(PIN_RB,HIGH) ;
  digitalWrite(PIN_LB,HIGH) ;

  // モーター
  moveMode = 0 ;  // 移動モード 0:停止 1:移動 2:テスト
  turnMode = 0 ;  // 回転モード 0:直進 1:左旋回 2~:右旋回
  ledcSetup(PWMCH_R, 1000, 10);   // 周波数 1KHz、解像度 10Bit
  ledcAttachPin(PIN_R, PWMCH_R);  // GPIOピンにPWMチャンネルを紐づけ
  ledcWrite(PWMCH_R,N_LEV) ;      // PWMチャンネルに値を設定：511-0x100 : デューティー比50%

  ledcSetup(PWMCH_L, 1000, 10);   // 周波数 1KHz、解像度 10Bit
  ledcAttachPin(PIN_L, PWMCH_L);  // GPIOピンにPWMチャンネルを紐づけ
  ledcWrite(PWMCH_R,N_LEV) ;      // PWMチャンネルに値を設定：511-0x100 : デューティー比50%

  moveMoter(0);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL

  // 表示：初期状態
  dispMoveMode( ) ;
  dispSenser(0) ;
  dispDist(0.0) ;
}

// ----- 移動処理
void loop() {
  // --- M5Atom BUTTON
  M5.update(); // update button state
  if (M5.BtnA.wasClicked()){
    // 移動モードを切り替える
    moveMode = (moveMode + 1) % 2 ;
    dispMoveMode( ) ;
    if (moveMode == 0) {
      moveMoter(0);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
    } else {
      delay(10000) ; // 10秒待つ※蓋を閉める時間を確保するため。なくても良い。
    }
  }

  // --- micro:bit BUTTON
  int readValue = analogRead(G2) ;
  int btn = 0 ;
  if (readValue < 3500) {
    if (readValue > 2500) {
      // - A - : MOVE TEST
      moveTest( ) ;
    } else if (readValue > 1800) {
      // - B - : Sound TEST
      melodyPlay( ) ;
    } else {
      // - A+B -
    }
  }
  // -- 超音波距離センサー読み取り
  int distInt = sonicSonerRead( ) ;
  if (distInt > 0) {
    dispSenser(distInt);
    float dist = distanceCalculation(distInt) ;
    dispDist(dist) ;
    if (moveMode == 1) {
      if (dist > 15.0) {
        // 障害物なし
        turnMode = 0;
        moveMoter(1);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
      } else {
        // 障害物があったら転回する。
        // 最初は左旋回、2回目以降は右旋回
        // 8回旋回してもダメならバックしてみる。
        turnMode ++ ;
        if (turnMode == 1) {
          moveMoter(3);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
          delay(1600) ;
          moveMoter(0);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
        } else
        if (turnMode < 9){
          moveMoter(4);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
          delay(800) ;
          moveMoter(0);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
        } else {
          turnMode = 0;
          moveMoter(2);  // 0:STOP 1:FOWARD 2:BACKWARD 3:LEFT TERN 4:RIGHT TURN 5:NEUTRAL
          delay(500) ;
        }
      }
    }
  }
  delay(200) ;  // 超音波距離センサーの連続使用を避けるためのディレイ。なくても動く気はする。
}