テーブルから落ちない自走ロボットのためのスケッチ：Toda Yorozu Kenkyujyo(TYK)

机から落ちない自走ロボットのスケッチです。
/*
 This sketch is for the automation clenaer of Toda Yorozu Kenkyujyo.
 */

#include < SoftwareSerial.h > 

//コネクタの接続。タイヤ制御は左右それぞれディジタル2,アナログ1出力が必要。
// left tire
#define LdirectionA 2 // 1st bit of direction of the left tire// INPUT1
#define LdirectionB 3 // 2nd bit of direction of the left tire// INPUT2
#define Lspeed 9  // Speed of left tire, analog write only 5,6,9,10,11 //Ain

// right tire
#define RdirectionA 4 // 1st bit of direction of the right tire//INPUT1
#define RdirectionB 5 // 2nd bit of direction of the right tire//INPUT2
#define Rspeed 10 // Speed of left tire, analog write only 5,6,9,10,11//Ain

// sensors  センサー用の入力analog inが3つ。
#define Lsensor 0 // sensor for tire analog in
#define Rsensor 1 // sensor for tire analog in
#define Fsensor 2 // sensor for tire /analog in

// PORTD=B00000; stop  //停止
// PORTD=B00010; turn right  //右展開(左タイヤのみon)
// PORTD=B01000; turn left  // 左展開(右タイヤのみon)
// PORTD=B01010; go forward //前進(左右のタイヤをon)
// PORTD=B10100; go back  //後退(左右の対ぎゃを逆転)

unsigned int Lrpm=250;  //0 to 255 左タイヤ回転速度
unsigned int Rrpm=250; //0 to 255 右タイヤ回転速度
unsigned long time1=1; //計測時間
unsigned long dt=50; //dt=0.1 msec  //制御の単位時間
unsigned int Ltire=1023;  //左タイヤのセンサー  
unsigned int Rtire=1023;  //右タイヤのセンサー
unsigned int FrontSens=1023;  //前センサー
unsigned int NoG=200;  //Not on the ground=センサー出力が200以下になった時に床が無いと判断する。
unsigned int DirectRandom=49;  //ランダム回転用の係数1
unsigned int DirectRandom2=49; //ランダム回転用の係数2
unsigned int TurnNo=0; // 左右のターンを何回繰り返したかを示す。

void Scan(){ //check the ground and decide the motion
Ltire=analogRead(Lsensor);  //左タイヤセンサーの値を読み取る
Rtire=analogRead(Rsensor); //右タイヤセンサーの値を読み取る
FrontSens=analogRead(Fsensor); //前センサーの値を読み取る

if (Ltire<NoG &&  Rtire<NoG &&  FrontSens<NoG){Handling();} //全てのセンサーがoffの時には、handling中としてタイヤを停止させる。
else{
if (Ltire<NoG &&  Rtire<NoG){ChangeToStraight();} //後ろのタイヤが二つともoffの時には前進。
if (Ltire<NoG &&  Rtire>NoG){ChangeToTurnRight();} //左のタイヤがoffの時には右に回転。
if (Ltire>NoG &&  Rtire<NoG){ChangeToTurnLeft();} //右のタイヤがoffの時には左に回転。
if (Ltire>NoG &&  Rtire>NoG &&  FrontSens<NoG){ChangeToBack();} //前センサーがoffの時には後退する。
}
}
void ChangeToStraight(){PORTD=B000000; delay(5); PORTD=B101000;delay(dt);TurnNo=0;}  //前進
void ChangeToTurnRight(){PORTD=B000000; delay(5); PORTD=B011000;delay(dt);TurnNo++;} //右回転(右タイヤを逆方向、左タイヤを順方向に回転させる)
void ChangeToTurnLeft(){PORTD=B000000; delay(5); PORTD=B100100;delay(dt);TurnNo++;} //左回転(左タイヤを逆方向、右タイヤを順方向に回転させる)
void ChangeToBack(){PORTD=B000000; delay(5); PORTD=B0101000;delay(dt);TurnNo=0;} //後退
void Handling(){PORTD=B000000;delay(2000);}  //しばらく停止。

