2015年5月25日月曜日

ジャパンロボテック社のRDS-X13の動作制御 (その1)

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

1.フィールド仕様
   サイズ: A3×4枚(縦79㎝ 横54㎝)
   黒線の厚さ: 1.5㎝
   外周の厚さ(外周から黒線までの幅): 3㎝




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

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


3.プログラムソース

//黒いサークル内だけを動くぷろぐらむ
// 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);
  
}

0 件のコメント: