Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@yahi65(山崎 浩)

M5Core2 を使って自動運転ロボットカーの製作

Posted at

自動運転ロボットカー

２個のセンサーで、『机などから落下しない』『物に打つからない』ロボットカーを製作しました。
システムは、マルチコア、マルチタスクを利用しています。

開発環境

対象機器は、M5statck core2 aws
開発環境は、Arduino IDE 2.3.2 言語はArduino
作業用PC　Windows11、ログ収集用 TeraTerm Ver5.2

ハードウェア

M5Stack Core2 for AWS(CPUのみ使用)

M5Burnerで、Core2FactoryTest v2.2にアップデート済みです。
M5Core2 M-BUSを直接使用しています。
M5Stack用プロトモジュール [M001]に、ピンヘッダーを接続して各pinにアクセスしています。

センサー

１．赤外線距離センサー(GP2Y0A21YK、シャープ社製)
　『物に打つからない』に利用しています。

sekigaisen.ino
#include <M5Core2.h>
// 赤外線センサーのピン定義
const int sensorPin = 35;

void setup() {
  Serial.begin(115200);
  M5.begin(); // M5Stackの初期化
  M5.Lcd.setTextSize(3); // テキストサイズの設定
  M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK); // テキストカラーの設定
}

void loop() {
  int sensorValue = analogRead(sensorPin); // センサーからのアナログ値を読み取る
  float voltage = sensorValue * (5.0 / 1023.0); // アナログ値を電圧に変換
  float distance = 26.549 * pow(voltage, -1.2091); // 電圧から距離を計算

  // LCDスクリーンに距離を表示
  M5.Lcd.fillScreen(TFT_BLACK); // 画面をクリア
  M5.Lcd.setCursor(0, 0); // カーソルを左上に設定
  M5.Lcd.printf("Distance: %.2f cm", distance*10); // 距離を表示
  Serial.print("Distance: ");
  Serial.print(distance*10);
  Serial.println(" cm");
  delay(100); // 100ミリ秒の遅延
}

ロボットカーに、赤外線センサーを実装してログから障害物との距離(Distance)の最適値を探し出します。

２．照度・近接一体型センサモジュール RPR-0521RS搭載
　『机などから落下しない』に利用しています。

RPR_0521RS.ino

#include <M5Core2.h>
#include <Wire.h>
#include <RPR-0521RS.h>
RPR0521RS rpr0521rs;
 
void setup() {
  byte rc;  
  Serial.begin(115200);
  while (!Serial);  
 
  Wire.begin(32,33);
  M5.begin();
  M5.Lcd.setTextSize(2);
 // M5.Lcd.setRotation(3);
 
  rc = rpr0521rs.init();
}
 
void loop() {
  Wire.begin(32,33);
  byte rc;
  unsigned short ps_val;
  float als_val;
  byte near_far;
 
  rc = rpr0521rs.get_psalsval(&ps_val, &als_val);
  if (rc == 0) {
    Serial.print(F("RPR-0521RS (Proximity)     = "));
    M5.Lcd.setCursor(10, 20);
    M5.Lcd.printf("RPR-0521RS: ");
    Serial.print(ps_val);
    Serial.print(F(" [count]"));
    near_far = rpr0521rs.check_near_far(ps_val);
    if (near_far == RPR0521RS_NEAR_VAL) {
      Serial.println(F(" Near"));
      M5.Lcd.printf("M5 Near!");
    } else {
      Serial.println(F(" Far"));
      M5.Lcd.printf("M5 Far !");
    }
  }
  delay(500);
}

ロボットカーに、照度・近接一体型センサモジュールを実装してログから
机などで模擬落下時のcount数を探し出します。

走行用モーター

360°連続回転サーボ(ローテーションサーボ) FS90R
制御回路は使っていませんので、制御用コントローラーを使っています

hard.png
systemkousei.png

プログラム（AIを利用して結果を検証しています）

各プログラムを部品化しました。

走行系

１．前進、障害物等の回避

zensinsound.h
#include <M5Core2.h>