//試験用のサブルーチン：dtの間だけ前進。
void GoStraight(){  //go straight for 0.1 sec
PORTD=B000000; delay (5);
time1=millis();
analogWrite(Lspeed,Lrpm);
analogWrite(Rspeed,Rrpm);
PORTD=B010100;  // PD1,PD3 =high
  while ((millis()-time1) < dt){delay(10);}
}

//試験用のサブルーチン：dtの間だけ右回転。
void TurnRight(){
PORTD=B000000; delay (5);
   time1=millis();
analogWrite(Lspeed,Lrpm);
analogWrite(Rspeed,Rrpm);
PORTD=B011000;  // PD1 =high
    while ((millis()-time1)<dt){delay(10);}
}

//試験用のサブルーチン：dtの間だけ左回転。
void TurnLeft(){
PORTD=B000000; delay (5);
   time1=millis();
analogWrite(Lspeed,Lrpm);
analogWrite(Rspeed,Rrpm);
PORTD=B100100;  // PD1 =high
    while ((millis()-time1)<dt){delay(10);}
}

//試験用のサブルーチン：dtの間だけ後退。
void GoBack(){
PORTD=B000000; delay (5);
 time1=millis();
analogWrite(Lspeed,Lrpm); 
analogWrite(Rspeed,Rrpm);
PORTD=B101000;  // PD1,PD3 =high
 while ((millis()-time1)<dt){delay(10);}
}

//試験用のサブルーチン：dtの間だけ停止。
void Freezing(){PORTD=B000000; delay(dt);}  // PD1,PD3 =high

//試験用のサブルーチン：dtの間だけブレーキ。
void Breaking(){
PORTD=B000000; delay (10);
 time1=millis();
  PORTD=B111100;
 while ((millis()-time1)<dt){delay(10);}
}  // PD1,PD3 =high


//試験用のサブルーチン：前進、停止、後退、ブレーキ、停止、右回転、左回転をテスト。
void TestRun(){
     Serial.println("straight");
 for (int i=0; i<5; i++){GoStraight();}
 Serial.println("freezing");
 for (int i=0; i<5; i++){Freezing();}
  Serial.println("go back");
  for (int i=0; i<5; i++){GoBack();}
 Serial.println("breaking");
  for (int i=0; i<5; i++){Breaking();}
  Serial.println("freezing");
  for (int i=0; i<5; i++){Freezing();}
  Serial.println("turn right");
  for (int i=0; i<5; i++){TurnRight();}
  Serial.println("turn left");
  for (int i=0; i<5; i++){TurnLeft();}
}

//セットアップ
void setup(){
  Serial.begin(9600);  //試験用のシリアル通信
pinMode(LdirectionA, OUTPUT); //LdirectionA=2番端子をアウトプットに設定
pinMode(LdirectionB, OUTPUT); //LdirectionB=3番端子をアウトプットに設定
pinMode(RdirectionA, OUTPUT); //RdirectionA=4番端子をアウトプットに設定
pinMode(RdirectionB, OUTPUT); //RdirectionA=5番端子をアウトプットに設定
pinMode(Lspeed, OUTPUT); //Lspeed=9番端子をアウトプットに設定
pinMode(Rspeed, OUTPUT); //Rspeed=10番端子をアウトプットに設定
DDRD=DDRD|B11111100;  //digital out D端子の2-7番を使用。
  TestRun();  //上記test runで動作確認。
  TurnNo=0;  //turn noを元に戻す。
  }
  
//メインプログラム
void loop(){
  Scan(); //センサーでスキャンして、進む方向を決定。
  //以下は乱数で回転運動を決める。この回転の途中でスキャンをして危険なら方向を変える。
  DirectRandom=random(100); 
  DirectRandom2=random(50000);
  if(DirectRandom2<5){
  if ((DirectRandom%2)==0){for (int i=0; i<(DirectRandom/70); i++){TurnRight(); Scan();}}
  else{for (int i=0; i<(DirectRandom/70); i++){TurnLeft(); Scan();}}
  }
  if(TurnNo>5){PORTD=B000000; delay(5); PORTD=B010100; delay(dt); TurnNo=0;} //5回左右に連続してターンしたら後ろが机の端に近いと判断して前進を始める。
}  ///void loop end
戸田よろず研究所トップページへ