[Arduino]Day-9-超音波加伺服馬達

程式碼
=========================================================

#include <Servo.h>
#define Servo_Pin      7  // 定義伺服馬達輸出腳位(PWM)
#define servo_delay    5  // 伺服馬達轉向後的穩定時間
Servo myservo;            // 宣告伺服馬達變數
int val = 0;
int Count_up = 1;

// Ultrasonic
int ECHO_Pin=2;  // ultrasonic ECHO
int TRIG_Pin=3; // ultrasonic TRIG
int distance = 0;

// initial
int Ini_Flag = 0;

void setup()
{
  Serial.begin(9600);
  pinMode(ECHO_Pin, INPUT);
  pinMode(TRIG_Pin, OUTPUT);
  myservo.attach(Servo_Pin);

  //
  val = 90;
  testServo(val);
  delay(3000);
  Serial.println("Servo 90 and serial ready" ) ;
  //
}
void loop()
{

//   //給安裝伺服馬達時的初始校正
//  if(Ini_Flag == 0)
//  {
//    testServo(0);
//    delay(3000);
//    testServo(180);
//    delay(3000);
//    testServo(90);
//    delay(5000);
//    Ini_Flag = 1;
//  }
  if(Count_up)
  {
    val+=1;
    if(val >= 170)
    {
      Count_up = 0;
    }
  }else{
    val-=1;
    if(val <= 9)
    {
      Count_up = 1;
    }
}
testServo(val);
distance = Get_Distance();
Serial.print("motor = ");
Serial.print(val);        //Output val
Serial.print(" distance = ");   //Output
Serial.println(distance);

}

int Get_Distance(void)
{
  digitalWrite(TRIG_Pin, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_Pin, HIGH); // Pulse for 10 micro sec to trigger ultrasonic detection
  delayMicroseconds(10);
  digitalWrite(TRIG_Pin, LOW);
  int distance = pulseIn(ECHO_Pin, HIGH);  // Read receiver pulse time
  distance= distance/58;   // Transform pulse time to distance
  return distance;
}

void testServo(int angle)
{
  myservo.write(angle);
  delay(servo_delay);  // 等待伺服馬達穩定
}