ROBOT VELOCISTA

Objetivo:

Disear un robot velocista que pueda seguir una lnea

Marco Terico:
El proyecto consiste en disear y fabricar un robot velocista, de tal
forma que sea capaz de seguir una lnea negra lo ms rpidamente
sin salirse de ella, El robot se controla por un ARDUINO, en el cual se
ubica el programa que hace que el robot funcione como un velocista.
El robot consta de dos motores que son controlados por el dispositivo
de potencia, que a su vez son dependientes del arduino. Mediante
una seal de PWM hacemos que a nuestro robot le sea posible ir ms
rpido o ms lento, segn las circunstancias que deseemos. Esto lo
hacemos segn la velocidad que imprimamos a los dos motores.
Si la velocidad es igual en ambos motores, el robot ir recto, si un
motor gira ms rpido que otro, har que nuestro robot gire

Materiales:
1 Arduino
2 Motores DC
1 Regulador de voltaje 7805
Un puente H L293D
4 sensores cny70
4 resistencias de 220
4 resistencias de 47 K
1 pulsador

Cables
1 Batera de 7 volts

Cdigo:
#include <QTRSensors.h>
#define MOTORLEFT_DIR_A
#define MOTORLEFT_DIR_B
#define MOTORLEFT_PWM
#define MOTORRIGH_DIR_A
#define MOTORRIGH_DIR_B
#define MOTORRIGH_PWM
#define NUM_SENSORS

7
8
6
4
3
5

// pin 1 de direccin del Motor Izquierdo

// pin 2 de direccin del Motor Izquierdo
// pin PWM del Motor Izquierdo
// pin 1 de direccin del Motor Derecho
// pin 2 de direccin del Motor Derecho
// pin PWM del Motor Derecho
4 // nmero de sensores usados

// cant. lecturas analgicas que sern ledas por sensor

