【MFT2019】1号機はシンプルにまとめる方向<力覚制御技術は大変だ>

 

クワエルマウス開発中ですが、単なる力センサの世界ではないことが判ってきました。
いくら多機能で精度の良い力センサを作っても、人間の制御する力が複雑なため
単純なマウスポインタとして変換するだけでも、非常に多くの作業が必要なことが判ってきました。

短期間では、完成度の高いモノが出来ないので、5月8日の出展申請には、機能限定版のシンプルで原理が見える作品に仕上げることにしました。
●全体の原理と構造図


●技術課題

①歯から発生する力は、合力しかでてこない
歯の上下で噛む力は、主に噛む圧縮力と顎を前後に動作させる前後せん断力の2軸があります。左右の力は少なくしかも、噛む力と前後せん断力と同時に発生するので、単独で左右動作力が発生することがありません。そのため、左右動作と同時に上下動作のせん断力が発生して勝手にカーソルが斜め移動になってしまう現象が多発しました。
=>しょうがないので、一動作で一方向移動の仕様に変更しました。将来的には、通常のマウス同様の全方向自在のクワエルマウスにするつもりですが、初歩段階で未だ到達できないと判断しました。

②歯から発生する力を受ける接触部材は、レオロジー的な視点で選ばないといけない。当初は、歯をネジ止めするような剛体接合すれば問題ないと思っていたのですが、実用上歯を痛めるとかよだれがが多すぎるとか実使用上の課題が多く、できるだけ、口先だけで、センサを奥にいれない方式がよいことが判ってきました。そのためクワエルだけで力を伝達する機構が必要となりました。
歯医者さんの解説記事をみると 現在使っている歯は切歯(中歯+側歯x左右x上下=8本)で
センサを支えながら、前後せん断力で上下移動、左右に振るせん断力で左右移動をさせてます。

これらの歯の食い込み方で、操作性が格段と向上するので、ゴム材料を集めてきてテストしてます。
現段階では、滑り止めようゴム材がいい感じです。要するに切歯が滑らずにめり込みながら力を伝達できるような材料と物性が必要です。
パラメータ1:歯でくわえて引っ張っても滑らない材料
パラメータ2:思い切り噛んでもめり込んで、柔らかくならない
パラメータ3:ハニカム的な穴構造

③噛んで支えている状態が時々刻々と変動する
  静置してあるわけではなく、口で咥えてセンサを支えているので噛む力、歯の当たり具合
顎の姿勢で、コロコロ、センサのゼロ設定値が変動します。
この変化を毎回TEARしながら、波形のプラスマイナスの区別をして、カーソルの向きを決めるプログラムのアルゴリズムが必要です。
今回は、10サンプリングデータの平均値とばらつきを連続的にとって、人間が除荷しているジ状態を検知して、ゼロ点をTEARしてます。

④カーソルの動かし方を最適化する
これは、IBMのトラックパッドの技術なのですが、多軸センサで分力の方向を決めたあと
その方向の力の大きさで、カーソルの速度を制御します。そうすることによって、各段に操作性が
あがります。しかし、これは、指でやる場合と歯で噛みながらやる場合で、最適化数値が違ってきます。さらに、計算方法も、数学的に処理することでよりスムーズなUIになると思います。
今回は、時間がないので、初歩的な加速制御で試行錯誤的にやっているので、カクカクした
動きになってしまって、ポインタ位置への位置決めがなかなか決まりませんが、この最適化処理を高度化することで、UIとして、使えるようになると思います。力覚センサ技術の一端を見たと感じました。

とりあえず、備忘録でmbedプログラム残しておきます。

//実装接続プログラム2016/10/16 rev.1.0
//単一プログラムでライブラリー化せず
// ********************WBite Mouse*****************************
// Target NUCLEO L432KC
//
// OMRON D6F-PH5050 I2C diff pressure meter i2C(PB_7.PB_6) SDA,SCL
// HX711
//*************************************************************
//————————————Header ———————————–#include “mbed.h”
#define D6F_ADDR 0xD8 //I2Cアドレス定義
#include “HX711.h”
//For L432KC 3CH board
HX711 scale1(PB_5,PB_4);//(DATA,CLOCK)
HX711 scale2(PA_1,PA_0);//(DATA,CLOCK)
HX711 scale3(PA_3,PA_4);//(DATA,CLOCK)
I2C i2c1(PB_7,PB_6);//SDA SCL
DigitalOut led1(LED1);
DigitalOut led2(LED2);
Serial pc(USBTX, USBRX);//USB serial
Serial xbee(PA_9,PA_10);//Tx Rx
Timer t;
//————————–HX711—————————————-//
int i,j,k;
int t0,t1,t2,t3;
int i0,i1,i2;
int yMove,xMove;
float distx=50;
float disty=50;
float w1,w2,w3;
float w1A[10]={};
float w2A[10]={};
float w3A[10]={};
float w1_Max,w1_Min,w2_Max,w2_Min;
float w1_level,w2_level,w3_level;
float w1z,w2z;//zero点補正済み
float w1_1,w2_1,w3_1;
float Fx,Fy,Fx_1,Fy_1;
float Fx_level,Fy_level;//
float xstep,ystep;
float sum_w1,sum_w2,sum_w3;
float ave_w1,ave_w2,ave_w3;
//————————-D6FPH 定義————————————//
float tmpr=20;
float Cof=0.9;
int wtime=50;
static const char d6f_config[5]={0x00,0xD0,0x40,0x18,0x06};
static const char d6f_comp_read[4]={0x00,0xD0,0x51,0x2c};//差圧データx51hを指定
static const char d6f_temp_read[4]={0x00,0xD0,0x61,0x2c};//温度テータx61hを指定
static const char d6f_mem_read[1]={0x07};//リードレジスタx07hから読む
static const char d6f_init[2]={0x0B,0x00};//初期化レジスタx0Bhをx00でリセット
//—————————————————————————–
//初期化関数//uint8_t d6fph_strt(void) {
uint8_t error;//D6F-PHのレジスタ通信データを宣言・初期化する

