やりたいこと
前回は、とりあえず相互にデータを投げ合う実験をしました。
今回は、より実用的な通信のためのライブラリを開発しました。
ライブラリ
設計
今回作成するのはロボットとコントローラを1対1で接続しリモート制御するためのライブラリで、シンプルで使いやすいライブラリを目指します。
データ構造
まず送受信するデータは、
- タイムスタンプ
- リアルタイムな通信が出来ているか確認するため
- アドレス
- 送信先が明確になるように(一応)
- サイズ
- 格納されるデータのサイズ
- データ
- 実際のデータ
これらに対して、通信内容のデータ化けを検出するためにCRC(CRC32)を付与したうえで送信します。データ構造を図示すると以下のとおりです。（なお、ESP-NOWの仕様から送信するデータ全体で250バイトまでなのに注意)
送受信
- ペアリングは1対1
- 通信処理の簡略化のため
(複数の場合受信ハンドラでアドレスを判定して振り分ける必要がある)
- 通信処理の簡略化のため
- 送信
- バッファリング無し
- 通信にかかる時間が短いため(150～200μs程度)
- バッファリング無し
- 受信
- バッファリングあり
- ロボットがコントローラ側にログなどを高速で送り返すケースを想定
- 受信処理が間に合わない場合は古いデータから破棄
- バッファリング無しのデータチェック
- とりあえず最新データをチェックしたいケースを想定
- バッファ削除機能
- データチェック機能と組み合わせてバッファリング無しでの受信機能を実現するため
- タイムアウト処理
- コントローラからの司令が途絶した場合、ロボットが安全に停止するため
- CRCによるデータ化け検出
- コントローラからの化けた司令でロボットが動作し、暴走するのを防ぐため。
- バッファリングあり
実装
本来であればクラス化するべきなのかもしれませんが、ESP-NOWのコールバック関数(onDataRecv/onDataSent)は静的関数（static）またはグローバル関数である必要があります。(ESP-NOWのAPIが関数ポインタを使ってコールバックを登録するため、アドレスが確定している必要がある）
本ライブラリではロボットとコントローラを1対1でしか紐づけないので、namespaceを活用して名前空間を分けるだけにとどめました。
ROBO_WCOM.h
#ifndef ROBO_WCOM_H
#define ROBO_WCOM_H
#include <Arduino.h>
namespace ROBO_WCOM
{
/**
* @brief ライブラリ共通のステータス
* @details
* - 正常系は Ok のみ（0）
* - 異常系は用途別に負の値を割当て
*/
enum class Status : int8_t {
Ok = 0,
// 共通
Timeout = -1,
CrcError = -2,
BufferEmpty = -3,
InvalidArg = -4,
// 初期化
EspNowInitFail = -10,
AddPeerFail = -11,
// 送信
SendFail = -20,
};
/**
* @brief 搬送データの最大サイズ（バイト）
*/
constexpr size_t CARRIED_DATA_MAX_SIZE = 200;
/**
* @brief 受信バッファの最大保持パケット数
*/
constexpr size_t RECEIVE_BUFFER_SIZE = 64;
/**
* @brief 通信パケットのデータ部構造
* @details
* - タイムスタンプ、送信元アドレス、データサイズ、データ本体を含む
* - `__attribute__((packed))` によりパディングを排除
*/
struct __attribute__((packed)) PacketData {
uint32_t timestamp; ///< 受信時刻（millis()）
uint8_t address[6]; ///< 送信元デバイスのMACアドレス
uint8_t carriedSize; ///< 搬送データサイズ
uint8_t carriedData[CARRIED_DATA_MAX_SIZE];///< 搬送データ本体
};
/**
* @brief 通信初期化
* @param sourceAddress 自デバイスのMACアドレス（6バイト）
* @param distAddress 通信相手のMACアドレス（6バイト）
* @param nowMillis 受信タイムアウト時間（ミリ秒）
* @param timeoutMS 受信タイムアウト時間（ミリ秒）
* @return ステータスコード (Status)
*/
Status Init(const uint8_t sourceAddress[6], const uint8_t distAddress[6], uint32_t nowMillis, uint32_t timeoutMS);
/**
* @brief パケット送信
* @param timestamp 送信時刻（任意の基準でOK）
* @param data 送信データへのポインタ
* @param size 送信データサイズ（最大 CARRIED_DATA_MAX_SIZE）
* @return ステータスコード (Status)
*/
Status SendPacket(uint32_t timestamp, const uint8_t* data, uint8_t size);
/**
* @brief パケット受信
* @param nowMillis 現在時刻（millis）
* @param timestamp 受信パケットのタイムスタンプ格納先
* @param address 送信元アドレス格納先（6バイト）
* @param data 受信データ格納先
* @param size 受信データサイズ格納先
* @return ステータスコード (Status)
*/
Status PopOldestPacket(uint32_t nowMillis, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size);
/**
* @brief バッファの最新データをチェックする。バッファは操作しない。
*
* @param nowMillis 現在時刻（millis）
* @param timestamp 受信パケットのタイムスタンプ格納先
* @param address 送信元アドレス格納先（6バイト）
* @param data 受信データ格納先
* @param size 受信データサイズ格納先
* @return ステータスコード (Status)
*/
Status PeekLatestPacket(uint32_t nowMillis, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size);
/**
* @brief バッファのクリア
*
* @return Status
*/
Status FlushBuffer(void);
/**
* @brief 受信バッファ内のパケット数を取得
* @return バッファ内のパケット数
*/
int16_t ReceivedCapacity(void);
/**
* @brief ステータスを人間可読な文字列へ変換
*/
const char* ToString(Status s);
}
#endif /* ROBO_WCOM_H */
ROBO_WCOM.cpp
#include "ROBO_WCOM.h"
#include <WiFi.h>
#include <esp_now.h>
#include <cstring>
namespace ROBO_WCOM
{
/**
* @brief 送受信パケット構造（CRC付き）
*/
struct __attribute__((packed)) Packet {
PacketData data; ///< データ部
uint32_t crc32; ///< CRC32値
};
//=== 内部状態 ===//
static Packet recvBuffer[RECEIVE_BUFFER_SIZE]; ///< 受信リングバッファ
static volatile uint16_t head = 0; ///< バッファ書き込み位置
static volatile uint16_t tail = 0; ///< バッファ読み出し位置
static volatile int16_t count = 0; ///< バッファ内パケット数
static Packet sendPacket; ///< 送信用パケット
static uint8_t ownAddr[6]; ///< 自デバイスMAC
static uint8_t peerAddr[6]; ///< 通信相手MAC
static esp_now_peer_info_t peerInfo{}; ///< ESP-NOW Peer情報
static uint32_t lastRecvMillis = 0; ///< 最終受信時刻
static uint32_t timeoutMillis = 0; ///< タイムアウト時間
//=== 内部関数プロトタイプ ===//
static uint32_t calcCRC32(const PacketData& data);
static void pushToBuffer(const Packet& pkt);
static bool popFromBuffer(Packet& pkt);
static void onDataRecv(const uint8_t* mac, const uint8_t* incomingData, int len);
static void onDataSent(const uint8_t*, esp_now_send_status_t);
static Status extractPacketData(const Packet& pkt, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size);
static void fillWithEmptyPacket(uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size);
//=== 内部関数実装 ===//
/**
* @brief CRC32計算
* @param data パケットデータ部
* @return CRC32値
*/
static uint32_t calcCRC32(const PacketData& data)
{
uint32_t crc = 0xFFFFFFFF;
const uint8_t* bytes = reinterpret_cast<const uint8_t*>(&data);
for (size_t i = 0; i < sizeof(PacketData); ++i)
{
crc ^= bytes[i];
for (int j = 0; j < 8; ++j)
{
crc = (crc & 1) ? (crc >> 1) ^ 0xEDB88320 : crc >> 1;
}
}
return crc ^ 0xFFFFFFFF;
}
/**
* @brief 受信バッファにパケットを追加
* @param pkt 追加するパケット
*/
static void pushToBuffer(const Packet& pkt)
{
recvBuffer[head] = pkt;
head = (head + 1) % RECEIVE_BUFFER_SIZE;
if (count < RECEIVE_BUFFER_SIZE)
{
count++;
}
else
{
// 上書き時はtailも進める
tail = (tail + 1) % RECEIVE_BUFFER_SIZE;
}
}
/**
* @brief 受信処理を切り替えるための定数
*
*/
enum class BufferMode { Pop, Peek };
/**
* @brief パケットを受信する処理。バッファ操作を行う場合と行わない場合での切り替えが可能
*
* @param mode 受信状態切り替え
* @param nowMillis 現在時刻（millis）
* @param timestamp 受信パケットのタイムスタンプ格納先
* @param address 送信元アドレス格納先（6バイト）
* @param data 受信データ格納先
* @param size 受信データサイズ格納先
* @return ステータスコード (Status)
*/
static Status getPacket(BufferMode mode, uint32_t nowMillis,
uint32_t* timestamp, uint8_t* address,
uint8_t* data, uint8_t* size)
{
Packet pkt;
if (nowMillis - lastRecvMillis > timeoutMillis)
{
return Status::Timeout;
}
if (mode == BufferMode::Pop)
{
// POP処理ではデータを取り出すとともにバッファ操作を行う
if (!popFromBuffer(pkt))
{
// バッファが空の場合、ゼロ埋めしたうえで、空であることを通知する
fillWithEmptyPacket(timestamp, address, data, size);
return Status::BufferEmpty;
}
}
else
{
pkt = recvBuffer[tail];
}
return extractPacketData(pkt, timestamp, address, data, size);
}
/**
* @brief 受信バッファからパケットを取り出す
* @param pkt 取り出したパケット格納先
* @return true:成功 / false:空
*/
static bool popFromBuffer(Packet& pkt)
{
if (count <= 0)
{
return false;
}
pkt = recvBuffer[tail];
tail = (tail + 1) % RECEIVE_BUFFER_SIZE;
--count;
return true;
}
/**
* @brief ESP-NOW受信コールバック
* @param mac 送信元MACアドレス
* @param incomingData 受信データ
* @param len データ長
*/
static void onDataRecv(const uint8_t* mac, const uint8_t* incomingData, int len)
{
if (len != sizeof(Packet))
{
return;
}
lastRecvMillis = millis();
Packet pkt;
memcpy(&pkt, incomingData, sizeof(Packet));
pushToBuffer(pkt);
}
/**
* @brief ESP-NOW送信完了コールバック
* @param mac_addr 送信先MAC
* @param status 送信ステータス
*/
static void onDataSent(const uint8_t*, esp_now_send_status_t)
{
// 送信完了時の処理（必要なら実装）
// 今回は特に何もしない
return;
}
/**
* @brief パケットからデータを抽出（CRCチェック付き）
* @param pkt 対象パケット
* @param timestamp タイムスタンプ格納先
* @param address アドレス格納先（6バイト）
* @param data データ本体格納先
* @param size データサイズ格納先
* @return Status::Ok / Status::CrcError / Status::InvalidArg
*/
static Status extractPacketData(const Packet& pkt, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size)
{
if(!timestamp || !address || !data || !size)
{
return Status::InvalidArg;
}
if (calcCRC32(pkt.data) != pkt.crc32)
{
return Status::CrcError;
}
*timestamp = pkt.data.timestamp;
memcpy(address, pkt.data.address, sizeof(pkt.data.address));
*size = pkt.data.carriedSize;
memcpy(data, pkt.data.carriedData, *size);
return Status::Ok;
}
/**
* @brief 空パケットを作成
*
* @return Packet 空のパケット
*/
static Packet makeEmptyPacket()
{
Packet zeroPkt;
zeroPkt.data.timestamp = 0;
memset(zeroPkt.data.address, 0, sizeof(zeroPkt.data.address));
zeroPkt.data.carriedSize = 0;
memset(zeroPkt.data.carriedData, 0, sizeof(zeroPkt.data.carriedData));
zeroPkt.crc32 = calcCRC32(zeroPkt.data);
return zeroPkt;
}
/**
* @brief 空のパケットを生成し、データをゼロにして返す
*
* @param timestamp ゼロを格納
* @param address ゼロを格納
* @param data ゼロを格納
* @param size ゼロを格納
*/
static void fillWithEmptyPacket(uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size)
{
Packet zeroPkt = makeEmptyPacket();
extractPacketData(zeroPkt, timestamp, address, data, size);
}
//======= 公開API実装 =======//
/**
* @brief 通信初期化
* @param sourceAddress 自デバイスのMACアドレス（6バイト）
* @param distAddress 通信相手のMACアドレス（6バイト）
* @param nowMillis 受信タイムアウト時間（ミリ秒）
* @param timeoutMS 受信タイムアウト時間（ミリ秒）
* @return ステータスコード (Status)
*/
Status Init(const uint8_t sourceAddress[6], const uint8_t distAddress[6], uint32_t nowMillis, uint32_t timeoutMS)
{
uint8_t i;
Packet zeroPkt = makeEmptyPacket(); // 中身が空でCRCが成立しているデータを作成
timeoutMillis = timeoutMS;
lastRecvMillis = nowMillis;
head = 0;
tail = 0;
count = 0;
// バッファをゼロ埋めして初期化する
for (i = 0; i < RECEIVE_BUFFER_SIZE; i++)
{
pushToBuffer(zeroPkt);
}
FlushBuffer();
// アドレスをコピー
memcpy(ownAddr, sourceAddress, sizeof(ownAddr));
memcpy(peerAddr, distAddress, sizeof(peerAddr));
// WiFiをステーションモードで初期化
WiFi.mode(WIFI_STA);
// ESP-NOWを初期化
if (esp_now_init() != ESP_OK)
{
return Status::EspNowInitFail;
}
memset(&peerInfo, 0, sizeof(peerInfo));
memcpy(peerInfo.peer_addr, distAddress, sizeof(peerAddr));
peerInfo.channel = 0;
peerInfo.encrypt = false;
// ペアリング
if (esp_now_add_peer(&peerInfo) != ESP_OK)
{
return Status::AddPeerFail;
}
// コールバック関数を設定
esp_now_register_recv_cb(onDataRecv);
esp_now_register_send_cb(onDataSent);
return Status::Ok;
}
/**
* @brief パケット送信
* @param timestamp 送信時刻（任意の基準でOK）
* @param data 送信データへのポインタ
* @param size 送信データサイズ（最大 CARRIED_DATA_MAX_SIZE）
* @return ステータスコード (Status)
*/
Status SendPacket(uint32_t timestamp, const uint8_t* data, uint8_t size)
{
sendPacket.data.timestamp = timestamp;
memcpy(sendPacket.data.address, ownAddr, sizeof(ownAddr));
// 送信データが大きすぎたらリミット
if (size > CARRIED_DATA_MAX_SIZE)
{
size = CARRIED_DATA_MAX_SIZE;
}
// 送信データを格納
sendPacket.data.carriedSize = size;
memcpy(sendPacket.data.carriedData, data, size);
sendPacket.crc32 = calcCRC32(sendPacket.data);
// 送信処理
if (esp_now_send(peerAddr, reinterpret_cast<uint8_t*>(&sendPacket), sizeof(Packet)) == ESP_OK)
{
return Status::Ok;
}
else
{
return Status::SendFail;
}
}
/**
* @brief パケット受信。バッファからデータを取り出す
* @param nowMillis 現在時刻（millis）
* @param timestamp 受信パケットのタイムスタンプ格納先
* @param address 送信元アドレス格納先（6バイト）
* @param data 受信データ格納先
* @param size 受信データサイズ格納先
* @return ステータスコード (Status)
*/
Status PopOldestPacket(uint32_t nowMillis, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size)
{
return getPacket(BufferMode::Pop, nowMillis, timestamp, address, data, size);
}
/**
* @brief バッファの最新データをチェックする。バッファは操作しない。
*
* @param nowMillis 現在時刻（millis）
* @param timestamp 受信パケットのタイムスタンプ格納先
* @param address 送信元アドレス格納先（6バイト）
* @param data 受信データ格納先
* @param size 受信データサイズ格納先
* @return ステータスコード (Status)
*/
Status PeekLatestPacket(uint32_t nowMillis, uint32_t* timestamp, uint8_t* address, uint8_t* data, uint8_t* size)
{
return getPacket(BufferMode::Peek, nowMillis, timestamp, address, data, size);
}
/**
* @brief 受信バッファ内のパケット数を取得
*
* @return int16_t バッファ内のパケット数
*/
int16_t ReceivedCapacity()
{
return count;
}
/**
* @brief バッファのクリア
*
* @return Status
*/
Status FlushBuffer(void)
{
head = 0;
tail = 0;
count = 0;
return Status::Ok;
}
/**
* @brief ステータスコードを文字列に変換
* @param s ステータスコード
* @return 文字列
*/
const char* ToString(Status s)
{
switch (s) {
case Status::Ok: return "Ok";
case Status::Timeout: return "Timeout";
case Status::CrcError: return "CRC error";
case Status::BufferEmpty: return "Buffer empty";
case Status::InvalidArg: return "Invalid argument";
case Status::EspNowInitFail: return "ESP-NOW init failed";
case Status::AddPeerFail: return "Add peer failed";
case Status::SendFail: return "Send failed";
default: return "Unknown";
}
}
}
テストコード
とりあえず、ボードAからボードBに、ランダムな数とカウント数、これらを掛け算した値を２秒間隔で一気に送りつけ、データが正しく受信できるか？抜けはないか？タイムアウトが実現出来ているか？チェックします。
（CRC化けは、ライブラリを加工して、任意のタイミングでCRCをわざと壊しチェックしました。モータ等のノイズでは化けは発生しませんでした）
main.cpp
#include <Arduino.h>
#include "ROBO_WCOM.h"
#define TEST_STRING_SIZE 64 ///< 送信メッセージの最大長（バイト）
//---------------------------------------------
// 動作モード切り替え
//---------------------------------------------
#define BOARD_A 1 ///< 送信側ボード
#define BOARD_B 2 ///< 受信側ボード
#define BOARD_IS BOARD_A
// #define BOARD_IS BOARD_B
//---------------------------------------------
// 通信相手のMACアドレス設定
//---------------------------------------------
uint8_t MACADDRESS_BOARD_A[6] = { 0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX}; // 送信先のデバイスのアドレス(6バイト)
uint8_t MACADDRESS_BOARD_B[6] = { 0xYY, 0xYY, 0xYY, 0xYY, 0xYY, 0xYY}; // 送信先のデバイスのアドレス(6バイト)
int16_t testCounter = 0; ///< 送信カウンタ
ROBO_WCOM::PacketData receivedPacket; ///< 受信データ格納用
/**
* @brief テスト送信関数
* @param counter 送信カウンタ値
* @param nowMillis 現在時刻（millis）
* @param randomVal 送信データに含める乱数値
*/
void testSend(uint16_t counter, uint32_t nowMillis, uint32_t randomVal);
/**
* @brief テスト受信関数
* @param nowMillis 現在時刻（millis）
*/
void testReceive(uint32_t nowMillis);
//---------------------------------------------
// Arduino標準関数
//---------------------------------------------
void setup()
{
Serial.begin(115200);
Serial.println("Board Boot");
#if BOARD_IS == BOARD_A
auto initStatus = ROBO_WCOM::Init(MACADDRESS_BOARD_A, MACADDRESS_BOARD_B, millis(), 1000);
Serial.print("Controller communication started. : Status=");
Serial.println(ROBO_WCOM::ToString(initStatus));
#else
auto initStatus = ROBO_WCOM::Init(MACADDRESS_BOARD_B, MACADDRESS_BOARD_A, millis(), 1000);
Serial.println("Robo communication started. : Status=");
Serial.println(ROBO_WCOM::ToString(initStatus));
#endif
}
void loop()
{
uint32_t nowMillis = millis();
int cap;
#if BOARD_IS == BOARD_A
// 送信側：100回連続送信
for (cap = 0; cap < 100; cap++)
{
uint32_t randomVal = random(0, 100);
testSend(testCounter, nowMillis, randomVal);
Serial.println();
testCounter++;
delay(1);
}
delay(2000);
#else
cap = ROBO_WCOM::ReceivedCapacity();
Serial.print("t=");
Serial.print(nowMillis);
Serial.print(" Capacity=");
Serial.print(cap);
testReceive(nowMillis);
Serial.println();
delay(10);
#endif
}
//---------------------------------------------
// ユーティリティ関数
//---------------------------------------------
void testSend(uint16_t counter, uint32_t nowMillis, uint32_t randomVal)
{
char sendData[TEST_STRING_SIZE];
snprintf(sendData, TEST_STRING_SIZE, "%d,\t%lu * %d =\t%lu",
counter, randomVal, counter, randomVal * counter);
// パケット送信
ROBO_WCOM::SendPacket(nowMillis, reinterpret_cast<uint8_t*>(sendData), strlen(sendData) + 1);
Serial.print(sendData);
}
void testReceive(uint32_t nowMillis)
{
char receivedString[128];
uint8_t size;
uint32_t timestamp;
uint8_t address[6];
uint8_t data[ROBO_WCOM::CARRIED_DATA_MAX_SIZE];
// パケット受信
// バッファを使用した受信の実験用
// auto rcv_status = ROBO_WCOM::PopOldestPacket(nowMillis, ×tamp, address, data, &size);
// 最新データの取り出しのみの実験用
auto rcv_status = ROBO_WCOM::PeekLatestPacket(nowMillis, ×tamp, address, data, &size);
// ステータス表示
switch (rcv_status)
{
case ROBO_WCOM::Status::Ok:
Serial.print("\t || RECEIVE OK ");
break;
case ROBO_WCOM::Status::Timeout:
Serial.print("\t || TIMEOUT ERROR ");
break;
case ROBO_WCOM::Status::BufferEmpty:
Serial.print("\t || BUFFER EMPTY ");
break;
case ROBO_WCOM::Status::CrcError:
Serial.print("\t || CRC ERROR ");
break;
default:
Serial.print("\t || UNKNOWN ERROR ");
break;
}
// 受信内容表示
snprintf(receivedString, sizeof(receivedString),
"|| Received: time=%ld,\taddr=%02X:%02X:%02X:%02X:%02X:%02X, data=%s,\tsize=%d",
timestamp,
address[0], address[1], address[2],
address[3], address[4], address[5],
data, size);
Serial.print(receivedString);
}
実際の使用例
コントローラ側のESP32からロボット側を制御しつつ、ロボット側からはステータスが返ってくるケースを想定し、適当なコードを書いて実験しました。
ロボット側・コントローラ側の通信データ
まずは共通のヘッダファイルで構造体を規定。通信データをひとまとめにしておきます。
robo_packet.h
#ifndef __ROBO_PACKET_H__
#define __ROBO_PACKET_H__
#define MOTOR_NUM 8
struct __attribute__((packed)) RoboStatus
{
struct __attribute__((packed))
{
float voltage; ///< 電源電圧
float current; ///< 電源電流
float wh; ///< 電力量
}Power;
float motors[MOTOR_NUM]; ///< モータの稼働状況
union
{
uint8_t FLAGS; ///< 武器フラグを一括操作
struct
{
// 左側武器フラグ
uint8_t WPL03:1; ///< 左武器4
uint8_t WPL02:1; ///< 左武器3
uint8_t WPL01:1; ///< 左武器2
uint8_t WPL00:1; ///< 左武器1
// 右側武器フラグ
uint8_t WPR03:1; ///< 右武器4
uint8_t WPR02:1; ///< 右武器3
uint8_t WPR01:1; ///< 右武器2
uint8_t WPR00:1; ///< 右武器1
}FLAG;
}WEAPON_FLAGS;
union
{
uint16_t SWITCHES;
struct
{
uint8_t S4 : 1;
uint8_t S3 : 1;
uint8_t S2 : 1;
uint8_t S1 : 1;
uint8_t SQUARE : 1;
uint8_t CIRCLE : 1;
uint8_t CROSS : 1;
uint8_t TRIANGLE: 1;
uint8_t LEFT : 1;
uint8_t RIGHT : 1;
uint8_t DOWN : 1;
uint8_t UP : 1;
}BUTTON;
}MANSWICH;
};
#endif /* __ROBO_PACKET_H__ */
controller_packet.h
#ifndef __CONTROLLER_PACKET_H__
#define __CONTROLLER_PACKET_H__
#include <Arduino.h>
struct __attribute__((packed)) RoboCommand{
float velocity_x; ///< x軸方向の速度（相対値） -100 ~ +100
float velocity_y; ///< y軸方向の速度（相対値） -100 ~ +100
float omega; ///< 角速度（相対値） -100 ~ +100
union
{
uint8_t FLAGS; ///< 武器フラグを一括操作
struct
{
// 左側武器フラグ
uint8_t WPL03:1; ///< 左武器4
uint8_t WPL02:1; ///< 左武器3
uint8_t WPL01:1; ///< 左武器2
uint8_t WPL00:1; ///< 左武器1
// 右側武器フラグ
uint8_t WPR03:1; ///< 右武器4
uint8_t WPR02:1; ///< 右武器3
uint8_t WPR01:1; ///< 右武器2
uint8_t WPR00:1; ///< 右武器1
}FLAG;
}WEAPON_FLAGS;
union
{
uint32_t RGB_DATAS; ///< フルカラーLEDの一括変更
struct
{
uint8_t RED;
uint8_t GREEN;
uint8_t BLUE;
}RGB;
}RGBLED;
uint16_t hp; ///< HP
};
#endif /* __CONTROLLER_PACKET_H__ */
実装
開発効率のためにplatformioのiniファイル内でコンパイルするmain.cppを切り替えます。
platformio.ini
[platformio]
;src_dir = examples/SimpleTest
;src_dir = examples/Controller
src_dir = examples/Robo
[env:esp32dev]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
コントローラ側
(コントローラ側)main.cpp
/***************************************************************************************************/
/*=========================== Controller ==========================================================*/
/***************************************************************************************************/
#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "ROBO_WCOM.h"
#include "controller_packet.h"
#include "robo_packet.h"
//---------------------------------------------
// 通信相手のMACアドレス設定
//---------------------------------------------
uint8_t MACADDRESS_BOARD_CONTROLLER[6] = { 0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX};
uint8_t MACADDRESS_BOARD_ROBO[6] = { 0xYY, 0xYY, 0xYY, 0xYY, 0xYY, 0xYY};
/** モータ配列のインデックス値 **/
#define MOTOR_CH_FL 0
#define MOTOR_CH_FR 1
#define MOTOR_CH_RL 2
#define MOTOR_CH_RR 3
void statusViewer(void* pvParameters); // ROBO側のステータスを取得して表示
TaskHandle_t thp[1]; // タスクハンドラ
RoboCommand sendCommand; // 送信するコマンド
RoboStatus rcvStatus; // 受信したステータス
uint32_t rcvTimeStamp; // 受信したロボット側の時刻
uint8_t rcvAddress[6]; // 受信したロボット側のアドレス
uint8_t rcvSize; // 受信したデータのサイズ
void setup()
{
Serial.begin(115200);
Serial.println("Board Boot");
Serial.println("==== THIS IS CONTROLLER ====");
xTaskCreateUniversal(
statusViewer, // タスク関数
"StatusViewer",
4096, // スタックサイズ
nullptr, // パラメータ
1, // 優先度
nullptr, // ハンドル不要
tskNO_AFFINITY // コア指定なし
);
auto initStatus = ROBO_WCOM::Init(MACADDRESS_BOARD_CONTROLLER, MACADDRESS_BOARD_ROBO, millis(), 1000);
Serial.print("Controller started. : Status=");
Serial.println(ROBO_WCOM::ToString(initStatus));
}
/**
* @brief ステータス表示用のタスク
*
* @param pvParameters
*/
void statusViewer(void* pvParameters)
{
char reportedString[128];
TickType_t lastWake = xTaskGetTickCount();
// 実行周期=50ms
const TickType_t period = pdMS_TO_TICKS(50);
ROBO_WCOM::Status readBufferStatus;
for (;;)
{
// ロボット側からの受信データをバッファから取り出す
for (;;)
{
readBufferStatus = ROBO_WCOM::PopOldestPacket(millis(), &rcvTimeStamp, rcvAddress, reinterpret_cast<uint8_t*>(&rcvStatus), &rcvSize);
// バッファからデータを取り出せなくなったら受信を中止
if (readBufferStatus == ROBO_WCOM::Status::BufferEmpty)
{
break;
}
// タイムアウト状態の場合それを通知し受信を中止
else if (readBufferStatus == ROBO_WCOM::Status::Timeout)
{
Serial.println("CONNECTION REFUSE");
break;
}
// CRCエラーの場合エラーを通知
else if (readBufferStatus == ROBO_WCOM::Status::CrcError)
{
Serial.println("!!CRC ERROR!!");
}
// 正常に受信できているならデータを
else
{
snprintf(reportedString, 128, "Time,%ld,POWER,%f,%f,%f,MOTORS,%f,%f,%f,%f,FLAGS,%0x,%0x",
rcvTimeStamp,
rcvStatus.Power.voltage, rcvStatus.Power.current, rcvStatus.Power.wh,
rcvStatus.motors[MOTOR_CH_FL], rcvStatus.motors[MOTOR_CH_FR], rcvStatus.motors[MOTOR_CH_RL], rcvStatus.motors[MOTOR_CH_RR],
rcvStatus.WEAPON_FLAGS.FLAGS, rcvStatus.MANSWICH.SWITCHES);
Serial.println(reportedString);
}
}
vTaskDelayUntil(&lastWake, period);
}
}
/** コントローラ部 適当な指示をロボットに送る **/
float vx;
bool vx_mode;
float vw;
bool vw_mode;
uint8_t wp;
void loop(void)
{
char cmdString[32];
uint32_t nowMillis;
// 武器フラグは乱数にしておく
wp = random(0, 0xFF);
// vx, vyを -100~100の間で増減させる
if (vx_mode)
{
vx += 0.05;
}
else
{
vx -= 0.05;
}
if (vw_mode)
{
vw += 0.01;
}
else
{
vw -= 0.01;
}
if (vx < 100.0)
{
vx_mode = true;
}
if (100.0 < vx)
{
vx_mode = false;
}
if (vw < 100.0)
{
vw_mode = true;
}
else
{
vw_mode = false;
}
// ロボット側へ送信するデータをまとめて送信
sendCommand.velocity.x = vx;
sendCommand.velocity.y = 0;
sendCommand.velocity.omega = vw;
sendCommand.WEAPON_FLAGS.FLAGS = wp;
nowMillis = millis();
ROBO_WCOM::SendPacket(nowMillis, reinterpret_cast<uint8_t*>(&sendCommand), sizeof(RoboCommand));
// デバッグ用に送信した時刻だけ表示する
snprintf(cmdString, 32, "<< SEND CMD : %ld >>", nowMillis);
Serial.println(cmdString);
delay(20);
}
ロボット側
(ロボット側)main.cpp
/***************************************************************************************************/
/*=========================== ROBO ================================================================*/
/***************************************************************************************************/
#include <Arduino.h>
#include <stdio.h>
#include <string.h>
#include "ROBO_WCOM.h"
#include "robo_packet.h"
#include "controller_packet.h"
#define CH_IRQ_TIMER 1
#define CONFORG_TIMER_DIV 80
#define CONFORG_TIMER_PRD_10MS 10000
#define CONFORG_TASK0_PRD_US 20000
#define CONFORG_TASK1_PRD_US 20000
//---------------------------------------------
// タスクハンドラ
//---------------------------------------------
TaskHandle_t taskHandler[2];
hw_timer_t* hwtimer = NULL;
//---------------------------------------------
// 通信相手のMACアドレス設定
//---------------------------------------------
uint8_t MACADDRESS_BOARD_CONTROLLER[6] = { 0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX};
uint8_t MACADDRESS_BOARD_ROBO[6] = { 0xYY, 0xYY, 0xYY, 0xYY, 0xYY, 0xYY};
// パワー関係の変数
float battery_voltage;
float power_current;
float wh;
// モータのCH
#define MOTOR_CH_FL 0
#define MOTOR_CH_FR 1
#define MOTOR_CH_RL 2
#define MOTOR_CH_RR 3
float motor_power[3];
uint8_t wp_flg;
uint16_t sw_flg;
/**送受信するデータ**/
RoboCommand rcvCommand;
RoboStatus sendStatus;
void MainTaskCore0(void *pvParameters);
void MainTaskCore1(void *pvParameters);
void TimerInterrupt(void);
void setup()
{
Serial.begin(115200);
Serial.println("Board Boot");
Serial.println("==== THIS IS ROBO ====");
auto initStatus = ROBO_WCOM::Init(MACADDRESS_BOARD_ROBO, MACADDRESS_BOARD_CONTROLLER, millis(), 1000);
Serial.print("Communication started. : Status=");
Serial.println(ROBO_WCOM::ToString(initStatus));
hwtimer = timerBegin(CH_IRQ_TIMER, CONFORG_TIMER_DIV, true);
timerAttachInterrupt(hwtimer, &TimerInterrupt, true);
timerAlarmWrite(hwtimer, CONFORG_TIMER_PRD_10MS, true);
timerAlarmEnable(hwtimer);
xTaskCreateUniversal( MainTaskCore0, "MainTaskCore0",
/* スタックサイズ タスクに渡すパラメータ(?) */
8192, NULL,
/* 優先度 タスクハンドラ 配置するコア */
1, &taskHandler[0], 0);
/* 関数名 関数名(文字列) */
xTaskCreateUniversal( MainTaskCore1, "MainTaskCore1",
/* スタックサイズ タスクに渡すパラメータ(?) */
8192, NULL,
/* 優先度 タスクハンドラ 配置するコア */
2, &taskHandler[1], 1);
}
/** Report and Logging **/
void loop()
{
uint32_t nowMillis = millis();
char loggingString[128];
// コントローラ側に送り返すデータをローカルでも表示しておく
snprintf(loggingString, 128, "RoboTime,%ld,POWER,%f,%f,%f,MOTORS,%f,%f,%f,%f,FLAGS,0x%02X,0x%04X",
nowMillis,
battery_voltage, power_current, wh,
motor_power[MOTOR_CH_FL], motor_power[MOTOR_CH_FR], motor_power[MOTOR_CH_RL], motor_power[MOTOR_CH_RR],
wp_flg, sw_flg);
Serial.println(loggingString);
// コントローラ側へ送信するデータをまとめる
sendStatus.Power.voltage = battery_voltage;
sendStatus.Power.current = power_current;
sendStatus.Power.wh = wh;
sendStatus.motors[MOTOR_CH_FL] = motor_power[MOTOR_CH_FL];
sendStatus.motors[MOTOR_CH_FR] = motor_power[MOTOR_CH_FR];
sendStatus.motors[MOTOR_CH_RL] = motor_power[MOTOR_CH_RL];
sendStatus.motors[MOTOR_CH_RR] = motor_power[MOTOR_CH_RR];
sendStatus.WEAPON_FLAGS.FLAGS = wp_flg;
sendStatus.MANSWICH.SWITCHES = sw_flg;
ROBO_WCOM::SendPacket(millis(), reinterpret_cast<uint8_t*>(&sendStatus), sizeof(sendStatus));
delay(50);
}
/** Measurement **/
/**計測関係は同時性が大事なのでハードウェア割り込みに入れておく**/
void TimerInterrupt(void)
{
// 仮想的に電流を計測したことにする(モータが動けば電流を消費するイメージ)
float current = ( motor_power[MOTOR_CH_FL] * motor_power[MOTOR_CH_FL] +
motor_power[MOTOR_CH_FR] * motor_power[MOTOR_CH_FR] +
motor_power[MOTOR_CH_RL] * motor_power[MOTOR_CH_RL] +
motor_power[MOTOR_CH_RR] * motor_power[MOTOR_CH_RR] ) * 0.1;
// 電流に応じて12Vの電源電圧が減る想定
float voltage = 12 - 0.1 * current;
battery_voltage = voltage;
power_current = current;
// 積算電力を計算しておく
wh += current * voltage * 0.01 / 3600;
}
// メインコントロール
// 受信したデータに応じてロボットを制御する
void MainTaskCore0(void *pvParameters)
{
uint32_t rcvTimeStamp;
uint8_t controllerAddress[6];
uint8_t rcvSize;
TickType_t lastWake = xTaskGetTickCount();
const TickType_t period = pdMS_TO_TICKS(20);
while(1)
{
auto rcvStatus = ROBO_WCOM::PeekLatestPacket(millis(), &rcvTimeStamp, controllerAddress, reinterpret_cast<uint8_t*>(&rcvCommand), &rcvSize);
if (rcvStatus == ROBO_WCOM::Status::Ok)
{
motor_power[MOTOR_CH_FL] = rcvCommand.velocity_x - rcvCommand.omega;
motor_power[MOTOR_CH_FR] = rcvCommand.velocity_x + rcvCommand.omega;
motor_power[MOTOR_CH_RL] = motor_power[MOTOR_CH_FL];
motor_power[MOTOR_CH_RR] = motor_power[MOTOR_CH_FR];
wp_flg = rcvCommand.WEAPON_FLAGS.FLAGS;
}
// 正常な受信ができていない場合モータを止めておく
else
{
motor_power[MOTOR_CH_FL] = 0;
motor_power[MOTOR_CH_FR] = 0;
motor_power[MOTOR_CH_RL] = 0;
motor_power[MOTOR_CH_RR] = 0;
wp_flg = 0x00;
}
vTaskDelayUntil(&lastWake, period);
}
}
// UIの管理
void MainTaskCore1(void *pvParameters)
{
TickType_t lastWake = xTaskGetTickCount();
const TickType_t period = pdMS_TO_TICKS(50);
while(1)
{
// ランダムに押される状況を想定
sw_flg = random(0, 0xFFFF);
vTaskDelayUntil(&lastWake, period);
}
}
実験結果
コントローラ側が送信した結果、ロボット側がモータのパワーを決定したり武器フラグを操作、コントローラ側にその結果が帰ってきているのがわかります。
