Embedded Files
遠隔操作ハンド : Vector Hand
右手の動きだけで操作する。
ハンドを備えた移動体を操作
送信機 ESP32 アナログ入力３つをハンドロボットへ
アナログ入力1 ハンドの開閉、ハンドを掴みきったら上に上げる。離すと下に下げてからハンドを開く
アナログ入力2 手首のひねり 左右の動輪のバランス制御 右曲げで右に、左曲げで左に
アナログ入力3 肘のひねり モーター回転の強さ 90度を中心として+-で正転、逆転
移動体 ESP8266
モーター*2
サーボ*2
雑感
製作期間 2023/10 ～ 2023/11
ペットボトルに少し水を入れたら徐々に落ちていってしまう。つかみが弱いからだと思うがもう調整が面倒なのでOKとする。
触覚フィードバックも目星はついているが今回は実装しない。
・目星に関してのメモ
ポテンショメーターと戻りバネを一体化、小型化し、人差し指の物体接触面につける。
物体をつかめばバネが動き、どれぐらいの力が加わっているかを計測する。
ロボ側は力が一定値を超えた場合はサーボの動きを止める。
計測値を操作系にフィードバックし、例えば振動モーターで感覚を伝える。
さらにサーボか何かを使って力加減次第で操作系を動かなくすれば物体を掴んでいる感覚をフィードバックできる、はず。
--
動きに関しても操作性が悪く、これも調整したらよくなるかもしれんが調整はしない。
これに関しては立ち上がりのトルクを上げれば微妙な力加減でもいい感じに動いてくれると思われるのだが
それをするにはモーターを変えなくてはいかんので面倒なのでしない。
Esp-Nowでどれだけ反応速度が上がるのか知らんがあまり大差はないのではと思っているがその差で操作感が変わるなら実装したい。
使用したサーボはちょい強めのやつでMG996Rの180度版、駆動電圧6.6Vから4.8V。電源が4.4Vぐらいを下回るとぐるぐる動いてしまう。
電源はマイコンとは別にしたが電池駆動は徐々に電圧が下がるので安全策として一定角度を超えたらサーボと指引っ張り機構が外れるようにした。
持ち上げ部はピンが折れる感じ。ここはもう少し詰めたいところだが…。まあ次回ということで。
2023.12.3 追記
調整はしないつもりだったが余裕が出来たのでESP-Nowを実装してみた。
正直UDP通信との違いがよくわからん。さらに余裕があれば遅延計測プログラムを作りたいがコードはESP-Nowの方がスッキリするので
今後はこちらを採用していきたい所。
操作性が悪かったのはアナログ入力値の扱いに問題があった為でプログラムを修正したらけっこういい感じになった。
//操作側 ESP32 アナログ入力3個 → ロボへ送信
#include <esp_now.h>
#include <WiFi.h>
//受信側のMAC Addressを記述
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
int delay_cnt = 0;
int v0 = 0;
int v1 = 0;
int v2 = 0;
uint8_t LED[12];
char buf[12];
esp_now_peer_info_t peerInfo;
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
//Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Deli_Success" : "Deli_Fail");
}
void setup() {
delay(1000);
Serial.begin(9600);
Serial.println("Serial.begin");
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
Serial.println("esp_now_init");
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
Serial.println("Failed to add peer");
return;
}
esp_now_register_send_cb(OnDataSent);
Serial.println("esp_now_register_send_cb");
}
void loop() {
v0 = alalogReadVal(v0, A0);
v1 = alalogReadVal(v1, A1);
v2 = alalogReadVal(v2, A2);
if (delay_cnt == 40) {
delay_cnt = 0;
int sv0 = map(v0, 0, 4095, 0, 510);//A0
int sv1 = map(v1, 0, 4095, 0, 510);//A1
int sv2 = map(v2, 0, 4095, 0, 510);//A2
sprintf(buf, "%3d %3d %3d", sv0, sv1, sv2);
memcpy(LED, buf, strlen(buf));
esp_err_t result = esp_now_send(broadcastAddress, LED, sizeof(buf));
//sprintf(buf, "%3d,%3d,%3d", sv0, sv1, sv2);
//Serial.println(buf);
} else {
delay_cnt++;
}
//Serial.println(delay_cnt);
//delay(10);
delay(1);
}
//ローパスフィルタ？処理
int alalogReadVal(int _v, int pin) {
return ( (_v * 0.8) + (analogRead(pin) * 0.2) );
}
//受信側 ESP8266 ロボ ← アナログ入力3個
#include <ESP8266WiFi.h>
#include <espnow.h>
#include <Servo.h>
//data rcv
char buf[13];
int delay_cnt = 0;
int _v0 = 0;
int _v1 = 0;
int _v2 = 0;
const int statusPin = 16;
//mortor
const int MT_PIN[4] =
{
4, 5, 12, 2
};
//Servo
Servo myservo1;
Servo myservo2;
const int SERVO1 = 13;
const int SERVO2 = 14;
int now_tukami_k = 70;//現在のつかみサーボ角度
int now_motiage_k = 40;
int v_wifi_hand = 0;
int _v_wifi_hand = 0;
void setup() {
delay(1000);
Serial.begin(9600);
pinMode(statusPin, OUTPUT);
digitalWrite(statusPin, HIGH);
for (int i = 0; i < 4; i++) {
pinMode(MT_PIN[i], OUTPUT);
digitalWrite(MT_PIN[i], LOW);
}
WiFi.mode(WIFI_STA);
if (esp_now_init() != ERR_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
Serial.println("esp_now_init");
esp_now_register_recv_cb(OnDataRecv);
Serial.println("esp_now_register_recv_cb");
myservo1.attach(SERVO1);
myservo2.attach(SERVO2);
myservo1.write(now_tukami_k);
myservo2.write(now_motiage_k);
}
void loop() {
//Serial.println(WiFi.macAddress());
delay(1);
delay_cnt++;
digitalWrite(statusPin, LOW);
}
//受信データ完了した時の処理
void OnDataRecv(uint8_t * mac, uint8_t *recvData, uint8_t len) {
digitalWrite(statusPin, HIGH);
memcpy(&buf[0], recvData, len);
buf[len] = '\0';
sscanf(buf, "%d %d %d", &_v0, &_v1, &_v2);
delay_cnt = 0;
/*
Serial.print("");
Serial.print(delay_cnt);
Serial.print(",");
Serial.print(_v0);//hiji open 360 c 310+-10 close 260
Serial.print(",");
Serial.print(_v1);//l 290 370+-15 450 r
Serial.print(",");
Serial.println(_v2);//hand open 260 close 180
/**/
moveServo();
moveMortor();
}
void moveServo() {
//物体を掴んでいるかのチェック 未実装
//hand open260 close 180 : 260 - 180 = 80step
int v_wifi_hand = _v2;
if(v_wifi_hand > 260) v_wifi_hand = 260;
if(v_wifi_hand < 180) v_wifi_hand = 180;
//値が更新されていればハンド用のサーボ（つかみ、持ち上げの2個）を動かす
if (_v_wifi_hand != v_wifi_hand) {
moveServo2(v_wifi_hand);
_v_wifi_hand = v_wifi_hand;
}
}
void moveServo2(int v) {
int k1 = ((v - 260) * -1) * 1;// 0 - 80
if (k1 <= 10) {
//最初の遊び
now_tukami_k = 70;
now_motiage_k = 40;
} else if (k1 <= 50) {
//つかみ
int tukami_v = k1 + 110;
now_tukami_k = tukami_v;
} else if (k1 <= 70) {
now_tukami_k = 160;
//持ち上げ
int motiage_v = (k1 - 50) * 2 + 40;
now_motiage_k = motiage_v;
} else {
//最後の遊び
now_tukami_k = 160;
now_motiage_k = 80;
}
//Serial.print("now_tukami_k"); Serial.println(now_tukami_k);
//Serial.print("now_motiage_k"); Serial.println(now_motiage_k);
myservo1.write(now_tukami_k);
myservo2.write(now_motiage_k);
}
void stopMortor() {
for (int i = 0; i < 4; i++) {
digitalWrite(MT_PIN[i], LOW);
}
}
void moveMortor() {
stopMortor();
int v0 = _v0;//hiji open 360 c 310+-10 close 260
int v1 = _v1;//tekubi l 290 c 370+-15 r 450
if ( v0 > 360) v0 = 360;
if ( v0 < 260 ) v0 = 260;
if ( v1 > 450) v1 = 450;
if ( v1 < 290 ) v1 = 290;
int r = v1 - 370;//r = -80 ... 80
int rp = 100;
int lp = 100;
//左右の力配分調整
if (r > 20) {
//R寄りにする、＝Lのパーセンテージを下げる
lp = map(r, 0, 80, 100, 50);
} else if (r < - 20) {
//L寄りに ＝Rのパーセンテージを下げる
rp = map(r, 0, -80, 100, 50);
}
//Serial.print("lrp=");Serial.print(lp); Serial.print(",");Serial.print(rp); Serial.println("");
int k = v0 - 310;// -50 0+-10 +50
int mortor_p = 55;//add v
//正転
if (k > 10) {
int p = k * 2 + mortor_p;
int p1 = p * rp / 100;
int p2 = p * lp / 100;
analogWrite(MT_PIN[0], p1);
analogWrite(MT_PIN[2], p2);
//Serial.print("p=");Serial.print(p1); Serial.print(",");Serial.print(p2); Serial.println("");
} else if (k < -10) {
int p = ( k * -1) * 2 + mortor_p;
int p1 = p * rp / 100;
int p2 = p * lp / 100 ;
analogWrite(MT_PIN[1], p1);
analogWrite(MT_PIN[3], p2);
//Serial.print("p=");Serial.print(p1); Serial.print(",");Serial.print(p2); Serial.println("");
}
}
Google Sites
Report abuse