DSC02223.JPG

ソフトウェア周りの改良で更に扱いやすくなりました.

DSC02224.JPG

その場で付け足したヒートシンク.まるで翼のよう.
FETも並列し,無理やり運用.
DSC02225.JPG
重さは3.75kg
改良点
・プロセッシングによりモニタリング.
 センサ値とモータ出力値をモニタリング.
 ついでにばねばかりで事前に計測したデータをもとにアシスト率を表示.
 デバッグ効率が上がった気がする.

PrtSc.JPG

・ローパスフィルタを実装.
 センサ値をより滑らかに扱えることに成功.
・比例制御部分を2次の関係式に.
 細やかな操作が可能に.アームを途中で止めることができるようになりました.
動作テスト
アシストOFFではほとんど上げることができません.
アシストONだと自由に上げられ,任意の位置で停止,ゆっくり降ろすことができます.

プロセッシングによるモニタリング
緑:感圧センサの生データ
黄色:ローパスフィルタ後の感圧センサ値
青色:モータ指令値
アシスト率を見ながら動作させると楽しい.

Arduino

// 感圧センサによりDCモータを制御するプログラム
// リミットスイッチ×2,握りスイッチ×1,感圧センサ×2,DCモータ×1
// 2012.12.28 shiroku and nakano
#include
#define IN1PIN (9) //モータドライバ用
#define IN2PIN (10) //モータドライバ用
#define PRESSPIN1 (2) //感圧センサ①
#define PRESSPIN2 (3) //感圧センサ②
#define LIMIT1 (18) //リミットスイッチ①
#define LIMIT2 (19) //リミットスイッチ②
#define SWITCHPIN (7) //握りセンサ
static int DFlag;
static int H1 = 10;
static int H2 = 9;
static int L1 = 15;//NegativeLogic
static int L2 = 14;//NegativeLogic
void motorDrivet(int, int);
void dedTime(int);
static int ival_filter = 0;
void setup()
{
Serial.begin(9600);
// Serial.begin(115200);
// pinMode(H1,OUTPUT);
// pinMode(H2,OUTPUT);
pinMode(L1,OUTPUT);
pinMode(L2,OUTPUT);
dedTime(300);
}
int Map( double dPr1, double dPr2, int iLimit1, int iLimit2, int iSwitch, double iIn1, double iIn2, int iOut1, int iOut2, boolean bConstrain = false )
{
static double val_filter = 0.0;
const float K_filter = 0.05; //フィルターのゲイン
//フィルタ
val_filter += ((dPr1 – dPr2) – val_filter) * K_filter;
//整数への変換
ival_filter = (0 < val_filter) ? (int)(val_filter + 0.5) : (int)(val_filter - 0.5); //四捨五入してint型に型変換 //フィードフォワード制御 //1次 //double dValue = (double)(val_filter - iIn1) * (iOut2 - iOut1) / (iIn2 - iIn1) + iOut1; //2次 double dValue = (double)(val_filter) / (iIn2) ; // -1 ~ 1の範囲に dValue = pow(dValue, 2); // 乗で増えていくように if(0 > val_filter) dValue = -dValue; //pow関数により全て正の値になったので,もともと負だった値は負にする
dValue = dValue * (iOut2); //モータ指令値の範囲に
if( dValue > 255.0 ) dValue = 255.0; //制御量の上限値
if( dValue < -255.0 ) dValue = -255.0; //制御量の下限値 int iValue = (0 < dValue) ? (int)(dValue + 0.5) : (int)(dValue - 0.5); //制御量を四捨五入し,int型に型変換 // リミットスイッチ if(iLimit1 == 0 && iValue > 0) iValue = 0;
if(iLimit2 == 0 && iValue < 0) iValue = 0; // 握りスイッチ if(iSwitch == 1) iValue = 0; if( bConstrain ){ int iOutMin, iOutMax; if( iOut1 < iOut2 ){ iOutMin= iOut1; iOutMax= iOut2; } else{ iOutMin= iOut2; iOutMax= iOut1; } if( iOutMin > iValue ){
return iOutMin;
}
if( iOutMax < iValue ){ return iOutMax; } } return iValue; } void motorDrive(int way , int value) /*way(0:free 1:rotate1way 2:rotate2way 3:breke) value:0~255*/ { /* ----VCC---- H1| |H2 | | |-----M-----| | | L1| |L2 ----GND---- */ switch(way) { case 0: if(DFlag =! 0) {dedTime(30); } /*flag check*/ /*dedtime*/ digitalWrite(L1,HIGH); digitalWrite(L2,HIGH); digitalWrite(H2,LOW); digitalWrite(H1,LOW); DFlag = 0; break; case 1: if(DFlag =! 1) {dedTime(30); } /*flag check*/ /*dedtime*/ digitalWrite(L1,HIGH); digitalWrite(L2,LOW); digitalWrite(H2,LOW); analogWrite(H1,value); DFlag = 1; break; case 2: if(DFlag =! 2) {dedTime(30); } /*flag check*/ /*dedtime*/ digitalWrite(L1,LOW); digitalWrite(L2,HIGH); digitalWrite(H1,LOW); analogWrite(H2,value); DFlag = 2; break; case 3: if(DFlag =! 3) {dedTime(30); } /*flag check*/ /*dedtime*/ digitalWrite(L1,HIGH); digitalWrite(L2,HIGH); digitalWrite(H2,HIGH); analogWrite(H1,value); DFlag = 3; break; } } void dedTime(int t) { digitalWrite(L1,HIGH); digitalWrite(L2,HIGH); delay(t); return; } void WaySet( int iIn1Pin, int iIn2Pin, int iMotor ){ int iMotor2 = iMotor; if(0 > iMotor) iMotor2 = iMotor * (-1); //制御量が負の場合,絶対値に直す
if( -5 < iMotor2 && 5 > iMotor2 ){ //モータ停止
motorDrive(0,255); // モータドライバへの書き込み
}
else if( 0 < iMotor ){ //モータ正回転 motorDrive(1,iMotor2); // モータドライバへの書き込み } else{ //モータ逆回転 motorDrive(2,iMotor2); // モータドライバへの書き込み } return; } void loop() { int iPress1 = analogRead(PRESSPIN1); //感圧センサ① int iPress2 = analogRead(PRESSPIN2); //感圧センサ② int iLimit1 = digitalRead(LIMIT1); //リミットスイッチ① int iLimit2 = digitalRead(LIMIT2); //リミットスイッチ② int iSwitch = digitalRead(SWITCHPIN); //握りセンサ int iMotor = Map(iPress1, iPress2, iLimit1, iLimit2, iSwitch, -1023, 1023, -255, 255, true); //制御量の決定 WaySet( IN1PIN, IN2PIN, iMotor ); //モータへ出力 // ----------シリアルモニタデバッグ---------- //(プロセッシングとの通信を行うときはコメントアウトする) // // 2個の感圧センサの差分 // Serial.print( "ival_filter : " ); // Serial.print(ival_filter); // Serial.print( ", " ); // // モータ制御量の出力 // Serial.print( "iMotor : " ); // Serial.print(iMotor); // Serial.print( ", " ); // // 感圧センサ値の出力 // Serial.print( "iPr1 : " ); // Serial.print(iPress1); // Serial.print( ", " ); // Serial.print( "iPr2 : " ); // Serial.print(iPress2); // Serial.print( ", " ); // // リミットスイッチの状態 // Serial.print( "Li1 : " ); // Serial.print(iLimit1); // Serial.print( ", " ); // Serial.print( "Li2 : " ); // Serial.print(iLimit2); // Serial.print( ", " ); // // 握りセンサの状態 // Serial.print( "Sw : " ); // Serial.print(iSwitch); // Serial.print( "\n" ); //---------------------------------- // ----------Processingとの送受信---------- if( Serial.available() > 0 ){
Serial.write((int)(((double)(iPress1 – iPress2)/1023 + 1)*50)); //0 ~ 100の範囲で送る
Serial.write((int)(((double)(ival_filter)/1023 + 1)*50)); //0 ~ 100の範囲で送る
Serial.write(((double)iMotor/255 + 1)*50); //0 ~ 100の範囲で送る
Serial.read(); //合図用データを読み込みバッファを空にする
}
// —————————————-
delay(10);
}

