とりあえず、動かしてみたかったので、配線とか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;
}
} // コマンド解析処理、ここまで。
}
0 件のコメント:
コメントを投稿