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);
}
}