DSC02206.JPG

昨日より扱いやすくなりました.

DSC02213.JPG

ハンドル

DSC02214.JPG

ブレッドボード
改良点
・感圧センサを変更.
 フォースレンジが大きいものに変更.より力を加えないと動作できないようにした.
・感圧センサを上面と下面に配置.
 昨日は下面だけ.
・ハンドルの取り付け
 力の伝達がしやすい.安全用のスイッチを取り付け.手を離したことを認識できるように.
以上の改良で,デューティー比100%でテストできました.
動作テスト(負荷:コンプレッサー)

ばねばかりでの計測1

ばねばかりでの計測2

Arduino

// 感圧センサによりDCモータを制御するプログラム
// リミットスイッチ×2,握りスイッチ×1,感圧センサ×2,DCモータ×1
// 2012.12.27 shiroku
#define IN1PIN (9) //モータドライバ用
#define IN2PIN (10) //モータドライバ用
#define PRESSPIN1 (2) //感圧センサ①
#define PRESSPIN2 (3) //感圧センサ②
#define LIMIT1 (18) //リミットスイッチ①
#define LIMIT2 (19) //リミットスイッチ②
#define SWITCHPIN (7) //握りセンサ
int i;//pwmValue
int f;//dedtimeShiftFlag
int pwmPin;
int H1 = 10;
int H2 = 9;
int L1 = 14;
int L2 = 15;
void pwmPinSet(int, int, int);
void dedTime(int);
void setup()
{
Serial.begin(9600);
// pinMode(H1,OUTPUT);
// pinMode(H2,OUTPUT);
pinMode(L1,OUTPUT);
pinMode(L2,OUTPUT);
pwmPinSet(1,0,2);
}
int Map( double dPr1, double dPr2, int iLimit1, int iLimit2, int iSwitch, double iIn1, double iIn2, int iOut1, int iOut2, boolean bConstrain = false )
{
double dForce = dPr1 – dPr2;
//2個の感圧センサの差分
Serial.print( “dForce : ” );
Serial.print(dForce);
Serial.print( “, ” );
//感圧センサ不感帯
if(dForce > -100 && dForce < 100) dForce = 0; //フィードフォワード制御 double dValue = (double)(dForce - iIn1) * (iOut2 - iOut1) / (iIn2 - iIn1) + iOut1; 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 pwmPinSet(int L1V, int L2V,int pwmPinV) //L1,L2,H1,H2がそれぞれのピン番号でグローバル宣言ある必要あり. //pwmPinがグローバル宣言してある必要有り. //L1V,L2Vはそれぞれのピンの状態指定(L(0)/H(1)). //pwmPinSelectorで指定されたFET番号をH1/H2にセット,逆をLOWにセット. //pwmPinSelectorで1/2以外が指定された場合は1/2をanalgWriteで0に指定. { if(1 == L1V){ digitalWrite(L1,HIGH); }else{ digitalWrite(L1,LOW); } if(1 == L2V){ digitalWrite(L2,HIGH); }else{ digitalWrite(L2,LOW); } switch(pwmPinV) { case 1: analogWrite(H2,LOW); pwmPin = H1; break; case 2: analogWrite(H1,LOW); pwmPin = H2; break; default: analogWrite(H1,0); analogWrite(H2,0); break; } // Serial.print( "L1V : " ); // Serial.print(L1V); // Serial.print( ", " ); // Serial.print( "L2V : " ); // Serial.print(L2V); // Serial.print( ", " ); return; } void dedTime(int t) { Serial.print( "!!dedTime!!" ); digitalWrite(L1,HIGH); digitalWrite(L2,HIGH); digitalWrite(H1,LOW); digitalWrite(H2,LOW); delay(t); return; } void MotorDrive( int iIn1Pin, int iIn2Pin, int iMotor ){ if( -5 < iMotor && 5 > iMotor ){ //モータ停止
// digitalWrite(L1,HIGH);
// digitalWrite(L2,HIGH);
//モータブレーキ
if(f != 3) dedTime(10);
digitalWrite(L1,LOW);
digitalWrite(L2,LOW);
f = 3;
}
else if( 0 < iMotor ){ //モータ正回転 if(f != 1) dedTime(10); pwmPinSet(1,0,2); f = 1; } else{ //モータ逆回転 if(f != 2) dedTime(10); pwmPinSet(0,1,1); f = 2; } // モータ制御量の出力 Serial.print( "iMotor : " ); Serial.print(iMotor); Serial.print( ", " ); if(0 > iMotor) iMotor = iMotor * (-1); //制御量が負の場合,絶対値に直す
analogWrite(pwmPin,iMotor); //pwmPinSet関数によりpwmPinを決定.iは0~255.
// ドライバへ書き込んだ変数の確認
Serial.print( “pwmPin : ” );
Serial.print(pwmPin);
Serial.print( “, ” );
Serial.print( “f : ” );
Serial.print(f);
Serial.print( “, ” );
Serial.print( “i : ” );
Serial.print(iMotor);
Serial.print( “, ” );
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, -900, 900, -255, 255, true);
MotorDrive( IN1PIN, IN2PIN, iMotor ); //モータへ出力
// 感圧センサ値の出力
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” );
delay(10);
}