error=i2c1.write(D6F_ADDR,d6f_init,2);

return(error);
}

//差圧読み込み関数//
float read_pressure(void) {
char error;
char rdata[2];
uint16_t raw_diff_pa;
float diff_pa;
error=i2c1.write(D6F_ADDR,d6f_config,5);
wait_us(33000);
error=i2c1.write(D6F_ADDR,d6f_comp_read,4);
error=i2c1.write(D6F_ADDR,d6f_mem_read,1,true);
error=i2c1.read(0xD9,rdata,2);
if (error){
return(error);
}
raw_diff_pa=(rdata[0]<<8)+rdata[1];
diff_pa=((float)raw_diff_pa-1024)/60-500;
return(diff_pa);
}
//温度読み取り関数//
float read_d6f_temp(void) {
char error;
char rdata[2];
uint16_t raw_ref_temp;
float ref_temp;

i2c1.write(D6F_ADDR,d6f_config,5);
wait_us(33000);
error=i2c1.write(D6F_ADDR,d6f_temp_read,4);
error=i2c1.write(D6F_ADDR,d6f_mem_read,1,true);
error=i2c1.read(0xD9,rdata,2);
if(error){
return(error);
}
raw_ref_temp=(rdata[0]<<8)+rdata[1];
ref_temp=((float)raw_ref_temp-10214)/37.39;
return(ref_temp);
}

//cout関数

//関数定義終わり—————————//

//————————-main—————————
// データ読みこみ毎にLEDが点滅する

