2012年7月8日日曜日

Motor Shield と PSDセンサで差動二輪

 以前に試した Motor Shield と この前PSD距離センサを接続するために作った センサコネクタシールドArduino Uno にスタックして、差動二輪のお試しをしてみました。


 とりあえず、動かしてみたかったので、配線とかUSBケーブルとかそのままねじりっこで束ねて、かなり酷い外観になっていますが、まあ、そのあたりは追々改善するということで・・・


 で、とりあえず、走らせてみる。


 適当すぎて、役には立たないだろうけど、一応、Arduinoのソース(スケッチ)を載せてみます。



#include <Arduino.h>

/**
* Motor & PSD センサのテストスケッチ。
*/

#define DBG_MODE 1

// PSDセンサのアナログ入力ピン設定。
#define PIN_PSD_L 4
#define PIN_PSD_R 5

// Motor Shield Rev.3 のピン設定。
#define PIN_CH1_PWM  3
#define PIN_CH1_DIR  12
#define PIN_CH1_BRK  9
#define PIN_CH2_PWM  11
#define PIN_CH2_DIR  13
#define PIN_CH2_BRK  8

// 各モータの配置・配線で正逆転を適宜変更。
#define CH1_CW  HIGH
#define CH1_CCW  LOW
#define CH2_CW  LOW
#define CH2_CCW  HIGH

#define DIR_CW  0
#define DIR_CCW  1

#define MOTOR_LEFT 1
#define MOTOR_RIGHT 2

// PSDセンサの近接側測定不可領域閾値。
const int PSD_DEADZONE = 80;

// 通信終端文字コード定義。
static char CHAR_CR = 0x0d;
static char CHAR_LF = 0x0a;

// グローバル変数定義。
char rcvChar;
char rcvBuf[64];
int rcvCnt;
boolean isRcvCmd;
int spd1, spd2;
int rngL, rngR;
char *tmpc1, *tmpc2;
int tmpi1, tmpi2;
String dbg_msg = "";
int nearL, nearR;
int verynearL, verynearR;

// PSDセンサの電圧をcmに変換。
int analogToCentimeter(int analogValue) {
 return (int)((6787 / (analogValue -3)) -4 );
}



//  回転方向(出力ピン)の設定。
//    ch  -  設定チャネル 1 or 2
//    dir  -  回転方向 DIR_CWで正転、DIR_CCWで逆転
//  戻り値  なし。
void setDirection(int ch, int dir) {
 if(ch == 1) {
  if(dir == DIR_CW) {
   digitalWrite(PIN_CH1_DIR, CH1_CW);
  } else if (dir == DIR_CCW) {
   digitalWrite(PIN_CH1_DIR, CH1_CCW);
  }
 } else if (ch == 2) {
  if (dir == DIR_CW) {
   digitalWrite(PIN_CH2_DIR, CH2_CW);
  } else if (dir == DIR_CCW) {
   digitalWrite(PIN_CH2_DIR, CH2_CCW);
  }
 }
}

//  各チャネルのモータ出力(スピード)設定。
//    ch  -  設定チャネル 1 or 2
//    spd  -  設定出力。0~255
//  戻り値  なし。
boolean setMotorSpeed(int ch, int spd) {
 if ((ch != 1) && (ch != 2))  return false;
 if (spd > 255)  spd = 255;
 if (spd < -255)  spd = -255;
 
 if (spd < 0) {
  spd = -1 * spd;
  setDirection(ch, DIR_CCW);
 }
 else {
  setDirection(ch, DIR_CW);
 }
 
 if (ch == 1) {
  analogWrite(PIN_CH1_PWM, spd);
 }
 else if (ch == 2) {
  analogWrite(PIN_CH2_PWM, spd);
 }
 
 return true;
}

//  モータ出力 ch1,2設定。
//    ch1spd  -  ch1の出力。0~255
//    ch2spd  -  ch2の出力。0~255
//  戻り値  なし。
void setMotorSpeed2(int ch1spd, int ch2spd) {
 setMotorSpeed(1, ch1spd);
 setMotorSpeed(2, ch2spd);
}

//  モータ設定の初期化。
//  引数  なし。
//  戻り値  なし。
void initMotor() {
 pinMode(PIN_CH1_PWM, OUTPUT);
 pinMode(PIN_CH1_DIR, OUTPUT);
 pinMode(PIN_CH1_BRK, OUTPUT);
 pinMode(PIN_CH2_PWM, OUTPUT);
 pinMode(PIN_CH2_DIR, OUTPUT);
 pinMode(PIN_CH2_BRK, OUTPUT);
 
 setMotorSpeed2(0, 0);
 
 digitalWrite(PIN_CH1_BRK, LOW);
 digitalWrite(PIN_CH2_BRK, LOW);
}

void breakMotor() {
 digitalWrite(PIN_CH1_BRK, HIGH);
 digitalWrite(PIN_CH2_BRK, HIGH);
 spd1 = 0;
 spd2 = 0;
 setMotorSpeed2(spd1, spd2);
 delay(200);
 digitalWrite(PIN_CH1_BRK, LOW);
 digitalWrite(PIN_CH2_BRK, LOW);
}

