ROBOT VELOCISTA

Objetivo:

Diseñar un robot velocista que pueda seguir una línea

Marco Teórico:

El proyecto consiste en diseñar y fabricar un robot velocista, de tal forma que sea capaz de seguir una línea negra lo más rápidamente 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 señal de PWM hacemos que a nuestro robot le sea posible ir más rápido o más lento, según las circunstancias que deseemos. Esto lo hacemos según la velocidad que imprimamos a los dos motores.

Si la velocidad es igual en ambos motores, el robot irá recto, si un motor gira más rápido 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 Batería de 7 volts

Código:

#include <QTRSensors.h>#define MOTORLEFT_DIR_A 7 // pin 1 de dirección del Motor Izquierdo#define MOTORLEFT_DIR_B 8 // pin 2 de dirección del Motor Izquierdo#define MOTORLEFT_PWM 6 // pin PWM del Motor Izquierdo#define MOTORRIGH_DIR_A 4 // pin 1 de dirección del Motor Derecho#define MOTORRIGH_DIR_B 3 // pin 2 de dirección del Motor Derecho#define MOTORRIGH_PWM 5 // pin PWM del Motor Derecho#define NUM_SENSORS 4 // número de sensores usados

// cant. lecturas analógicas que serán leídas 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 // número del pin de test#define BUTTONPIN 2 // boton externo

// función para pulsar el botón y esperar que deje de pulsarlo#define esperarBoton() while(!digitalRead(BUTTONPIN)); while(digitalRead(BUTTONPIN))

// estructura para los sensoresQTRSensorsAnalog qtra((unsigned char[]) {A0, A1, A2, A3}, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);

// arreglo para almacenamiento de valores por sensorunsigned int sensorValues[NUM_SENSORS];

// función Velocidad Motor Izquierdovoid 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);}

// función Velocidad Motor Derechovoid 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);}

// función Velocidad Motoresvoid setMotors(int left, int righ){ setMotorLeft(left); setMotorRigh(righ);}

// función Freno en Motoresvoid 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 botón para activar calibración while ( !digitalRead(BUTTONPIN) );

// calibrar sensores QTRA, titilando LED como guía 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 botón 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; // posición actual de los sensoresint derivative = 0; // derivadaint proportional = 0; // proporcionalint power_difference = 0; // velocidad diferencial

// Máxima velocidad en el poder diferencialint max = 140;

// Ultima Proporcionalint last_proportional=0;

// Constantes Proporcional y Derivativafloat KP = 8.6; //7.4float KD = 0; ;

// Constante para Rango de Freno (Range Brake)#define RANGEBRAKE 1500

void loop(){ // Obtiene la posición de la linea // Aquí no estamos interesados en los valores

// individuales de cada sensor position = qtra.readLine(sensorValues);

// El término proporcional debe ser 0 cuando estamos en línea 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 término derivativo (cambio) de la posición derivative = proportional - last_proportional;

// Recordando la última posición last_proportional = proportional;

// Calcula la diferencia entre la potencia de los dos motores [ m1 - m2 ]. // Si es un número positivo, el robot gira a la [ derecha ] // Si es un número negativo, el robot gira a la [ izquierda ] // y la magnitud del número 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 máxima 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);}Diseño:

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