1.フィールド仕様
サイズ: A3×4枚(縦79㎝ 横54㎝)
黒線の厚さ: 1.5㎝
外周の厚さ(外周から黒線までの幅): 3㎝
2.動作
ロボットには2つの(モータ・ギア・車輪)が付いており、マイコンボードから独立に制御できます。 モータの回転方向を制御することで、ロボットは前進・後退・右回転・左回転が可能です。モーターのスピートはモーターがDCブラシモーターであることから制御できません。
ロボットの底面には、赤外線ラインセンサーが2つ付いています。この2つのラインセンサーのどちらかが黒線を検知すると、ロボットは30cmほどバックし、右に約90度回転します。そしてまた、前進を開始します。
ロボットの底面には、赤外線ラインセンサーが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 件のコメント:
コメントを投稿