ジャパンロボテック社のRDS-X13の動作制御 （その１）

黒い枠のフィールド内に留まってロボットが動くプログラム

１．フィールド仕様

　　　サイズ：　A３×４枚（縦７９㎝　横５４㎝)

　　　黒線の厚さ：　１．５㎝

　　　外周の厚さ（外周から黒線までの幅）：　３㎝

２．動作

ロボットには２つの（モータ・ギア・車輪）が付いており、マイコンボードから独立に制御できます。 モータの回転方向を制御することで、ロボットは前進・後退・右回転・左回転が可能です。モーターのスピートはモーターがDCブラシモーターであることから制御できません。

ロボットの底面には、赤外線ラインセンサーが２つ付いています。この２つのラインセンサーのどちらかが黒線を検知すると、ロボットは３０ｃｍほどバックし、右に約９０度回転します。そしてまた、前進を開始します。

３．プログラムソース

//黒いサークル内だけを動くぷろぐらむ

// motor port mapping fro JRTA1 and RDC-102

#define PWM_PIN_A 6

#define MoterL_PIN_A 4

#define MoterR_PIN_A 5

#define PWM_PIN_B 9

#define MoterL_PIN_B 7

#define MoterR_PIN_B 8

#define Forward F

#define Back B

#define Left L

#define Right R

#define ON 1

#define OFF 0

#define LNS_R A1        // LNS_R Line Sensor R

#define LNS_L A2        // LNS_L Line Sensor L

int LNS_R_Value;  // Line Sensor Right Read Value

int LNS_L_Value;  //Line Sensor Left Read Value

 void ReadLineSensor(void){ 

  delay(5);

  LNS_R_Value = analogRead(LNS_R);

  delay(5);

  LNS_L_Value = analogRead(LNS_L); 

  delay(5);  

 }  

void Forward_Turn(long msec,int speed_value){

   digitalWrite(MoterL_PIN_A, OFF);

   digitalWrite(MoterR_PIN_A, ON); 

   digitalWrite(MoterL_PIN_B, OFF);

   digitalWrite(MoterR_PIN_B, ON); 

   analogWrite(PWM_PIN_A,speed_value*255/100);   

   analogWrite(PWM_PIN_B,speed_value*255/100);

   delay(msec);

   digitalWrite(PWM_PIN_A, OFF);

   digitalWrite(PWM_PIN_B, OFF);   

}  

void Back_Turn(long msec,int speed_value){

   digitalWrite(MoterL_PIN_A, ON);

   digitalWrite(MoterR_PIN_A, OFF); 

   digitalWrite(MoterL_PIN_B, ON);

   digitalWrite(MoterR_PIN_B, OFF); 

   digitalWrite(PWM_PIN_A, speed_value*255/100);

   digitalWrite(PWM_PIN_B, speed_value*255/100);

   delay(msec);

   digitalWrite(PWM_PIN_A, OFF);

   digitalWrite(PWM_PIN_B, OFF);   

}  

void Left_Turn(long msec,int speed_value){

   digitalWrite(MoterL_PIN_A, ON);

   digitalWrite(MoterR_PIN_A, OFF); 

   digitalWrite(MoterL_PIN_B, OFF);

   digitalWrite(MoterR_PIN_B, ON); 

   digitalWrite(PWM_PIN_A, speed_value*255/100);

   digitalWrite(PWM_PIN_B, speed_value*255/100);

   delay(msec);

   digitalWrite(PWM_PIN_A, OFF);

   digitalWrite(PWM_PIN_B, OFF);   

}

void Right_Turn(long msec,int speed_value){

   digitalWrite(MoterL_PIN_A, OFF);

   digitalWrite(MoterR_PIN_A, ON); 

   digitalWrite(MoterL_PIN_B, ON);

   digitalWrite(MoterR_PIN_B, OFF); 

   digitalWrite(PWM_PIN_A, speed_value*255/100);

   digitalWrite(PWM_PIN_B, speed_value*255/100);

   delay(msec);

   digitalWrite(PWM_PIN_A, OFF);

   digitalWrite(PWM_PIN_B, OFF);   

}

void Break_Turn(long msec){

   digitalWrite(MoterL_PIN_A, ON);

   digitalWrite(MoterR_PIN_A, ON); 

   digitalWrite(MoterL_PIN_B, ON);

   digitalWrite(MoterR_PIN_B, ON);  

   digitalWrite(PWM_PIN_A, ON);

   digitalWrite(PWM_PIN_B, ON);   

   delay(msec);

   digitalWrite(PWM_PIN_A, OFF);

   digitalWrite(PWM_PIN_B, OFF);   

}

void setup() {

  

  Serial.begin(9600);

  Serial.println("Start Line Sensor Test!");

  

     // set pin mode for motor

     pinMode(6, OUTPUT);

     pinMode(4, OUTPUT);

     pinMode(5, OUTPUT);

     pinMode(9, OUTPUT);

     pinMode(7, OUTPUT);

     pinMode(8, OUTPUT);

     digitalWrite(6, 0);

     

    delay(5000);

}

void loop() {

   ReadLineSensor();

   if(LNS_R_Value < 200 || LNS_L_Value < 200){

     Break_Turn(100);

     Back_Turn(500,100);

     Right_Turn(500,100);

   }else{  

   digitalWrite(MoterL_PIN_A, OFF);

   digitalWrite(MoterR_PIN_A, ON); 

   digitalWrite(MoterL_PIN_B, OFF);

   digitalWrite(MoterR_PIN_B, ON);          

   digitalWrite(PWM_PIN_A, ON);

   digitalWrite(PWM_PIN_B, ON);   

   } 

  Serial.print("LNS_R =");  Serial.print(LNS_R_Value);

  Serial.print("      ");

  Serial.print("LNS_L ="); Serial.println(LNS_L_Value);

  

}