// DRV8833の制御ピン
const int ZENSIN_MOTOR_LEFT_FWD = 27; // 左モーター前進
const int ZENSIN_MOTOR_LEFT_BWD = 19; // 左モーター後退
const int ZENSIN_MOTOR_RIGHT_FWD = 25; // 右モーター前進
const int ZENSIN_MOTOR_RIGHT_BWD = 26; // 右モーター後退

class Robot {
public:
 Robot() {
        pinMode(ZENSIN_MOTOR_LEFT_FWD, OUTPUT);
        pinMode(ZENSIN_MOTOR_LEFT_BWD, OUTPUT);
        pinMode(ZENSIN_MOTOR_RIGHT_FWD, OUTPUT);
        pinMode(ZENSIN_MOTOR_RIGHT_BWD, OUTPUT);
    }
    void zensin() {
        analogWrite(ZENSIN_MOTOR_LEFT_FWD, 0); // 左モーター前進（低速）
        analogWrite(ZENSIN_MOTOR_LEFT_BWD, 35);
        analogWrite(ZENSIN_MOTOR_RIGHT_FWD, 0); // 右モーター前進（低速）
        analogWrite(ZENSIN_MOTOR_RIGHT_BWD, 35);
    }
    void koutai() {
        analogWrite(ZENSIN_MOTOR_LEFT_FWD, 100); // 左モーター後退
        analogWrite(ZENSIN_MOTOR_LEFT_BWD, 0);
        analogWrite(ZENSIN_MOTOR_RIGHT_FWD, 100); // 右モーター後退
        analogWrite(ZENSIN_MOTOR_RIGHT_BWD, 0);
    }
    void kaihi() {
        analogWrite(ZENSIN_MOTOR_LEFT_FWD, 100); // 左モーター後退
        analogWrite(ZENSIN_MOTOR_LEFT_BWD, 0);
        analogWrite(ZENSIN_MOTOR_RIGHT_FWD, 0);
        analogWrite(ZENSIN_MOTOR_RIGHT_BWD, 100); // 右モーター前進
    }
};

Robot robot;

LCDの顔

１．前進時の顔

zensinface.h
#include <M5Core2.h>

bool eyesOpen = true;
int mouthState = 0;
unsigned long previousMillis = 0;
const long interval = 500; // 瞬きや口の動きの間隔 (ミリ秒)

void drawFace() {
    M5.Lcd.fillScreen(TFT_WHITE); // 画面を白でクリア

    // 目の描画
    if (eyesOpen) {
        // 目が開いている場合
        M5.Lcd.fillCircle(100, 90, 30, TFT_WHITE); // 左目の白目
        M5.Lcd.drawCircle(100, 90, 30, TFT_BLACK); // 左目の輪郭
        M5.Lcd.fillCircle(100, 90, 20, TFT_BLACK); // 左目の黒目

        M5.Lcd.fillCircle(220, 90, 30, TFT_WHITE); // 右目の白目
        M5.Lcd.drawCircle(220, 90, 30, TFT_BLACK); // 右目の輪郭
        M5.Lcd.fillCircle(220, 90, 20, TFT_BLACK); // 右目の黒目
    } else {
        // 目が閉じている場合
        M5.Lcd.fillRect(70, 85, 60, 10, TFT_BLACK); // 左目
        M5.Lcd.fillRect(190, 85, 60, 10, TFT_BLACK); // 右目
    }

    // 口の描画
    switch (mouthState) {
        case 0:
            M5.Lcd.fillRect(140, 180, 40, 10, TFT_PURPLE); // 閉じた口
            break;
        case 1:
            M5.Lcd.fillRect(140, 170, 40, 20, TFT_PURPLE); // 開いた口（小）
            break;
        case 2:
            M5.Lcd.fillRect(140, 160, 40, 30, TFT_PURPLE); // 開いた口（大）
            break;
    }
}

void setupFaceBlink() {
    // 初回の顔の描画
    drawFace();
}

void updateFaceBlink() {
    unsigned long currentMillis = millis();

    // 瞬きと口の動きを一定間隔で更新
    if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;

        // 目の状態を切り替える
        eyesOpen = !eyesOpen;

        // 口の状態を更新
        mouthState = (mouthState + 1) % 3;

        // 顔を再描画
        drawFace();
    }
}

２．回避時の顔

kaihiface.h
#include <M5Core2.h>

// 関数プロトタイプの宣言
void drawSurprisedFace();
void drawCrossEyes(uint16_t color);
void drawXShape(int x, int y, int size, uint16_t color, int thickness);
void drawOvalMouth(uint16_t color);

void drawSurprisedFace() {
  // 背景を白に設定
  M5.Lcd.fillScreen(TFT_WHITE);
  // 目をバッテンにする
  drawCrossEyes(TFT_BLACK); // バッテン目を描く
  // 口を描く
  drawOvalMouth(TFT_PURPLE); // 楕円形の口を描く
  delay(2000);
}

void drawCrossEyes(uint16_t color) {
  // 左目
  drawXShape(90, 100, 30, color, 5);
  // 右目
  drawXShape(220, 100, 30, color, 5);
}

void drawXShape(int x, int y, int size, uint16_t color, int thickness) {
  int halfSize = size / 2;

  // X形を描く（線の太さを指定）
  for (int i = -thickness / 2; i <= thickness / 2; i++) {
    M5.Lcd.drawLine(x - halfSize + i, y - halfSize, x + halfSize + i, y + halfSize, color);
    M5.Lcd.drawLine(x - halfSize + i, y + halfSize, x + halfSize + i, y - halfSize, color);
  }
}

void drawOvalMouth(uint16_t color) {
  // 楕円の口を描く
  M5.Lcd.fillEllipse(160, 180, 40, 20, color);
}

LED

１. 走行時(青)

zensinled.h
#include <M5Core2.h>

#define RED_LED_PIN 13
#define BLUE_LED_PIN 14

bool blueState = false;
unsigned long bluePreviousMillis = 0;
const long blueintervalled = 500; // 青色LEDのブリンク間隔 (ミリ秒)
void blinkLed_B() {
    digitalWrite(RED_LED_PIN, LOW);
    unsigned long blueCurrentMillis = millis();
        if (blueCurrentMillis - bluePreviousMillis >= blueintervalled) {
            bluePreviousMillis = blueCurrentMillis;
            // 青色LEDの状態を切り替え         
            int blueState = digitalRead(BLUE_LED_PIN);
            digitalWrite(BLUE_LED_PIN, !blueState ? HIGH : LOW);
        }
}

２. 回避時(赤)　

kaihiled.h
#include <M5Core2.h>

#define RED_LED_PIN 13
#define BLUE_LED_PIN 14

bool redState = false;
unsigned long redPreviousMillis = 0;
const long redintervalled = 500; // 赤色LEDのブリンク間隔 (ミリ秒)

void blinkLed_R() {
    digitalWrite(BLUE_LED_PIN, LOW);
    unsigned long redCurrentMillis = millis();
        if (redCurrentMillis - redPreviousMillis >= redintervalled) {
           redPreviousMillis = redCurrentMillis;
            // 赤色LEDの状態を切り替え
           int redState = digitalRead(RED_LED_PIN);
           digitalWrite(RED_LED_PIN, !redState ? HIGH : LOW);          
        }
}

障害物回避

１．衝突防止、落下防止
main.inoに記述

BGM

１．走行時

zensinsound.h
#include <driver/i2s.h>
#include <M5Core2.h>
#include "soukou.h" //走行時の音

#define CONFIG_I2S_BCK_PIN      12
#define CONFIG_I2S_LRCK_PIN     0
#define CONFIG_I2S_DATA_PIN     2
#define CONFIG_I2S_DATA_IN_PIN  34

#define SPEAKER_I2S_NUMBER      I2S_NUM_0

#define MODE_MIC                0
#define MODE_SPK                1

