用蓝牙控制二自由度舵机+超声波测距

#include "Servo.h"
#include "Arduino.h"
#include "SoftwareSerial.h"

#define MAX_BUF_LEN 60   //max input/output buffer length
#define SERVO1_DPIN 8    //1st servo pin
#define SERVO2_DPIN 9    //2nd servo pin
#define TP 6      //Trig_pin
#define EP 7      //Echo_pin

Servo servo1,servo2;
char scan_mode1, scan_mode2;
int inc1, inc2;
int dir1, dir2;

void setup(){
  pinMode(TP,OUTPUT);       // set TP output for trigger
  pinMode(EP,INPUT);        // set EP input for echo

  servo1.attach(SERVO1_DPIN);
  servo2.attach(SERVO2_DPIN);
  servo1.write(90);
  servo2.write(110);

  Serial.begin(38400);      // init serial 9600

  scan_mode1='0';
  scan_mode1='0';
  dir1=1;
  dir2=1;
  inc1=0;
  inc2=0;
}

void loop(){
    int i,n;
    char buf[MAX_BUF_LEN] = {0};
    int degree1, degree2;
    int inc;

    if(Serial.available()){
        i=0;
        do{
            buf[i++] = Serial.read();
            delay(2);
        }while(Serial.available() && i<MAX_BUF_LEN);

        n=i;
        for(i=0;i<n;i+=4){
            degree1=servo1.read();
            degree2=servo2.read();

            // get the degree increment from serial input
            inc=buf[i+2]-'0';
            inc=10*inc+buf[i+3]-'0';

            // get the inc sign: positve or negative
            if(buf[i+1]=='-')inc=-inc;


            if(buf[i]=='x'){
                servo1.write(degree1+inc);
                scan_mode1='0';
            }else if(buf[i]=='y'){
                servo2.write(degree2+inc);
                scan_mode2='0';
            }else if(buf[i]=='r'){
                // reset the servos
                servo1.write(90);
                servo2.write(110);
                scan_mode1='0';
                scan_mode2='0';
                inc1=0;
                inc2=0;
            }else if(buf[i]=='s' && buf[i+1]=='x'){
                scan_mode1='x';
                inc1=inc;
            }else if(buf[i]=='s' && buf[i+1]=='y'){
                scan_mode2='y';
                inc2=inc;
            }

        }
    }

    if(scan_mode1=='x'){
      degree1=servo1.read();
      if(degree1+dir1*inc1>=180)
      {
        dir1=-1;
        if(scan_mode2=='y'){
          degree2=servo2.read();
          if(degree2+dir2*inc2>=180) dir2=-1;
          else if(degree2+dir2*inc2<=0) dir2=1;
          servo2.write((degree2+dir2*inc2)%180);
        }
      }
      else if(degree1+dir1*inc1<=0){
        dir1=1;
        if(scan_mode2=='y'){
          degree2=servo2.read();
          if(degree2+dir2*inc2>=180) dir2=-1;
          else if(degree2+dir2*inc2<=0) dir2=1;
          servo2.write((degree2+dir2*inc2)%180);
        }
      }
      servo1.write((degree1+dir1*inc1)%180);
    }

    delay(20);

    long microseconds = TP_init();
    int dist_cm = Distance(microseconds);
    Serial.print("Distance_CM = ");
    Serial.println(dist_cm);


    delay(200);
}


int Distance(long time)
{
    // Distance_CM  = ((Duration of high level)*(Sonic :340m/s))/2
    //              = ((Duration of high level)*(Sonic :0.034 cm/us))/2
    //              = ((Duration of high level)/(Sonic :29.4 cm/us))/2
    int dist = time/29/2  ;

    return dist;
}

long TP_init()
{
  digitalWrite(TP, LOW);
  delayMicroseconds(2);
  digitalWrite(TP, HIGH);                 // pull the Trig pin to high level for more than 10us impulse
  delayMicroseconds(10);
  digitalWrite(TP, LOW);
  long microseconds = pulseIn(EP,HIGH);   // waits for the pin to go HIGH, and returns the length of the pulse in microseconds
  return microseconds;                    // return microseconds
}