#define NUM_SAMPLES_PER_SENSOR 4
#define EMITTER_PIN
11 // pin emisor del QTR
#define ENCODERPIN 10
// pin del encoder
#define LEDPIN
13
// nmero del pin de test
#define BUTTONPIN 2
// boton externo
// funcin para pulsar el botn y esperar que deje de pulsarlo
#define esperarBoton() while(!digitalRead(BUTTONPIN));
while(digitalRead(BUTTONPIN))
// estructura para los sensores
QTRSensorsAnalog qtra((unsigned char[])
{A0, A1, A2, A3}
, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
// arreglo para almacenamiento de valores por sensor
unsigned int sensorValues[NUM_SENSORS];
// funcin Velocidad Motor Izquierdo
void setMotorLeft(int value)
{
if ( value >= 0 )
{
// si valor positivo vamos hacia adelante
digitalWrite(MOTORRIGH_DIR_A,HIGH);
digitalWrite(MOTORRIGH_DIR_B,LOW);
}
else
{
// si valor negativo vamos hacia atras
digitalWrite(MOTORRIGH_DIR_A,LOW);
digitalWrite(MOTORRIGH_DIR_B,HIGH);
value *= -1;
}
// Setea Velocidad

analogWrite(MOTORRIGH_PWM,value);
}
// funcin Velocidad Motor Derecho
void setMotorRigh(int value)
{
if ( value >= 0 )
{
// si valor positivo vamos hacia adelante
digitalWrite(MOTORLEFT_DIR_A,HIGH);
digitalWrite(MOTORLEFT_DIR_B,LOW);
}
else
{
// si valor negativo vamos hacia atras
digitalWrite(MOTORLEFT_DIR_A,LOW);
digitalWrite(MOTORLEFT_DIR_B,HIGH);
value *= -1;
}
// Setea Velocidad
analogWrite(MOTORLEFT_PWM,value);
}
// funcin Velocidad Motores
void setMotors(int left, int righ)
{
setMotorLeft(left);
setMotorRigh(righ);
}
// funcin Freno en Motores
void setBrake(boolean left, boolean righ, int value)
{
if ( left )
{
// pines LEFT motor
digitalWrite(MOTORRIGH_DIR_A,HIGH);
digitalWrite(MOTORRIGH_DIR_B,HIGH);
analogWrite (MOTORRIGH_PWM, value);
}
if ( righ )
{
// pines RIGH motor
digitalWrite(MOTORLEFT_DIR_A,HIGH);
digitalWrite(MOTORLEFT_DIR_B,HIGH);
analogWrite (MOTORLEFT_PWM, value);
}
}

void setup()
{
// inicializar pines de salida
pinMode(LEDPIN
,OUTPUT);
pinMode(MOTORRIGH_DIR_A ,OUTPUT);
pinMode(MOTORRIGH_DIR_B ,OUTPUT);
pinMode(MOTORRIGH_PWM ,OUTPUT);
pinMode(MOTORLEFT_DIR_A ,OUTPUT);
pinMode(MOTORLEFT_DIR_B ,OUTPUT);
pinMode(MOTORLEFT_PWM ,OUTPUT);
pinMode(BUTTONPIN
,INPUT);
// presiona botn para activar calibracin
while ( !digitalRead(BUTTONPIN) );
// calibrar sensores QTRA, titilando LED como gua
for ( int i=0; i<70; i++)
{
digitalWrite(LEDPIN, HIGH); delay(20);
qtra.calibrate();
digitalWrite(LEDPIN, LOW); delay(20);
}
// apagar LED
digitalWrite(LEDPIN, LOW);
// presionar botn para correr el robot
while ( !digitalRead(BUTTONPIN) );
// esperar 5 segundos
delay(5000);
// mover el robot suave para ganar inercia
setMotors(130,130);
// durante 0.3 segundos
delay(300);
}
unsigned int position = 0; // posicin actual de los sensores
int derivative = 0;
// derivada
int proportional = 0;
// proporcional
int power_difference = 0; // velocidad diferencial
// Mxima velocidad en el poder diferencial
int max = 140;
// Ultima Proporcional
int last_proportional=0;
// Constantes Proporcional y Derivativa

float KP = 8.6; //7.4

float KD = 0;

// Constante para Rango de Freno (Range Brake)

#define RANGEBRAKE 1500
void loop()
{
// Obtiene la posicin de la linea
// Aqu no estamos interesados en los valores
// individuales de cada sensor
position = qtra.readLine(sensorValues);
// El trmino proporcional debe ser 0 cuando estamos en lnea
proportional = ((int)position) - 1500;
// Si entra en el rango de freno, aplicarlo en la
// direccion de la curva
if ( proportional <= -RANGEBRAKE )
{
setMotorRigh(0);
setBrake(true,false,255);
delay(1);
}
else if ( proportional >= RANGEBRAKE )
{
setMotorLeft(0);
setBrake(false,true,255);
delay(1);
}
// Calcula el trmino derivativo (cambio) de la posicin
derivative = proportional - last_proportional;
// Recordando la ltima posicin
last_proportional = proportional;
// Calcula la diferencia entre la potencia de los dos motores [ m1 - m2 ].
// Si es un nmero positivo, el robot gira a la [ derecha ]
// Si es un nmero negativo, el robot gira a la [ izquierda ]
// y la magnitud del nmero determina el ngulo de giro.
int power_difference = ( proportional*KP ) + ( derivative * KD );
// Si velocidad diferencial es mayor a la posible tanto positiva como
negativa,
// asignar la mxima permitida
if ( power_difference > max ) power_difference=max;
else if ( power_difference < -max ) power_difference =-max;
// Asignar velocidad calculada en el poder diferencial de los motores
if( power_difference < 0 )
setMotors(max+power_difference,max) ;
else

setMotors(max,max-power_difference);
}

Diseo:

UNIVERSIDAD MAYOR DE SAN SIMON

FACULTAD DE CIENCIAS Y TECNOLOGIA

PREINFORME
Proyecto de ROBOTICA
INTEGRANTES DEL GRUPO:
- ANA MAMANI LOPEZ

- RUDDY ANGEL
CABALLOTTI TORRICO
HORARIO:
MARTES 14:15 - 15:45
FECHA:
16/10/14
AUXILIAR:
MARIO ZEVERICH ZURITA
GRUPO:
13
COCHABAMBA BOLIVIA