void InitI2SSpeakerOrMic_zenshin(int mode) {
  esp_err_t err = ESP_OK;

  i2s_driver_uninstall(SPEAKER_I2S_NUMBER);
  i2s_config_t i2s_config = {
    .mode                 = (i2s_mode_t)(I2S_MODE_MASTER),
    .sample_rate          = 16000,
    .bits_per_sample      = I2S_BITS_PER_SAMPLE_16BIT,
    .channel_format       = I2S_CHANNEL_FMT_ALL_RIGHT,
    .communication_format = I2S_COMM_FORMAT_I2S,
    .intr_alloc_flags     = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count        = 6,
    .dma_buf_len          = 60,
    .use_apll             = false,
    .tx_desc_auto_clear   = true,
    .fixed_mclk           = 0
  };
  if (mode == MODE_MIC) {
    i2s_config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_PDM);
  } else {
    i2s_config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
  }

  err += i2s_driver_install(SPEAKER_I2S_NUMBER, &i2s_config, 0, NULL);

  i2s_pin_config_t tx_pin_config = {
    .bck_io_num           = CONFIG_I2S_BCK_PIN,
    .ws_io_num            = CONFIG_I2S_LRCK_PIN,
    .data_out_num         = CONFIG_I2S_DATA_PIN,
    .data_in_num          = CONFIG_I2S_DATA_IN_PIN,
  };
  err += i2s_set_pin(SPEAKER_I2S_NUMBER, &tx_pin_config);

  if (mode != MODE_MIC) {
    err += i2s_set_clk(SPEAKER_I2S_NUMBER, 16000, I2S_BITS_PER_SAMPLE_16BIT, I2S_CHANNEL_MONO);
  }

  i2s_zero_dma_buffer(SPEAKER_I2S_NUMBER);
}

void zensinbgm() {
     size_t bytes_written;
     M5.Axp.SetSpkEnable(true);
     InitI2SSpeakerOrMic_zenshin(MODE_SPK);

     // Write Speaker
     i2s_write(SPEAKER_I2S_NUMBER, soukou, soukou_size, &bytes_written, portMAX_DELAY);
     i2s_zero_dma_buffer(SPEAKER_I2S_NUMBER);
}

２．障害物回避時
３. PowerON
４. PowerOFF
２～４までは、BGM音のみ変更の為省略
サウンドに関しては下記リンクを参照してください。
https://github.com/tanakamasayuki/M5StickC-examples/blob/master/other/M5StackCore2/Core2WavePlay/Core2WavePlay.ino

全体の流れ

  • 電源ON
  • バッテリチェック
batterychk.h
#include <M5Core2.h>
#include "batterystart.h"
#include "batteryend.h"

// 定数
const float LOW_VOLTAGE_THRESHOLD = 3.3;

void checkBattery() {
  // 電圧を取得
  float voltage = M5.Axp.GetBatVoltage();

  // 電圧が低下した場合
  if (voltage*1000 < LOW_VOLTAGE_THRESHOLD) {
    // シアン背景を設定
    M5.Lcd.fillScreen(TFT_CYAN);
    M5.Lcd.setTextSize(3);
    M5.Lcd.printf(" Bat Voltage:%.2f\n", voltage * 1000);

    // 枠の表示
    int centerX = M5.Lcd.width() / 2;
    int centerY = M5.Lcd.height() / 2;
    int boxSize = 100;
    // 外側の枠を描画
    M5.Lcd.drawRect(centerX - boxSize / 2 - 5, centerY - boxSize / 2 - 5, boxSize + 10, boxSize + 10, TFT_BLACK);
    // 内側の□を描画
    M5.Lcd.drawRect(centerX - boxSize / 2, centerY - boxSize / 2, boxSize, boxSize, TFT_BLACK);

    // 5秒後にバッテリ交換
    delay(5000);
    M5.Lcd.fillScreen(TFT_RED);
    M5.Lcd.setTextSize(3);
    M5.Lcd.println("  Battery");
    M5.Lcd.print("  exchange!");
    delay(1000);
    // バッテリ交換用ビープ音
    endtone();
  } else {
    // Start用ビープ音
    starttone();
  }
}
  • LCDにPowerONと表示
  • LCD内のボタンAを押すと自動運転が始まる
  • 前進する
  • 障害物回避
  • 前進する、障害物回避を繰り返す
  • LCD内のボタンCで電源オフ
  • PowerOFFと表示する
  • 電源OFF

メインプログラム