int main() {
float prs;

prs=0;
pc.baud(115200);
xbee.baud(9600);//To Arduino SoftSerial
sum_w1=0;
sum_w2=0;
sum_w3=0;
for (i=1;i<100;i++){
wait(0.05);
w1 =scale1.getGram();
w2 =scale2.getGram();
w3 =scale3.getGram();
sum_w1=sum_w1+w1;
sum_w2=sum_w2+w2;
sum_w3=sum_w3+w3;
}
ave_w1=sum_w1/100;
ave_w2=sum_w2/100;
ave_w3=sum_w3/100;

t.start();
t0=t.read_ms();//start time=t0
w1_level=0;
w2_level=0;
while(1){
i0++;
wait_ms(wtime);
//pc.printf(“measuring\n\r”);
int er=d6fph_strt();
//pc.printf(“er=%d,d6fph_strt=%d\n\r”,er,d6fph_strt());
prs=read_pressure();//差圧データ読んで代入////
//=========1個前のデータ保持================
w1_1=w1;
w2_1=w2;
w3_1=w3;
//=========HX711==============
w1 =scale1.getGram();//Fy1
w2 =scale2.getGram();//Fy2
w3 =scale3.getGram();//Non
w1A[i0%10]=w1;
w2A[i0%10]=w2;
w3A[i0%10]=w3;
//===============10データ最大最小値平均===================

sum_w1=0;
sum_w2=0;

//if (i0%10==0){
w1_Max=w1A[0];
w1_Min=w1A[0];
w2_Max=w2A[0];
w2_Min=w2A[0];
//pc.printf(“——MaxMinInit:w1A[0]%3.0f,w1_Max%3.0f,w1_Min%3.0f,w2A[0]%3.0f,w2_Max%3.0f,w2_Min%3.0f——–\n\r”,w1A[0],w1_Max,w1_Min,w2A[0],w2_Max,w2_Min);
//pc.printf(“<<<<<Main:%d:w1A[0]=%3.0f,w2A[0]=%3.0f,w1_level=%3.0f,w2_level=%3.0f,w1z=%3.0f,w2z=%3.0f>>>>>\n\r”,i0,i0%10,w1A[0],i0%10,w2A[0],w1_level,w2_level,w1z,w2z);
for (i=1;i<=9;i++){
//pc.printf(“%d=========w1[%d]=%3.0f,w2[%d]=%3.0f==============\n\r”,i0,i,w1A[i],i,w2A[i]);
if(w1A[i]>=w1_Max){w1_Max=w1A[i];}//;pc.printf(“%d:if,w1_Max=%4.0f\n\r”,i,w1_Max);}
if(w2A[i]>=w2_Max){w2_Max=w2A[i];}//;pc.printf(“%d:if,w2_Max=%4.0f\n\r”,i,w2_Max);}
if(w1A[i]<=w1_Min){w1_Min=w1A[i];}//;pc.printf(“%d:if,w1_Min=%4.0f\n\r”,i,w1_Min);}
if(w2A[i]<=w2_Min){w2_Min=w2A[i];}//;pc.printf(“%d,if,w2_Min=%4.0f\n\r”,i,w2_Min);}
//pc.printf(“if:w1A[%d]=%3.0f,w2A[%d]=%3.0f,w1_Max=%3.0f,w1_Min=%3.0f,w2_Max=%3.0f,w2_Min:%3.0f\n\r”,i,w1A[i],i,w2A[i],w1_Max,w1_Min,w2_Max,w2_Min);
wait_ms(1);
sum_w1+=w1A[i];
sum_w2+=w2A[i];
}
//pc.printf(“w1[]:%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,w1_Max=%3.0f,w1_Min=%3.0f\n\r”,w1A[0],w1A[1],w1A[2],w1A[3],w1A[4],w1A[5],w1A[6],w1A[7],w1A[8],w1A[9],w1_Max,w1_Min);////////
//pc.printf(“w2[]:%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,%3.0f,w2_Max=%3.0f,w2_Min=%3.0f\n\r”,w2A[0],w2A[1],w2A[2],w2A[3],w2A[4],w2A[5],w2A[6],w2A[7],w2A[8],w2A[9],w2_Max,w2_Min);/////////////////////
//pc.printf(“%d:,w1_ave=%4.0f,w1_Max=%4.0f,w1_Min=%4.0f\n\r”,i0,sum_w1/10,w1_Max,w1_Min);
//pc.printf(“%d:,w2_ave=%4.0f,w2_Max=%4.0f,w2_Min=%4.0f\n\r”,i0,sum_w2/10,w2_Max,w2_Min);
if (abs(w1_Max-w1_Min)<4){w1_level=sum_w1/10;}//pc.printf(“———w1_Tared=%4.0f——\n\r”,w1_level);}
if (abs(w2_Max-w2_Min)<4){w2_level=sum_w2/10;}//pc.printf(“———w2_Tared=%4.0f——\n\r”,w2_level);}
w1z=w1-w1_level;
w2z=w2-w2_level;

//********************************gain set**********************************************
float Fygain=1.2;
float Fxgain=2;
Fy=(w1z+w2z)*Fygain;//w1+w2和がFy
Fx=(w1z-w2z)*Fxgain;//w1-w2差がFx左右バランス
//}
//***************************************************************************************

//Up-Down-Right-Left
//U/D/R/L/UR/UL/DR/DL8通り
float ystep_level=200;
float xstep_level=200;
//================加速処理=======================
if(abs(Fy)<=300){disty=20;}
if (abs(Fy)>300 && abs(Fy)<=400 ){disty=30;}
if (abs(Fy)>400 && abs(Fy)<=600 ){disty=90;}
if(abs(Fy)>600){disty=100;}
//—————
if(abs(Fx)<=500){distx=20;}
if (abs(Fx)>500 && abs(Fx)<=600 ){distx=70;}
if (abs(Fx)>600 && abs(Fx)<=700 ){distx=90;}
if(abs(Fx)>700){distx=100;}
//==========================================

if (Fy<=-300){ystep=-1*disty;xstep=0;}//D=下移動のみ
else if(Fy>=300){ystep=disty;xstep=0;}//U=上移動のみ
else if(Fx<=-200){xstep=distx;ystep=0;}//左移動
else if(Fx>=200){xstep=-1*distx;ystep=0;}//右移動
else if(abs(Fx)<xstep_level && abs(Fy)< ystep_level){ystep=0;xstep=0;}//停止
pc.printf(“%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%d\n\r”,w1,w2,w1z,w2z,Fx,Fy,xstep,ystep,t.read_ms());//For Calibrations
//pc.printf(“w1=%4.0f,w2*gain=%4.0f,w3=%4.0f,pre=%4.0f\n\r”,w1,w2*gainz,w3,prs);//For Calibrations
//pc.printf(“%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%4.0f,%4.0f\r\n”,My,Fz,Mys,Fzs,w3,ystep,Dxs);

xbee.printf(“%4.0f,%4.0f,%4.0f,%3.2f,\r\n”,xstep,ystep,w3,prs);

}
}

 

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です