const int LedPin=13; // pin del led indicador

const int MotorRight1 = 5; // pin 5 para IN1

const int MotorRight2 = 6; // pin 6 para IN2
const int MotorRightPWM = 3; // pin 3 para ENA
const int MotorLeft1 = 10; // pin 10 para IN3
const int MotorLeft2 = 11; // pin 11 para IN4
const int MotorLeftPWM = 9; // pin 9 para ENB
const int Sensor = 2 ; // pin 2 para el sensor de l�nea
const int echoPin = 4; // pin 4 para el echo del SR-04
const int triggerPin = 7; // pin 7 para el trigger del SR-04
const int delta=10;
// Declaraciones de variables globales
char val; // Variable para guardar el car�cter recibido por puerto serie
int iSpeed = 150; // Velocidad "de crucero" para mis motores
int gSpeed = 72; // Velocidad "de giro" normal para mis motores
int suelo;
int lecturaact;
int lecturaant;
int dif;
int dir=1;
int i;
int angriv;
int detectado;
int distancia=40; // distancia l�mite para encontrar rival
void setup() {
//*********General SETUP: configuro puerto serie y los pines E/S
Serial.begin(9600);
pinMode(MotorRight1, OUTPUT);
pinMode(MotorRight2, OUTPUT);
pinMode(MotorLeft1, OUTPUT);
pinMode(MotorLeft2, OUTPUT);
pinMode(MotorRightPWM, OUTPUT);
pinMode(MotorLeftPWM, OUTPUT);
pinMode(Sensor, INPUT);
pinMode(echoPin, INPUT);
pinMode(triggerPin, OUTPUT);
// En las competiciones de sumo hay que esperar 5 segundos
delay(5000);
}
void loop()
{
suelo= digitalRead(Sensor);
busca();
if(suelo==1 && detectado==0){turnL(10);}
if(suelo==1 && detectado==1){advance(20);}
if(suelo==0){
// Si leo suelo blanco ...
turnL(20);
back(20);
}
}
void busca()
{
lecturaact=getDistance();
if(lecturaact<distancia){detectado=1;}
if(lecturaact>distancia){detectado=0;}
}
// ******************************* FUNCIONES
********************************************
//
// ejemplos de uso de las funciones
// advance(50);
// left(100);
// variable = getDistance();
// variable = digitalRead(SensorMiddle);
// getMin devuelve la posici�n del array que contiene el m�nimo valor
// x = getMin(lecturas, 30); // se le pasa el nombre del array y su tama�o y
devuelve INTERNAL
int getMin(int* array, int size){
int salida=0;
int minimum = array[0];
for (int i = 0; i < size; i++)
{
if (array[i] < minimum){
minimum = array[i];
salida=i;
}
}
return salida;
}
void advance(int d) {
// avanzar
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, HIGH);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 1);
}
void right(int d) {
// Giro a derecha con una sola rueda
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, LOW);
analogWrite(MotorRightPWM, gSpeed);
analogWrite(MotorLeftPWM, gSpeed);
delay(d * 1);
}
void left(int d) {
// Giro a la izquierda una sola rueda
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, gSpeed);
analogWrite(MotorLeftPWM, gSpeed);
delay(d * 1);
}
void turnR(int d) {
// Giro a la derecha con ambas ruedas
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
analogWrite(MotorRightPWM, gSpeed);
analogWrite(MotorLeftPWM, gSpeed);
delay(d * 1);
}
void turnL(int d) {
// Giro a la izquierda con ambas ruedas
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, HIGH);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, gSpeed);
analogWrite(MotorLeftPWM, gSpeed);
delay(d * 1);
}
void stopp(int d) {
// Detenerse
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 1);
}
void back(int d) {
// Retrocede
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 1);
}
float getDistance() {
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);
digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);
digitalWrite(triggerPin, LOW);
float distance = pulseIn(echoPin, HIGH);
distance = distance / 2 / 129;
Serial.print(int(distance)); // output distancia (cm)
Serial.print("\n");
return distance;
}