main.ino
#include <M5Core2.h>
#include <Wire.h>
#include <RPR-0521RS.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "zensinface.h" // 前進時のLCDの顔
#include "zensinled.h" // 前進時のLED(青色)
#include "zensinsoukou.h" // 前進走行、回避後退、回避旋回
#include "zensinsound.h" // 前進時のBGM
#include "kaihiface.h" // 回避時のLCDの顔
#include "kaihiled.h" // 回避時のLED(赤)
#include "kaihisound.h" // 回避時のBGM
#include "batterychk.h" // バッテリチェック
#include "offtone.h" // 電源OFF時の音
#include <esp_task_wdt.h>

// ピン設定
const int irSensorPin = 35; // 赤外線センサーのピン
const int rprInterruptPin = 32; // RPR-0521RSセンサーの割り込みピン
const int ledRedPin = 13;   // LED赤のピン
const int ledBluePin = 14;  // LED青のピン

// 割り込みフラグ
volatile bool irInterruptOccurred = false;
volatile bool rprInterruptOccurred = false;

// タスク制御フラグ
volatile bool tasksStarted = false; // タスクが開始されたかどうかを示すフラグ

// タスクハンドル
TaskHandle_t Task1Handle, Task2Handle, Task3Handle, Task4Handle, Task5Handle, Task6Handle, Task7Handle, Task8Handle;

// RPR-0521RSセンサーオブジェクト
RPR0521RS rpr0521rs;

void setup() {
    M5.begin();
    Serial.begin(115200);
    Wire.begin(32, 33);
    pinMode(ledRedPin, OUTPUT);
    pinMode(ledBluePin, OUTPUT);
    // バッテリチェック
    checkBattery();
    M5.Lcd.fillScreen(TFT_WHITE);
    M5.Lcd.setTextColor(TFT_BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.drawString("Power ON", 35, 110, 4);
    delay(500);
     
    // IDLEタスクをウォッチドッグから削除
    esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(0));
    esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(1));
    delay(500);

    // ヒープメモリの初期状態を表示
    Serial.print("Free heap: ");
    Serial.println(heap_caps_get_free_size(MALLOC_CAP_8BIT));
    delay(500);

    // RPR-0521RSセンサーの初期化
    byte rc = rpr0521rs.init();
    if (rc != 0) {
        Serial.println("RPR-0521RS initialization failed");
        while (1);
    } else {
        Serial.println("RPR-0521RS initialized successfully");
    }

    // 赤外線センサーの割り込み設定
    pinMode(irSensorPin, INPUT_PULLUP);
    delay(500);

    // RPR-0521RSセンサーの割り込み設定
    pinMode(rprInterruptPin, INPUT_PULLUP);
    delay(500);
  
    // タスクの作成とコアの割り当て
    xTaskCreatePinnedToCore(Task1, "Task1", 4096, NULL, 1, &Task1Handle, 0); // Core 0
    xTaskCreatePinnedToCore(Task2, "Task2", 4096, NULL, 1, &Task2Handle, 0); // Core 0
    xTaskCreatePinnedToCore(Task3, "Task3", 4096, NULL, 1, &Task3Handle, 0); // Core 0
    xTaskCreatePinnedToCore(Task4, "Task4", 4096, NULL, 1, &Task4Handle, 0); // Core 0
    xTaskCreatePinnedToCore(Task5, "Task5", 4096, NULL, 1, &Task5Handle, 1); // Core 1
    xTaskCreatePinnedToCore(Task6, "Task6", 4096, NULL, 1, &Task6Handle, 1); // Core 1
    xTaskCreatePinnedToCore(Task7, "Task7", 4096, NULL, 1, &Task7Handle, 1); // Core 1
    xTaskCreatePinnedToCore(Task8, "Task8", 4096, NULL, 1, &Task8Handle, 1); // Core 1
    Serial.println("Task Core Setup");
    delay(500);
}

