Android, Servo i Buzzer

2012-02-13 23:30:32

#include <Servo.h>
Servo myservo;
int EN1 = 6;
int EN2 = 5;
int IN1 = 7;
int IN2 = 4;
int R = 1; //jazda do przodu
int F = 0; //jazda do tyłu
int SEN1 = 8;
int SERVO = 11;
int pos = 0;
int BUZZ = 13;

void Motor1(int pwm, boolean reverse) {
  analogWrite(EN1,pwm); //ustawia wypełnienie PWM: 0 - silnik zatrzymany, 255 - pełna prędkość
  if(reverse) {
    digitalWrite(IN1,HIGH);
  }else{
    digitalWrite(IN1,LOW);
  }
}

void Motor2(int pwm, boolean reverse) {
  analogWrite(EN2,pwm); //ustawia wypełnienie PWM: 0 - silnik zatrzymany, 255 - pełna prędkość
  if(reverse) {
    digitalWrite(IN2,HIGH);
  }else{
    digitalWrite(IN2,LOW);
  }
}

//dla drugiego silnika identycznie, tylko IN1 i EN1 zamienione są na IN2 i EN2

void setup(){
  int i;
  for(i=4;i&lt;=7;i++){ //Dla Arduino Motor Shield
    pinMode(i, OUTPUT);  //ustaw piny 4,5,6,7 jako wyjścia
  }
  pinMode(SEN1,INPUT); //pin pod który podłączono czujnik jako wejście
  //servo
  myservo.attach(SERVO);
  myservo.write(90);
  Serial.begin(9600);
  pinMode(BUZZ, OUTPUT);
}

void loop() {
  boolean in_range;
  //aktywnym stanem czujnika IR jest 0
  in_range = !digitalRead(SEN1);
  if(in_range) {
    buzz(2500, 100);
    buzz(2000, 50);
    buzz(2500, 100);
    //jeśli wykryto przeszkodę
    Motor1(0,F);
    Motor2(0,F);
    int ii = wolna();
    //Serial.println(ii);
    if(ii == 2){
      //Serial.println('RF');
      Motor1(255,R);
      Motor2(255,F);
      delay(1000);
    }else if(ii == 1){
      Serial.println('FR');
      Motor1(255,F);
      Motor2(255,R);
      delay(1000);
    }else{
    Serial.println('FR');
      Motor1(255,F);
      Motor2(255,R);
      delay(1000);
    }
    //
  }else {
    //jeśli nie widać przeszkody jedź prosto
    Motor1(255,F);
    Motor2(255,F);
  }
}

int wolna(){
  int _return = 0;

  myservo.attach(SERVO);
  myservo.write(135);
  delay(500);
  if(!digitalRead(SEN1)) {_return = 1;}
  myservo.detach();
  delay(500);

  myservo.attach(SERVO);
  myservo.write(45);
  delay(500);
  if(!digitalRead(SEN1)) {_return = 2;}
  myservo.detach();
  delay(500);

  myservo.attach(SERVO);
  myservo.write(90);
  delay(500);
  myservo.detach();

  return _return;
}

void buzz(long frequency, long length) {
  long delayValue = 1000000/frequency/2; // calculate the delay value between transitions
  //// 1 second's worth of microseconds, divided by the frequency, then split in half since
  //// there are two phases to each cycle
  long numCycles = frequency * length/ 1000; // calculate the number of cycles for proper timing
  //// multiply frequency, which is really cycles per second, by the number of seconds to
  //// get the total number of cycles to produce
 for (long i=0; i &lt; numCycles; i++){ // for the calculated length of time...
    digitalWrite(BUZZ,HIGH); // write the buzzer pin high to push out the diaphram
    delayMicroseconds(delayValue); // wait for the calculated delay value
    digitalWrite(BUZZ,LOW); // write the buzzer pin low to pull back the diaphram
    delayMicroseconds(delayValue); // wait againf or the calculated delay value
  }
}