3Dプリンターでロボットアーム作ってみた(SDカードに動作記憶再現テスト編)
ロボットアームシリーズ4回目の投稿です。
どうせなら、自分で動作させた動きを記録して再現できたら面白いかなと思い早速テストしてみました。
色々とうまく動作しなくてトラブっていましたが、なんとか動きましたので紹介します。
最初に接続図です。
追加した部品は見ての通りです。
SDカードモジュール
LED＋抵抗
ブザー
押しボタンスイッチ
動作の流れ
・動作記録するのに、RECボタンを押してメモリーモードにします。
・次にロボットアームを適当に動かします。
・STOPボタンで記録終了
・PLAYボタンでSDカードから記録データを読み込んで動きを再現します。
・その後、自動で終了します。
と言った具合です。
送信側は、変更ないので以前のままです。
受信側のプログラムはこんな感じです。
// ロボットメモリーアーム ジョイスティック操作 受信側 ESP32
// ESP-NOW 無線受信 konan0119 Ver261105
//ライブラり #include <esp_now.h> #include <SD.h> #include <SPI.h> #include <WiFi.h> #include <ESP32Servo.h>
// 通信開始フラグ
bool dataReceived = false;
bool started = false;
bool initialApproachDone = false; // 起動直後のスロー移動完了フラグ
// REC/STOP関係 #define REC_BTN 4 #define PLAY_BTN 15 #define STOP_BTN 32 #define REC_LED 25 #define PLAY_LED 26 #define SD_CS_PIN 5 #define BUZZER_PIN 33
bool recording = false;
File recordFile;
unsigned long recordStartTime = 0;
unsigned long lastRecordTime = 0;
// PLAY関係
bool playing = false;
File playFile;
unsigned long playStartTime = 0; // 再生が始まった瞬間（ミリ秒）を記録するタイマー変数
int lastP1 = 90; // ベースサーボの前回の位置
int lastP2 = 170; // 肩サーボの前回の位置
int lastP3 = 100; // 肘サーボの前回の位置
int lastP4 = 35; // グリッパーの前回の位置
// 再生時の目標位置（滑らかに追従させるための変数）
float targetP1 = 90.0;
float targetP2 = 170.0;
float targetP3 = 100.0;
float targetP4 = 35.0;
// 次のデータを読み込むタイミングを管理するタイマー
unsigned long lastPlayStepTime = 0;
// サーボ定義
Servo servo1; // ベース回転
Servo servo2; // 肩
Servo servo3; // 肘
Servo servo4; // グリッパー
// ESP-NOW受信データ構造体 ＆ 変数定義
typedef struct{
int x1;
int y1;
int x2;
int y2;
} JoyData;
JoyData data;
// 現在のサーボ位置
float pos1 = 90.0; // ベース
float pos2 = 170.0; // 肩
float pos3 = 100.0; // 肘
float pos4 = 35.0; // グリッパー
// ESP-NOW受信時に呼ばれる関数
void OnDataRecv(const esp_now_recv_info_t *info,
const uint8_t *incomingData, int len){
memcpy(&data, incomingData, sizeof(data));
dataReceived = true;
}
// ブザー関数
void beep(int freq, int timeMs){
if (freq > 0) {
ledcAttach(BUZZER_PIN, freq, 8);
ledcWrite(BUZZER_PIN, 128);
}
delay(timeMs);
ledcWrite(BUZZER_PIN, 0);
ledcDetach(BUZZER_PIN);
}
// 初期化
void setup(){
Serial.begin(115200);
WiFi.mode(WIFI_STA);
WiFi.disconnect();
if (esp_now_init() != ESP_OK){
Serial.println("ESP-NOW初期化失敗");
return;
}
esp_now_register_recv_cb(OnDataRecv);
servo1.attach(18); // ベース
servo2.attach(19); // 肩
servo3.attach(21); // 肘
servo4.attach(22); // グリッパー
pinMode(REC_BTN, INPUT_PULLUP);
pinMode(PLAY_BTN, INPUT_PULLUP);
pinMode(STOP_BTN, INPUT_PULLUP);
pinMode(REC_LED, OUTPUT);
pinMode(PLAY_LED, OUTPUT);
digitalWrite(REC_LED, LOW);
digitalWrite(PLAY_LED, LOW);
SPI.begin(14, 27, 13, 5);
if(!SD.begin(SD_CS_PIN)){
Serial.println("SD初期化失敗");
}
else{ Serial.println("SD初期化成功");
}
Serial.println("Robot Arm Ready");
delay(3000);
}
// メインループ
void loop(){
// 最初にジョイスティックを大きく動かすまで待機
if(!started){
int diff1 = abs(data.y1 - 1850);
int diff2 = abs(data.x1 - 1850);
int diff3 = abs(data.x2 - 1850);
int diff4 = abs(data.y2 - 1850);
if(diff1 > 500 || diff2 > 500 || diff3 > 500 || diff4 > 500){
started = true;
}
else{ return;
}
}
if(!dataReceived){
return;
}
// 起動直後：最初の一回だけ初期位置へスローで移動する
if(!initialApproachDone){
pos1 += (90.0 - pos1) * 0.05;
pos2 += (170.0 - pos2) * 0.05;
pos3 += (100.0 - pos3) * 0.05;
pos4 += (35.0 - pos4) * 0.05;
servo1.write((int)pos1);
servo2.write((int)pos2);
servo3.write((int)pos3);
servo4.write((int)pos4);
if(abs(pos1 - 90.0) < 0.5 && abs(pos2 - 170.0) < 0.5 &&
abs(pos3 - 100.0) < 0.5 && abs(pos4 - 35.0) < 0.5){
pos1 = 90;
pos2 = 170;
pos3 = 100;
pos4 = 35;
initialApproachDone = true;
Serial.println("初期位置へのスロー移動完了。通常操作へ移行します。");
}
delay(20);
return;
}
// REC開始
if(digitalRead(REC_BTN) == LOW && !recording){
recording = true;
digitalWrite(REC_LED, HIGH);
recordFile = SD.open("/record.csv", FILE_WRITE);
recordStartTime = millis();
Serial.println("録画開始");
beep(1800, 100);
delay(300);
}
// PLAY開始
if(digitalRead(PLAY_BTN) == LOW && !playing){
Serial.println("PLAYボタン押された");
playing = true;
digitalWrite(PLAY_LED, HIGH);
playFile = SD.open("/record.csv");
if(playFile){
Serial.println("record.csv オープン成功");
playFile.seek(0);
// 再生開始時の位置を現在の位置として同期
targetP1 = pos1;
targetP2 = pos2;
targetP3 = pos3;
targetP4 = pos4;
}
else{
Serial.println("record.csv オープン失敗");
}
delay(300);
playStartTime = millis();
lastPlayStepTime = millis();
Serial.println("再生開始");
beep(1800, 300);
delay(300);
}
// STOP
if(digitalRead(STOP_BTN) == LOW){
if(recording){
recording = false;
digitalWrite(REC_LED, LOW);
recordFile.close();
Serial.println("録画終了");
beep(1800, 80);
delay(80);
beep(1800, 80);
}
if(playing){
playing = false;
digitalWrite(PLAY_LED, LOW);
playFile.close();
Serial.println("再生停止");
beep(1800, 100);
}
delay(300);
}
// 再生処理（滑らか補間対応）
if(playing){
if(millis() - lastPlayStepTime >= 20) {
lastPlayStepTime = millis();
if(playFile.available()) {
String line = playFile.readStringUntil('\n');
int t, p1, p2, p3, p4;
int ret = sscanf(line.c_str(), "%d,%d,%d,%d,%d", &t, &p1, &p2, &p3, &p4);
if(ret == 5){
targetP1 = p1;
targetP2 = p2;
targetP3 = p3;
targetP4 = p4;
}
}
else{
playFile.close();
playing = false;
digitalWrite(PLAY_LED, LOW);
Serial.println("再生終了");
beep(1800, 200);
delay(100);
beep(1800, 200);
return;
}
}
//滑らかに動かしたい場合: 0.3 の数値を 0.15 などに小さくする
pos1 += (targetP1 - pos1) * 0.15;
pos2 += (targetP2 - pos2) * 0.15;
pos3 += (targetP3 - pos3) * 0.15;
pos4 += (targetP4 - pos4) * 0.15;
servo1.write((int)pos1);
servo2.write((int)pos2);
servo3.write((int)pos3);
servo4.write((int)pos4);
delay(10);
return;
}
// 通常時のジョイスティック操作
static int baseFilter = 1875;
baseFilter = (baseFilter * 7 + data.y1) / 8;
//ベース
int diff1 = baseFilter - 1875;
if(diff1 > 500){
pos1 += map(diff1, 500, 2200, 1, 2);
}else if(diff1 < -500){
pos1 -= map(-diff1, 500, 2200, 1, 2);
}
//肩
int diff2 = data.x1 - 1850;
if(diff2 > 300){
pos2 -= map(diff2, 300, 2200, 1, 2);
}else if(diff2 < -300) {
pos2 += map(-diff2, 300, 1850, 1, 2);
}
//肘
int diff3 = data.x2 - 1850;
if(diff3 > 300){
pos3 += map(diff3, 300, 2200, 1, 2);
} else if(diff3 < -300){
pos3 -= map(-diff3, 300, 1850, 1, 2);
}
//グリッパー
int diff4 = data.y2 - 1850;
if(diff4 > 300) {
pos4 += map(diff4, 300, 2200, 1, 3);
}else if(diff4 < -300){
pos4 -= map(-diff4, 300, 1850, 1, 3);
}
// 可動範囲制限
pos1 = constrain(pos1, 0, 180);
pos2 = constrain(pos2, 90, 170);
pos3 = constrain(pos3, 100, 180);
pos4 = constrain(pos4, 0, 70);
// サーボへ出力
servo1.write((int)pos1);
servo2.write((int)pos2);
servo3.write((int)pos3);
servo4.write((int)pos4);
// 記録中ならSDへ保存
if(recording){
if(millis() - lastRecordTime > 5){
lastRecordTime = millis();
recordFile.print(millis() - recordStartTime);
recordFile.print(",");
recordFile.print((int)pos1);
recordFile.print(",");
recordFile.print((int)pos2);
recordFile.print(",");
recordFile.print((int)pos3);
recordFile.print(",");
recordFile.println((int)pos4);
recordFile.flush();
}
}
delay(10);
}
何か簡単なことをさせるには面白いかもしれません。
プログラムを改造して同じ動作を繰り返し実行させるとか、動作パターンを数種類登録して好みの動作を読み出して、実行させたりとか色々応用は可能ですね。
AIに聞いてみると色々教えてくれますので、試してください。
モノとしては学習用的な作りになっているので、ちょっと応用的な使い方にするには改造か、ネットで他のロボットアームを捜してみるのがいいかと思います。
これからケースを製作して、基板を収めようかと思ったのですが、プレイステーションのジョイスティックで操作できる動画を発見したので、試してみようかと思っているところです。
無事動いたら、また紹介しますので期待しないで待っててください。
皆さんの参考になれば幸いです。
それでは、また。