Prosessing

// センサ値とモータ指令値よりアシスト率を表示するプログラム
// 2012.12.28 shiroku
//シリアルライブラリを取り入れる
import processing.serial.*;
//myPort(任意名)というインスタンスを用意
Serial myPort;
int x=0;
int y=0;
int z=0;
PFont fontA;
int GMulti = 2;
int[] valsA = new int[1024];
int[] valsB = new int[1024];
int[] valsC = new int[1024];
int valsD = 0;
float filterX;
void setup(){
//画面サイズ
//size(512, 512);
size(1024, 512);
//フォント
PFont fontA = loadFont(“MS-Gothic-24.vlw”);
textFont(fontA, 24);
textAlign(CENTER);
//シリアルポートの設定
myPort=new Serial(this,”COM1″,9600);
}
void draw(){
//画面高さ
int drawheight = 255 * GMulti;
//背景色
background(51);
//テキスト描画
//fill(100, 100, 100);
fill(255, 255, 255);
text(valsB[1023], 240, 45);
text(valsC[1023], 240, 75);
text(valsD, 120, 170);
text(“[%] センサ取得値”, 360, 45);
text(“[%] モータ指令値”, 360, 75);
text(“モータ指令値”, 120, 140);
if(valsD != 0){
//センサkg換算式 2012.12.28 experimented
double valsB_kg = 0;
if(0 < valsB[1023]) { //正の場合 valsB_kg = 0.1 * (double)valsB[1023] - 5; if(60 > valsB[1023]) valsB_kg = 1.0;
}
else{ //負の場合
valsB_kg = 0.1 * (double)valsB[1023] + 5;
if(-60 < valsB[1023]) valsB_kg = -1.0; } //モータkg換算式 2012.12.28 experimented double valsC_kg = 0; if(0 < valsC[1023]) { //正の場合 valsC_kg = 0.1929 * (double)valsC[1023] - 2.381; if(20 > valsC[1023]) valsC_kg = 1.0;
}
else{ //負の場合
valsC_kg = 0.1929 * (double)valsC[1023] + 2.381;
if(-20 < valsC[1023]) valsC_kg = -1.0; } //アシスト率の計算 double Assist = valsC_kg / valsB_kg *100 - 100; text((int)Assist, 230, 450); text("% !!!ASSIST!!!", 350, 450); } //バー,円の描画 fill(255, 255, 255); stroke(100, 100, 100); line(120, 30, 120, 70); stroke(204, 255, 0); line(120, 35, valsB[1023]+120, 35); ellipse(valsB[1023]+120, 35, 10, 10); stroke(0, 255, 255); line(120, 65, valsC[1023]+120, 65); ellipse(valsC[1023]+120, 65, 10, 10); // 目盛り描画 stroke(100); for( int i = 5; i < drawheight; i += 10 ) line(width-5, i, width-10, i); for( int i = 55; i < drawheight-50; i += 100 ) line(width-5, i, width-20, i); // グラフ描画 stroke(150, 255, 0); for( int i = 0; i < 1023; i++ ) line(i, drawheight/2 - valsA[i], (i + 1), drawheight/2 - valsA[i + 1]); stroke(255, 255, 0); for( int i = 0; i < 1023; i++ ) line(i, drawheight/2 - valsB[i], (i + 1), drawheight/2 - valsB[i + 1]); stroke(0, 255, 255); for( int i = 0; i < 1023; i++ ) line(i, drawheight/2 - valsC[i], (i + 1), drawheight/2 - valsC[i + 1]); } //シリアル通信処理 void serialEvent(Serial p){ if(myPort.available()>2){ //Arduinoから送られてきたデータが3個(2より多い)の場合
x=myPort.read();
y=myPort.read();
z=myPort.read();
// グラフ用データ生成
for( int i = 0; i < 1023; i++ ){ valsA[i] = valsA[i + 1]; valsB[i] = valsB[i + 1]; valsC[i] = valsC[i + 1]; } valsA[1023] = (int)(((double)x/50-1)*100); //-100 ~ 100の範囲にする valsB[1023] = (int)(((double)y/50-1)*100); //-100 ~ 100の範囲にする valsC[1023] = (int)(((double)z/50-1)*100); //-100 ~ 100の範囲にする valsD= (int)(((double)z/50 - 1)*255); //-255 ~ 255の範囲にする myPort.write(65); //読み込み後、合図データ送信 } } //マウスが押されたら通信開始とする void mousePressed(){ myPort.clear(); myPort.write(65); }