DSC02041.JPG

モータのトルク制御は抑えて置きたいのでやってみました.
今回はProsessingにより,目標値と計測値をリアルタイムで描画することにもチャレンジしてみました.


手順
①ブレッドボード上のポテーショメータにより目標電流値を与える.
Groveの電流センサによりモータの電流値を取得.(これを計測電流値とする.)
③目標電流値と計測電流値の差分から,モータのPWM制御量を決定.
④目標電流値,計測電流値,モータ制御量をProsessingによりグラフで描画.
使用している電流センサのレンジが片側しかないため,
モータの正転,逆転時の電流センサの使い分けに苦労しました.
Prosessing での描画

Arduino_20121103.JPG

動作中,右から左にデータが流れていきます.
-0.3~0.3[A]くらいの範囲であれば,だいたい追従してる感じがしました.
以下,ソースコード.
Arduino

// 「可変抵抗器で目標電流値を与えて,電流センサとの差分により,DCモータをPWM制御するプログラム」
// 2012.11.03 shiroku
// PWMの周期を1/8にするための準備
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
#define IN1PIN (9)
#define IN2PIN (10)
#define VOLUMEPIN (0)
void setup(){
pinMode(IN1PIN, OUTPUT);
pinMode(IN2PIN, OUTPUT);
Serial.begin(115200);
//Serial.begin(9600);
// PWMの周期を1/8にする処理
cbi(TCCR0B, CS02);
cbi(TCCR0B, CS01);
sbi(TCCR0B, CS00);
delay(100);
}
int CurrentMonitor(int iMotor){
static double val = 0.0;
double val2 = 0.0;
static int zeroflag = 0;
static int ivalFinal = 0;
static int sensorpin = 1;
int v = analogRead(sensorpin);
static double val_filter = 0.0;
static int ival_filter = 0;
const float K_filter = 0.05; //フィルターのゲイン
// パルス波の処理
if(v != 0){ //PWM制御なので,電流値が瞬間的にゼロのものは計測に含めない
val = (double)v/1024*5/800*2000000; // [mA]へ変換 Grove – Electricity Sensor
zeroflag = 0; //電流値のゼロフラッグをリセット
}
else{ //電流値がゼロであるときが続いたら,計測値をゼロとみなす
if(zeroflag >= 100){
val = 0;
zeroflag = 0;
}
zeroflag ++; //電流値のゼロフラッグをカウント
}
// 電流センサの使い分け
if(iMotor >= 0){
sensorpin = 1;
val2 = val; //電流センサの値は正のまま
}
else{
sensorpin = 2;
val2 = -val; //電流センサの値を負にする
}
//フィルタ
val_filter += (val2 – val_filter) * K_filter;
//整数への変換
ival_filter = (0 < val_filter) ? (int)(val_filter + 0.5) : (int)(val_filter - 0.5); //四捨五入してint型に型変換 return ival_filter; } int Map( int iIn, int Current, int iIn1, int iIn2, int iOut1, int iOut2, boolean bConstrain = false ){ //フィードフォワード制御 //double dValue = (double)(iIn - iIn1) * (iOut2 - iOut1) / (iIn2 - iIn1) + iOut1; //フィードバック制御(P制御) //制御式概要:iInとCurrentの範囲を「-1~1」にし,dValueの上限と下限に合わせて,比例ゲインKpをかけた const double Kp = 6.0; //比例ゲイン double dValue = Kp * 255 * (((double)iIn/(1023)-0.5) * 2 - (double)Current/(600)); 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( 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 iIn1Pin, int iIn2Pin, int iMotor ){ if( -5 < iMotor && 5 > iMotor ){
//digitalWrite(iIn1Pin, HIGH);
//digitalWrite(iIn2Pin, HIGH);
digitalWrite(iIn1Pin, LOW);
digitalWrite(iIn2Pin, LOW);
}
else if( 0 < iMotor ){ analogWrite(iIn1Pin, iMotor); analogWrite(iIn2Pin, 0); } else{ analogWrite(iIn1Pin, 0); analogWrite(iIn2Pin, -iMotor); } } void loop(){ static int iMotor_old = 0; int iValue = analogRead(VOLUMEPIN); int iCurrent = CurrentMonitor(iMotor_old); //電流値のモニタリング int iMotor = Map(iValue, iCurrent, 0, 1023, -255, 255, true); //PWM制御 MotorDrive( IN1PIN, IN2PIN, iMotor ); //モータへ出力 iMotor_old = iMotor; // Arduinoシリアルモニタでのデバッグ用 // Serial.print( "iValue : " ); // Serial.print(((double)iValue/1023-0.5)*2*100); // Serial.print( "," ); // Serial.print( "iCurrent : " ); // Serial.print(((double)iCurrent/600)*100); // Serial.print( "," ); // Serial.print( "iMotor : " ); // Serial.print(iMotor); // Serial.print( "\n" ); // Processingとの送受信 if( Serial.available() > 0 ){
Serial.write((int)(((double)iValue/1023)*100)); //0 ~ 100の範囲で送る
Serial.write((int)(((double)iCurrent/600 + 1)*50)); //0 ~ 100の範囲で送る
Serial.write(((double)iMotor/255 + 1)*50); //0 ~ 100の範囲で送る
Serial.read(); //合図用データを読み込みバッファを空にする
}
}

