二足歩行ロボット動作データ作成支援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_