Aprende Robotica

sábado, 7 de julio de 2018

Programacion completa del autito incluido los sensores

#include <QTRSensors.h>

//Mapeo de pines
#define STBY 9
#define AIN1 3
#define AIN2 4
#define PWMB 5
#define PWMA 6
#define BIN1 8
#define BIN2 7
#define NUM_SENSORS 8
#define NUM_SAMPLES_PER_SENSOR 4
#define EMITTER_PIN 11
#define LED 13

// Constantes para PID
float KP = 0.20;
float KD = 1.0;
float Ki = 0.006;

// Regulación de la velocidad Máxima
int Velmax = 100;

// Datos para el integral
int error1=0;
int error2=0;
int error3=0;
int error4=0;
int error5=0;
int error6=0;
int error7=0;
int error8=0;
// Configuración de la librería QTR-8A
QTRSensorsAnalog qtra((unsigned char[])
{A0, A1, A2, A3, A4, A5, A6, A7}
, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

// Función de accionamiento del motor izquierdo
void Motoriz(int value)
{
if ( value >= 0 )
{
digitalWrite(BIN1,HIGH);
digitalWrite(BIN2,LOW);
}
else
{
digitalWrite(BIN1,LOW);
digitalWrite(BIN2,HIGH);
value *= -1;
}
analogWrite(PWMB,value);
}

// Función de accionamiento del motor derecho
void Motorde(int value)
{
if ( value >= 0 )
{
digitalWrite(AIN1,HIGH);
digitalWrite(AIN2,LOW);
}
else
{
digitalWrite(AIN1,LOW);
digitalWrite(AIN2,HIGH);
value *= -1;
}
analogWrite(PWMA,value);
}

//Accionamiento de ambos motores
void Motor(int left, int righ)
{
digitalWrite(STBY,HIGH);
Motoriz(left);
Motorde(righ);
}

//función de freno
void freno(boolean left, boolean righ, int value)
{
digitalWrite(STBY,HIGH);
if ( left )
{
digitalWrite(BIN1,HIGH);
digitalWrite(BIN2,HIGH);
analogWrite (PWMB, value);
}
if ( righ )
{
digitalWrite(AIN1,HIGH);
digitalWrite(AIN2,HIGH);
analogWrite (PWMA, value);
}
}

void setup()
{
// Declaramos como salida los pines utilizados
pinMode(LED ,OUTPUT);
pinMode(BIN2 ,OUTPUT);
pinMode(STBY ,OUTPUT);
pinMode(BIN1 ,OUTPUT);
pinMode(PWMB ,OUTPUT);
pinMode(AIN1 ,OUTPUT);
pinMode(AIN2 ,OUTPUT);
pinMode(PWMA ,OUTPUT);
//adapaciòn para el QTR Chino
pinMode(2,OUTPUT);
digitalWrite(2,HIGH);
Serial.begin(9600);
// Calibramos con la función qtra.calibrate();, y dejamos parpadeando el led, mientras se produce la calibración.

for ( int i=0; i<100; i++)
{
digitalWrite(LED, HIGH); delay(20);
qtra.calibrate();
digitalWrite(LED, LOW); delay(20);
}
delay(3000);
}

unsigned int position = 0;

//declaramos variables para utilizar PID
int proporcional = 0; // Proporcional
int integral = 0; //Integral
int derivativo = 0; // Derivativo

int diferencial = 0; // Diferencia aplicada a los motores
int last_prop; // Último valor del proporcional (utilizado para calcular la derivada del error)
int Target = 3500; // Setpoint (Como utilizamos 8 sensores, la línea debe estar entre 0 y 5000, por lo que el ideal es que esté en 3500)

void loop()
{

position = qtra.readLine(sensorValues, true, true);
proporcional = ((int)position) - 3500;
Serial.print(position);
Serial.print(" - ");
Serial.println(proporcional);

if ( proporcional <= -Target )
{
Motorde(0);
freno(true,false,255);
}
else if ( proporcional >= Target )
{
Motoriz(0);
freno(false,true,255);
}

derivativo = proporcional - last_prop;
integral = error1+error2+error3+error4+error5+error6+error7+error8;
last_prop = proporcional;

error8=error7;
error7=error6;
error6=error5;
error5=error4;
error4=error3;
error3=error2;
error2=error1;
error1=proporcional;

int diferencial = ( proporcional * KP ) + ( derivativo * KD )+ (integral*Ki) ;

if ( diferencial > Velmax ) diferencial = Velmax;
else if ( diferencial < -Velmax ) diferencial = -Velmax;

( diferencial < 0 ) ?
Motor(Velmax+diferencial, Velmax) : Motor(Velmax, Velmax-diferencial);
}

jueves, 5 de julio de 2018

Proyecto inicial del semáforo

En esta publicación te dejo el código de Arduino para el encendido de leds que componen el semáforo (Rojo, amarillo y verde).

/*************/
/* SEMAFORO */
/*************/

/*** Grupo ROBOTIZATE de la UAGRM ***/

//** Definiciones **//
int rojo=2; //definimos el valor del pin para el led rojo
int amarillo=4; //definimos el valor del pin para el led amarillo
int verde=7; //definimos el valor del pin para el led verde

//** Programa **//

void setup() {
pinMode(verde,OUTPUT); //declaramos el pin verde como salida
pinMode(amarillo,OUTPUT);//declaramos el pin amarillo como salida
pinMode(rojo,OUTPUT); //declaramos el pin rojo como salida
}

void loop() {

digitalWrite(rojo,HIGH); //encendemos el led rojo
delay(2000); //esperamos 2 segundos
digitalWrite(rojo,LOW); //apagamos el led rojo
delay(500); //esperamos medio segundo

digitalWrite(amarillo,HIGH); //encendemos el led amarillo
delay(2000); //esperamos 2 segundos
digitalWrite(amarillo,LOW); //apagamos el led amarillo
delay(500); //esperamos medio segundo

digitalWrite(verde,HIGH); //encendemos el led verde
delay(2000); //esperamos 2 segundos
digitalWrite(verde,LOW); //apagamos el led verde
delay(500); //esperamos medio segundo

}

Encendido de un motor DC con Arduino

Aqui te dejo el codigo de Arduino para el proyecto del autito de velocidad.

#define pwm_i 6 //~
#define izq_1 3
#define izq_2 4
#define STBY 9
#define pwm_d 5 //~
#define der_1 8
#define der_2 7

void setup() {
delay(1000);
pinMode(izq_1,OUTPUT);
pinMode(izq_2,OUTPUT);
pinMode(der_1,OUTPUT);
pinMode(der_2,OUTPUT);
}

void loop() {
motores(-50,-50);
delay(5000);
motores(50,50);
delay(5000);
motores(-50,50);
delay(5000);
motores(50,-50);
delay(5000);
}

void motores(int izq, int der)
{
if(izq>=0)
{
digitalWrite(izq_1,HIGH);
digitalWrite(izq_2,LOW);
digitalWrite(STBY,HIGH);
analogWrite(pwm_i,izq);
}
if(izq<0)
{
digitalWrite(izq_1,LOW);
digitalWrite(izq_2,HIGH);
digitalWrite(STBY,HIGH);
izq=izq*-1;
analogWrite(pwm_i,izq);
}

if(der>=0)
{
digitalWrite(der_1,HIGH);
digitalWrite(der_2,LOW);
digitalWrite(STBY,HIGH);
analogWrite(pwm_d,der);
}
if(der<0)
{
digitalWrite(der_1,LOW);
digitalWrite(der_2,HIGH);
digitalWrite(STBY,HIGH);
der=der*-1;
analogWrite(pwm_d,der);
}
}