Prosessing

// 「可変抵抗器で目標電流値を与えて,電流センサとの差分により,DCモータをPWM制御するプログラム」
// 2012.11.03 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[512];
int[] valsB = new int[512];
int valsC = 0;
float filterX;
void setup(){
//画面サイズ
size(512, 512);
//フォント
PFont fontA = loadFont(“MS-Gothic-24.vlw”);
textFont(fontA, 24);
textAlign(CENTER);
//シリアルポートの設定
//myPort=new Serial(this,”COM1″,9600);
myPort=new Serial(this,”COM1″,115200);
}
void draw(){
//画面高さ
int drawheight = 255 * GMulti;
//背景色
background(255);
//テキスト描画
fill(100, 100, 100);
text(valsA[511], 240, 45);
text(valsB[511], 240, 75);
text(valsC, 120, 170);
text(“×10[mA] 目標電流”, 360, 45);
text(“×10[mA] 計測電流”, 360, 75);
text(“モータ指令値”, 120, 140);
//バー,円の描画
fill(255, 255, 255);
stroke(100, 100, 100);
line(120, 30, 120, 70);
stroke(255, 0, 0);
line(120, 35, valsA[511]+120, 35);
ellipse(valsA[511]+120, 35, 10, 10);
stroke(0, 0, 255);
line(120, 65, valsB[511]+120, 65);
ellipse(valsB[511]+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(255, 0, 0); for( int i = 0; i < 511; i++ ) line(i, drawheight/2 - valsA[i], (i + 1), drawheight/2 - valsA[i + 1]); stroke(0, 0, 255); for( int i = 0; i < 511; i++ ) line(i, drawheight/2 - valsB[i], (i + 1), drawheight/2 - valsB[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 < 511; i++ ){ valsA[i] = valsA[i + 1]; valsB[i] = valsB[i + 1]; } valsA[511] = (int)(((double)x/50-1)*60); //-60 ~ 60の範囲にする valsB[511] = (int)(((double)y/50-1)*60); //-60 ~ 60の範囲にする valsC = (int)(((double)z/50 - 1)*255); //-255 ~ 255の範囲にする myPort.write(65); //読み込み後、合図データ送信 } } //マウスが押されたら通信開始とする void mousePressed(){ myPort.clear(); myPort.write(65); }

参考にしたサイト
Arduino PWM考 – 糸井@小林研究室
Arduino とレゴで倒立振子(2)モータドライバの設計 – Ichiro Maruta Homepage
Arduino – Arduino Sound Sensor Shield: SatE-O
建築発明工作ゼミ2008: Arduino-Processing シリアル通信1
なんでも作っちゃう、かも。 Arduino 1.0 リリース