Android, Servo, Motor Sheild and I/O Expresion

2013-05-11 16:31:26

#include <Servo.h>
Servo myservo;

int IrSenstoPin = 13;
int ServoPin = 9;

int EN1 = 6;
int EN2 = 5;
int IN1 = 7;
int IN2 = 4;

void setup(){
  pinMode(IN2, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(EN1, OUTPUT);
  pinMode(IN1, OUTPUT);

  pinMode(IrSenstoPin,INPUT);
  myservo.attach(ServoPin);
  myservo.write(90);
  Serial.begin(9600);
}

void loop() {
  boolean in_range;

  in_range = !digitalRead(IrSenstoPin);
  if(in_range) {

    Motor1(0, false);
    Motor2(0, false);

    String _direction = findAWay();

    if(_direction == "left"){
      Motor1(255, true);
      Motor2(255, false);
      delay(1000);
    }else if(_direction == "right"){
      Motor1(255, false);
      Motor2(255, true);
      delay(1000);
    }else if(_direction == "back"){
      Motor1(255, true);
      Motor2(255, false);
      delay(1000);
    }

  }else {
    Motor1(255, true);
    Motor2(255, true);
  }
  delay(100);
}

String findAWay(){
  String _return = "back";

  myservo.attach(ServoPin);
  myservo.write(135);
  delay(500);
  if(digitalRead(IrSenstoPin)) {
    _return = "right";
  }
  myservo.detach();
  delay(500);

  myservo.attach(ServoPin);
  myservo.write(45);
  delay(500);
  if(digitalRead(IrSenstoPin)) {
    _return = "left";
  }
  myservo.detach();
  delay(500);

  myservo.attach(ServoPin);
  myservo.write(90);
  delay(500);
  myservo.detach();
  delay(500);
  return _return;
}

void Motor1(int _speed, boolean reverse) {
  analogWrite(EN1, _speed);
  if(reverse) {
    digitalWrite(IN1,HIGH);
  }else{
    digitalWrite(IN1,LOW);
  }
}

void Motor2(int _speed, boolean reverse) {
  analogWrite(EN2, _speed);
  if(reverse) {
    digitalWrite(IN2,HIGH);
  }else{
    digitalWrite(IN2,LOW);
  }
}