// 初期設定。
void setup() {
 // シリアル通信初期化。
 Serial.begin(115200);
 if (DBG_MODE) {
  Serial.println("Hello, Motor and PSD Test program...");
 }
 
 // モーター初期化。
 initMotor();
 
 // 変数初期化。
 spd1 = 0;
 spd2 = 0;
 rngL = 0;
 rngR = 0;
 nearL = 0;
 verynearL = 0;
 nearR = 0;
 verynearR = 0;
}

// メインループ。
void loop() {
 
 if (DBG_MODE) {
  dbg_msg = "";
 }
 
 // PSDセンサ読み取り処理。
 int value = analogRead(PIN_PSD_L);
 // 読み取り値が閾値を超えていれば、距離に変換。
 if (value > PSD_DEADZONE) {
  rngL = analogToCentimeter(value);
 } else {
  // しきい値に達しなければ、とりあえず0。
  rngL = 0;
 }
 // もう一つのPSDも同様。
 value = analogRead(PIN_PSD_R);
 if (value > PSD_DEADZONE) {
  rngR = analogToCentimeter(value);
 } else {
  rngR = 0;
 }
 
 // 速度制限。
 if ((30 >= rngL) && (rngL > 15)) {
  /// 距離30~15で
  nearL++;
  verynearL = 0;
 } else if ((15 >= rngL) && (rngL > 0)) {
  /// 距離15以下。ただし0(測定不能かも)は除く
  verynearL++;
 } else {
  /// それ以外はとりあえず制限なし。
  nearL = 0;
  verynearL = 0;
 }
 if ((30 >= rngR) && (rngR > 15)) {
  /// 距離30~15で
  nearR++;
  verynearR = 0;
 } else if ((15 >= rngL) && (rngL > 0)) {
  /// 距離15以下。ただし0(測定不能かも)は除く
  verynearR++;
 } else {
  /// それ以外はとりあえず制限なし。
  nearR = 0;
  verynearR = 0;
 }
 
 // 車輪スピード設定。
 setMotorSpeed2(spd1, spd2);
 if (verynearL > 2) {
  /// 左側停止。
  setMotorSpeed(MOTOR_LEFT, 0);
 } 
 if (verynearR > 2) {
  /// 右側停止。
  setMotorSpeed(MOTOR_RIGHT, 0);
 }
 if (nearL > 0) {
  /// 左側が近接なら、右へ旋回。
  setMotorSpeed(MOTOR_RIGHT, (int)(spd2/2));
  nearL= 0;
 } 
 if (nearR > 0) {
  /// 右側が近接なら、左へ旋回。
  setMotorSpeed(MOTOR_LEFT, (int)(spd1/2));
  nearR = 0;
 }
 /*
 if (DBG_MODE) {
  Serial.print(rngL, DEC);
  Serial.print(", ");
  Serial.println(rngR, DEC);
 }
 */
 
 // シリアル受信処理。受信バッファにデータがあれば、
 if (Serial.available() > 0) {
  /// 1文字読み込み。
  rcvChar = Serial.read();
  /// 受信文字が改行文字の場合は、
  if ((rcvChar == CHAR_CR) || (rcvChar == CHAR_LF)) {
   //// 既に1文字以上受信していれば、
   if (rcvCnt > 0){
    ///// 文字列終端を設定して、コマンド受信フラグ立てる。
    rcvBuf[rcvCnt] = '\0';
    isRcvCmd = true;
    rcvCnt = 0;
   }
  }
  /// 受信文字が改行文字以外の場合は、
  else {
   //// 配列に順次取り込み。
   rcvBuf[rcvCnt] = rcvChar;
   rcvCnt++;
  }
 }    // シリアル受信処理、ここまで。
 
 // コマンド解析処理。
 if (isRcvCmd == true) {
  /// 受信フラグクリア。
  isRcvCmd = false;
  /// 1文字目で処理振り分け。
  switch (rcvBuf[0]) {
   case 'D':
   case 'd':
   //// モータスピード指定。
   tmpc1 = strtok(rcvBuf, ",");
   tmpc1 = strtok(NULL, ",");
   tmpc2 = strtok(NULL, ",");
   if ((tmpc1 == NULL) || (tmpc2 == NULL))  break;
   tmpi1 = atoi(tmpc1);
   tmpi2 = atoi(tmpc2);
   if ((tmpi1 < -255) || (tmpi1 > 255)) break;
   if ((tmpi2 < -255) || (tmpi2 > 255)) break;
   spd1 = tmpi1;
   spd2 = tmpi2;
   if (DBG_MODE) {
    Serial.print("D command : ");
    Serial.print(spd1, DEC);
    Serial.print(", ");
    Serial.println(spd2, DEC);
   }
   break;
   case 'B':
   case 'b':
   //// ブレーキ。
   breakMotor();
   if (DBG_MODE) {
    Serial.println("Break");
   }
   break;
   case '?':
   //// デバッグ情報取得。
   if (DBG_MODE) {
    dbg_msg += "spd1 = ";
    dbg_msg += spd1;
    dbg_msg += ", ";
    dbg_msg += "spd2 = ";
    dbg_msg += spd2;
    dbg_msg += ", ";
    dbg_msg += "PSD Left = " ;
    dbg_msg += rngL;
    dbg_msg += ", ";
    dbg_msg += "PSD Right = ";
    dbg_msg += rngR;
    dbg_msg += ", ";
    Serial.println(dbg_msg);
   }
   break;
   default:
   //// 該当なし。
   break;
  }
 }    // コマンド解析処理、ここまで。
 
}