二足歩行ロボット動作データ作成支援3
二足歩行ロボットのソフト作成
二足歩行ロボットもほぼそのまま書籍の通り作った。「この記事」を参考にして、三端子レギュレータをNJM2845に変更しました。しかし自分の環境ではまだ動作が安定しなかった為、3.3v電源に接続されている47uFの電解コンデンサーを470uFの物に変更しました。他には、起き上がりモーション用に足の可動範囲を広げる為、腿や膝のパーツを外しています。
プログラムを以下に示します。サーボ動作関係の部分は、書籍からそのまま持ってきていますが、元々書籍に記述されている物ですのでサーボ動作部分を削除しています。BluetoothSerialで受信した文字データを数値データに変換している部分は、別ファイルにしています。
メインプログラム
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
#include "rx_angle.h"
////////////////////////////////////////////////////////
// マクロ
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
// 変数
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
// 各種初期化
////////////////////////////////////////////////////////
// Serial/BluetoothSerial初期化
void srl_bt_srl_init(void)
{
Serial.begin(115200); // シリアルBluetooth共に同じボーレートが設定される。
SerialBT.begin("ESP32test");
}
////////////////////////////////////////////////////////
// 初期化メイン
////////////////////////////////////////////////////////
void setup() {
// I/O関係初期化
srl_bt_srl_init(); // Serial/BluetoothSerial初期化
// 各種タスク初期化
rx_agl_task_init(); // Bluetooth受信タスク ここで受信する前から90°に初期化
}
////////////////////////////////////////////////////////
// 各種タスク
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
// アイドルループ
////////////////////////////////////////////////////////
void loop() {
rx_agl_task(); // Bluetooth受信タスク
}データ変換プログラム"rx_angle.h"
#if !defined(_RX_ANGLE_H_)
#define _RX_ANGLE_H_
////////////////////////////////
// 変数
////////////////////////////////
char rx_agl_str[16][10]; // 受信キャラクタ
int rx_agl_str_num; // 受信中チャンネル
int rx_agl_str_index; // 受信中文字数
int rx_agl_data[16]; // 角度数値データ(12個受信完了する毎にアップデート)
int rx_agl_sqc; // 受信処理実行中シーケンス
////////////////////////////////
// 関数
////////////////////////////////
// ライブラリ関数の替わり
// isdigit()
bool my_isdigit(int ch)
{
if ('0' <= ch && ch <= '9') return true;
else return false;
}
// atoi()
// int範囲を超えると不定。
// -側の方が絶対値が大きいから-側で計算
// オーバーフローチェックを入れた。
int my_atoi(char *ss)
{
// 初期化
int retdt = 0; // 戻り値
bool minus = false; // 符号
// 先行空白無視
while (*ss == ' ') ss++;
// 符号読み取り
if (*ss == '-') {
minus = true;
ss++;
}
else if (*ss == '+') ss++;
// 数値の読み取り、計算、オーバーフローチェック
while (my_isdigit(*ss)) {
retdt *= 10;
retdt -= (*ss++ - '0'); // -側で計算
if (retdt > 0) return retdt; // 整数オーバーフローが発生したら終わる。
}
// 符号処理
if (!minus) retdt *= -1;
// 終わり
return retdt;
}
// 通常関数
// 変数初期化(文字列関係)
void rx_agl_init(void)
{
for (int i = 0; i < 16; i++) {
for (int j = 0; j < 10; j++) {
rx_agl_str[i][j] = '\0';
}
}
rx_agl_str_num = 0;
rx_agl_str_index = 0;
}
// 角度データ受信処理初期化(シーケンスと数値データ)
void rx_agl_task_init(void)
{
rx_agl_sqc = 0;
for (int i = 0; i < 16; i++) {
rx_agl_data[i] = 90;
}
}
// 角度データ受信処理(定期的に実行する。受信したデータは全て処理しておく)
void rx_agl_task(void)
{
int ch; // 一文字受信用バッファ
while (SerialBT.available()) { // 受信用バッファチェック
ch = SerialBT.read(); // 受信
//Serial.write(ch); // debug1 単純エコーバックOK
// シリアルから文字列を受信して16個分受信したら数値データに変換する
switch (rx_agl_sqc) {
case 0: // 改行待ち(前の文字列の終端)エラー及び初回のみ実行
if (ch == '\n') {
rx_agl_init(); // 文字受信バッファー初期化
rx_agl_sqc++;
}
break;
case 1: // 文字受信処理
if (ch == '\n') { // 改行？
for (int i = 0; i < 16; i++) { // データ数値化
rx_agl_data[i] = my_atoi(rx_agl_str[i]);
}
for (int i = 0; i < 16; i++) { // debug2 数値化データ送信
//SerialBT.print(rx_agl_data[i]); SerialBT.print(",");
Serial.print(rx_agl_data[i]); Serial.print(",");
}
//SerialBT.println(); // 改行
Serial.println(); // 改行
rx_agl_init(); // 文字受信バッファー初期化
}
else if (ch == ',') { // カンマ？
if (rx_agl_str_num >= 16) // エラー(17個目のカンマを受信)
rx_agl_sqc = 0; // シーケンス初期化
else {
rx_agl_str_num++; // ブロックを進める
rx_agl_str_index = 0; // 文字最初
}
}
else if (my_isdigit(ch)) { // 数字なら真
// 文字保存バッファーオーバーフロー対策入り
if ((0 <= rx_agl_str_num && rx_agl_str_num <= 15) &&
(0 <= rx_agl_str_index && rx_agl_str_index <= 8)) {
rx_agl_str[rx_agl_str_num][rx_agl_str_index++] = ch;
}
}
break;
default: // エラー
rx_agl_sqc = 0; // エラー／初期化処理へ
break;
}
}
}
#endif // _RX_ANGLE_H_