|
|
補上程式碼：
/*
三感測器自走車程式
-- 感測器訊號輸入為數位訊號 1: white, 0: black
-- 雙馬達控制轉向
-- 使用PWM控制馬達轉速, 但無後退動作
狀態與動作
白黑白 直進 (全速) (全速)
黑黑白 左轉 (半速) (全速)
白黑黑 右轉 (全速) (半速)
黑白白 快速左轉 (停止) (全速)
白白黑 快速右轉 (全速) (停止)
白白白 停止 (停止) (停止)
*/
// constants won't change. They're used here to
// set pin numbers:
const int SensorLeft = 2; //左感測器輸入腳
const int SensorMiddle = 3; //中感測器輸入腳
const int SensorRight = 4; //右感測器輸入腳
const int MotorLeft = 5; //左馬達輸出腳
const int MotorRight = 6; //右馬達輸出腳
// variables will change:
int SL; //左感測器狀態
int SM; //中感測器狀態
int SR; //右感測器狀態
void setup() {
// 輸出入椄腳初始設定, 指定為輸入或輸出
pinMode(SensorLeft, INPUT);
pinMode(SensorMiddle, INPUT);
pinMode(SensorRight, INPUT);
pinMode(MotorLeft, OUTPUT);
pinMode(MotorRight, OUTPUT);
// 預設馬達輸出為 0 (停止)
digitalWrite(MotorRight, LOW);
digitalWrite(MotorLeft, LOW);
}
void loop(){
// 讀取感測器狀態值
SL = digitalRead(SensorLeft);
SM = digitalRead(SensorMiddle);
SR = digitalRead(SensorRight);
// HIGH(1) : white region.
// LOW(0) : black region.
if (SM == LOW) { //中感測器在黑色區域
if (SL == LOW & SR == HIGH) { // 左黑右白, 向左轉彎
analogWrite(MotorLeft, 127); // 左輪半速
analogWrite(MotorRight, 255); // 右輪全速
}
else if (SR == LOW & SL == HIGH) { // 左白右黑, 向右轉彎
analogWrite(MotorLeft, 255); // 左輪全速
analogWrite(MotorRight, 127); // 右輪半速
}
else { // 兩側均為白色, 直進
analogWrite(MotorLeft, 255); // 兩輪都全速
analogWrite(MotorRight, 255);
}
}
else { // 中感測器在白色區域
if (SL == LOW & SR == HIGH) { // 左黑右白, 快速左轉
//digitalWrite(MotorLeft, LOW);
//digitalWrite(MotorRight, HIGH);
analogWrite(MotorLeft, 0); // 左輪停止
analogWrite(MotorRight, 255); // 右輪全速
}
else if (SR == LOW & SL == HIGH) { // 左白右黑, 快速右轉
//digitalWrite(MotorRight, LOW);
//digitalWrite(MotorLeft, HIGH);
analogWrite(MotorLeft, 255); // 左輪全速
analogWrite(MotorRight, 0); // 右輪停止
}
else { // 都是白色, 停止
//digitalWrite(MotorRight, LOW);
//digitalWrite(MotorLeft, LOW);
analogWrite(MotorLeft, 0);
analogWrite(MotorRight, 0);
}
}
} |
|
發表於 2011-4-8 10:52:02
收藏
分享
樓主