void loop() {
    M5.update();
    if (M5.BtnA.wasPressed()) {
        tasksStarted = true; // ボタンAを押すとタスク開始する
        delay(500);
        Serial.println("Bot A Start");
        Serial.print("Tasks started: ");
        Serial.println(tasksStarted);    
    }
    
    if (M5.BtnC.wasPressed()) {
        // ボタンCを押すと電源OFF
        Serial.println("Bot C Stop");
        delay(1000);
        tasksStarted = false; // ボタンCを押すとタスク終了する
        delay(1000);
        vTaskSuspend(Task8Handle);
        delay(500);
        vTaskSuspend(Task1Handle);
        delay(500);
        vTaskSuspend(Task2Handle);
        delay(500);
        vTaskSuspend(Task3Handle);
        delay(500);
        vTaskSuspend(Task4Handle);
        delay(500);
        M5.Lcd.fillScreen(TFT_WHITE);
        M5.Lcd.fillScreen(TFT_YELLOW);
        M5.Lcd.setTextColor(TFT_BLACK);
        M5.Lcd.setTextSize(2);
        M5.Lcd.drawString("Power OFF", 30, 110, 4);
        delay(5000); // 5秒後電源OFF
        // Poweroff音
        stoptone();
        Serial.println("stopped");
        delay(5000); // 5秒後電源OFF
        M5.Axp.PowerOff();
    }
}

void Task1(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
        }
        if (!irInterruptOccurred && !rprInterruptOccurred) {
            Serial.println("Task1 is running");
            vTaskSuspend(NULL);
            delay(500);
           // 前進時のBGM
            zensinbgm();
            Serial.println("Zensin BGM running");
            delay(500);
        }
        taskYIELD();
    }
}

void Task2(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
       }
        if (!irInterruptOccurred && !rprInterruptOccurred) {
            Serial.println("Task2 is running");
            vTaskSuspend(NULL); 
            delay(500);
            // 前進時のLCD顔
            updateFaceBlink();
            Serial.println("Zensin Face running");
            delay(500);
        }
        taskYIELD();
    }
}

void Task3(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
        }
        if (!irInterruptOccurred && !rprInterruptOccurred) {
            Serial.println("Task3 is running");
            vTaskSuspend(NULL);
            delay(500);
             // 前進走行
           robot.zensin();
           Serial.println("Zensin Soukou  running");
           delay(500);
        }
        taskYIELD();
    }
}

void Task4(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
       }
        if (!irInterruptOccurred && !rprInterruptOccurred) {
            Serial.println("Task4 is running");
            vTaskSuspend(NULL);
            delay(500);
            // 前進時の走行LED(青)
            blinkLed_B();
            Serial.println("Zensin LED  running");
            delay(500);
        }
        taskYIELD();
    }
}

void Task5(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
        }
        if (irInterruptOccurred || rprInterruptOccurred) {
            Serial.println("Task5 (Interrupt) is running");
            vTaskSuspend(NULL);
            delay(500);
            // 割り込み 回避時のLCD顔
            drawSurprisedFace();
            Serial.println("Kaihi Face running");
            Serial.println("Task5 (Interrupt) is finished");
        }
        taskYIELD();
    }
}

void Task6(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
        }
        if (irInterruptOccurred || rprInterruptOccurred) {
            Serial.println("Task6 (Interrupt) is running");
            vTaskSuspend(NULL);
            delay(500);
            // 割り込み 回避時のBGM
            kaihibgm();
            Serial.println("Kaihi BGM running");
            delay(500); 
            Serial.println("Task6 (Interrupt) is finished");
           }
        taskYIELD();
    }
}

