Arduino 程式

2 個 IR Sensor

// vehicle02.ino
// 2個 IR Sensor
byte a1, a2;
unsigned long s0, s1;
int s2 = 50;
unsigned long m0, m1;
int intSpeed = 200;

void setup() {
  Serial.begin(9600); // put your setup code here, to run once:
  pinMode(2, INPUT);
  pinMode(4, INPUT);
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
}

void loop() {
  // 偵測時間
  if ((millis() - s0) > s2) {
    s0 = millis();
    a1 = digitalRead(2);
    a2 = digitalRead(4);
    Serial.print(a1); Serial.print("  "); Serial.println(a2);
  }
  // 前進
  if ((a1 == 1) && (a2 == 1)) {
    analogWrite(5, intSpeed);
    analogWrite(3, 0);
    analogWrite(9, intSpeed);
    analogWrite(6, 0);
  }
  // 停止/後退
  if ((a1 == 0) && (a2 == 0)) {
    analogWrite(3, 0);
    analogWrite(5, 0);
    analogWrite(6, 0);
    analogWrite(9, 0);
  }
  // 右轉
  if ((a1 == 1) && (a2 == 0)) {
    // 左輪往錢
    analogWrite(5, intSpeed);
    analogWrite(3, 0);
    // 右輪不動
    analogWrite(9, 0);
    analogWrite(6, 0);

  }
  // 左轉
  if ((a1 == 0) && (a2 == 1)) {
    // 左綸不動
    analogWrite(5, 0);
    analogWrite(3, 0);
    // 右輪往前
    analogWrite(9, intSpeed);
    analogWrite(6, 0);
  }
}

3 個 IR Sensor

// vehicle03.ino
// 3個 IR Sensor
const int SLeft = 9;      //左感測器輸入腳
const int SMiddle = 8;     //中間感測器輸入腳
const int SRight = 7;     //右感測器輸入腳

// 馬達與Arduino的腳位對應
const int Motor_E2 = 6; // 控制馬達2轉速 digital pin 6 of Arduino (PWM)
const int Motor_E1 = 5;  // 控制馬達1轉速 digital pin 5 of Arduino (PWM)
const int Motor_M1 = 10;     // 控制馬達1正反轉 digital pin 10 of Arduino
const int Motor_M2 = 11;    // 控制馬達2正反轉 digital pin 11 of Arduino

void setup() {
   //set up serial communications
   Serial.begin(57600);

  // 輸出入接腳初始設定
  pinMode(SLeft, INPUT);
  pinMode(SMiddle, INPUT);
  pinMode(SRight, INPUT);

  pinMode(Motor_M1, OUTPUT);  // 設定 Motor_M1為輸出腳位
  pinMode(Motor_M2, OUTPUT);  // 設定 Motor_M2為輸出腳位
}

//////////// 主程式 ////////
void loop(){
  int nIRStatus;

  //清除感測器狀態值
  byte byteSensorStatus = 0;
  // 讀取左感測器狀態值
  nIRStatus = digitalRead(SLeft);
  if(nIRStatus == 1)
     byteSensorStatus = (byteSensorStatus | (0x01 << 2));

  // 讀取中間感測器狀態值
  nIRStatus = digitalRead(SMiddle);
  if(nIRStatus == 1)
     byteSensorStatus = (byteSensorStatus | (0x01 << 1));

    // 讀取右邊感測器狀態值
  nIRStatus = digitalRead(SRight);
  if(nIRStatus == 1)
     byteSensorStatus = (byteSensorStatus | 0x01);

   drivemotor(byteSensorStatus);
}
///////////////////////////

void drivemotor(byte nStatus)
{
  switch(nStatus)
  { // 　　感測器 黑色:1  　白色:0
    case 7: // SL:1 SM:1 SR:1  //黑黑黑
            motorstop(0, 0);
            break;
    case 6: // SL:1 SM:1 SR:0  //黑黑白
            left(0, 255);
            break;
    case 5: // SL:1 SM:0 SR:1  //黑白黑
            motorstop(0, 255);
            break;
    case 4: // SL:1 SM:0 SR:0  //黑白白
            left(0, 255);
            break;
    case 3: // SL:0 SM:1 SR:1 // 白黑黑
             right(0, 255);
             break;
    case 2: // SL:0 SM:1 SR:0  // 白黑白
             forward(0, 255);
             break;
    case 1: // SL:0 SM:0 SR:1  // 白白黑
             right(0, 255);
             break;
    case 0: // SL:0 SM:0 SR:0  //白白白
             forward(0, 255);
  }
}

void motorstop(byte flag, byte motorspeed)
{
  Serial.println("stop!");

  digitalWrite( Motor_E1, motorspeed);
  digitalWrite( Motor_E2, motorspeed);
}

void forward(byte flag, byte motorspeed)
{
  Serial.println("forward!");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);
  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, motorspeed);
}

void backward(byte flag, byte motorspeed)
{
  Serial.println("backward!");

  digitalWrite( Motor_M1, LOW);
  digitalWrite( Motor_M2, LOW);
  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, motorspeed);
}

void right(byte flag, byte motorspeed)
{
  Serial.println("right!");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);
  analogWrite( Motor_E1, 0);
  analogWrite( Motor_E2, motorspeed);
}

void left(byte flag, byte motorspeed)
{
  Serial.println("left!");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);
  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, 0);
}