void Task7(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
            delay(500);
        }
        if (irInterruptOccurred || rprInterruptOccurred) {
            Serial.println("Task7 (Interrupt) is running");
            vTaskSuspend(NULL);
            delay(500);
            // 割り込み 回避時のLED(赤)
            blinkLed_R();
            Serial.println("Kaihi LED running");
            delay(500);
            Serial.println("Task7 (Interrupt) is finished");
        }
        taskYIELD();
    }
}

 void Task8(void *pvParameters) {
    for (;;) {
        if (!tasksStarted) {
            continue;
        }
        if (!irInterruptOccurred && !rprInterruptOccurred) {
            Serial.println("Task8 is running");
            vTaskResume(Task1Handle);
            delay(500);
            vTaskResume(Task2Handle);
            delay(500);
            vTaskResume(Task3Handle);
            delay(500);
            vTaskResume(Task4Handle);
            delay(500);

            //赤外線センサーでの距離取得
            int sensorValue = analogRead(irSensorPin);
            float voltage = sensorValue * (3.3 / 4095.0); // 3.3V ADC変換
            float distance = 26.549 * pow(voltage, -1.2091);
            Serial.print("IR Distance: ");
            Serial.print(distance);
            Serial.println(" cm ");
            delay(500);

            //接近センサーでの距離取得
            uint16_t ps_val;
            float als_val;
            byte rc = rpr0521rs.get_psalsval(&ps_val, &als_val);
            Serial.print("RPR-0521RS Proximity: ");
            Serial.print(ps_val);
            Serial.println(" count ");
            delay(500);
            
            // 赤外線センサー 15cm以下または、接近センサー 300count以下の場合、割り込み発生
            if (distance < 15.0 or ps_val <= 300) { 
                robot.koutai(); // 回避後退
                delay(500);
                Serial.println("koutai (Interrupt) is running");
                robot.kaihi(); // 回避旋回
                delay(500);
                Serial.println("kaihi (Interrupt) is running");
                irInterruptOccurred = false; //割り込処理発生
                rprInterruptOccurred = true;
                vTaskResume(Task5Handle);
                delay(500);
                vTaskResume(Task6Handle);
                delay(500);
                vTaskResume(Task7Handle);
                delay(500);
                irInterruptOccurred = false; //割り込み処理終了
                rprInterruptOccurred = false;
                delay(500);
            }
        }
      }          
      taskYIELD();
  }

マルチタスクでのシステムの考え方。

複数のタスクを同時に動作させる事に多くの時間を要しました。
結果として、タスクサスペンド、タスクレジュームで動作が可能となりました。

走行時のログ

Free heap: 4463855　⇒メモリー容量
RPR0521RS Part ID Register Value = 0xA
RPR0521RS MANUFACT_ID Register Value = 0xE0
RPR-0521RS initialized successfully
Task Core Setup　⇒タスクセットアップ終了
Task4 is running
Task8 is running
Task3 is running
Task1 is running
Task2 is running

Bot A Start　⇒スタート
Tasks started: 1
Zensin Face running
Zensin Soukou running
Task2 is running
Zensin LED running
IR Distance: 71.48 cm ⇒障害物通常走行
Task3 is running
Task4 is running
RPR-0521RS Proximity: 16380 count ⇒落下通常走行
Task8 is running
Zensin Face running
Zensin Soukou running
Task2 is running
Zensin LED running
IR Distance: 228.77 cm ⇒障害物通常走行
RPR-0521RS Proximity: 3 count ⇒落下回避走行
koutai (Interrupt) is running
kaihi (Interrupt) is running
Task5 (Interrupt) is running
Task6 (Interrupt) is running
Task7 (Interrupt) is running
Zensin BGM running
Task3 is running
Task4 is running
Kaihi LED running
Task1 is running
Task8 is running
Task7 (Interrupt) is finished
Kaihi BGM running
Task6 (Interrupt) is finished
Zensin Face running
Zensin Soukou running
Task2 is running
Zensin LED running
IR Distance: 6.27 cm ⇒障害物回避走行
RPR-0521RS Proximity: 215 count
Task3 is running
koutai (Interrupt) is running
kaihi (Interrupt) is running
Task7 (Interrupt) is running
Task6 (Interrupt) is running
Zensin BGM running
Task1 is running
Task4 is running
Kaihi LED running
Task8 is running
Task7 (Interrupt) is finished

Bot C Stop　⇒電源OFF
Kaihi BGM running
Kaihi Face running
Task5 (Interrupt) is finished
Task6 (Interrupt) is finished
Zensin Face running
Zensin Soukou running
Zensin LED running
IR Distance: 11.50 cm
RPR-0521RS Proximity: 73 count
koutai (Interrupt) is running
kaihi (Interrupt) is running
stopped

後書き

AI（主にChatGPT4、時間制限ありの無料版）の助言を参考にプログラミングをしました。C言語は不慣れでAが作ったプログラムを解析、検証することで勉強になりました。

0
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?