arduinocode/bandaCircular.ino at main · judasRV/arduinocode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#include <Servo.h>


Servo Servo1;			//Valores del Servo
const int servo=3;

const int motor=10;		//motor de la banda

int rojoLed = 7;		//Rojo
int verdeLed = 8;		//Verde
int azulLed = 9;		//Azul
int leds[3] = {0,0,0};

int pieza = 0;  // indica si se ha detectado una pieza.

const int pbRojo = 4; //Rojo
const int pbVerde = 5; //Verde
const int pbAzul = 6; //Azul

const int eccho = 11;
const int trig = 12;

int i;

// estas variables cuentan la piezas de colores
int r = 0;
int v = 0;
int a = 0;

/**
 * setup
 *
 */

void setup(){
	Serial.begin(9600);
  	pinMode( rojoLed, OUTPUT);
	pinMode( verdeLed, OUTPUT);
	pinMode( azulLed, OUTPUT);
	pinMode( motor, OUTPUT);						//motor banda

	pinMode( pbRojo, INPUT);						//Asignar PB
	pinMode( pbVerde, INPUT);
	pinMode( pbAzul, INPUT);

  	// sensor HC-SR04
  	pinMode( eccho, INPUT);
  	pinMode( trig, OUTPUT);

	Servo1.attach(3);   						//Servomotor
}

/**
 *  Usa el sensor HC-SR04
 * 	retorn a la distancia que sensa
 *
 * */

int obtenerDistancia(){
	digitalWrite(trig,LOW);
  	delayMicroseconds(5);
  	digitalWrite(trig,HIGH);
  	delayMicroseconds(10);
  	digitalWrite(trig,LOW);
	return pulseIn(eccho,HIGH)/58;
}

/**
 *  TCS 3200
 * 	detecta el color y cuenta el tipo
 * de pieza según su color.
 *
 * **/

void detectarColor(){
	if (digitalRead(pbRojo) == HIGH and digitalRead(pbVerde) == LOW and digitalRead(pbAzul) == LOW){
      	leds[0] = 1; leds[1] = 0; leds[2] = 0;
	   	pieza = 1;
      	r++;
      	return;

    } else if(digitalRead(pbRojo) == LOW and digitalRead(pbVerde) == HIGH and digitalRead(pbAzul)== LOW){
		leds[0] = 0; leds[1] = 1; leds[2] = 0;
	   	pieza = 1;
      	v++;
      	return;

    } else if(digitalRead(pbRojo) == LOW and digitalRead(pbVerde) == LOW and digitalRead(pbAzul) == HIGH){
		leds[0] = 0; leds[1] = 0; leds[2] = 1;
		pieza = 1;
      	a++;
      	return;

    } else if(digitalRead(pbRojo) == LOW and digitalRead(pbVerde) == LOW and digitalRead(pbAzul) == LOW){
		leds[0] = 0; leds[1] = 0; leds[2] = 0;
		pieza = 0;
  	}
}

/**
 *  Muestra las piezas
 *
 * */

void mostrarPiezas(){
	// muestra las piezas contadas y etiquetadas.
  	delay( (pieza)? 3000 : 0 );
  	Serial.print(" Rojas : ");
  	Serial.print(r);
  	Serial.print(" Verde : ");
  	Serial.print(v);
  	Serial.print(" Azul  : ");
  	Serial.println(a);
}

/* Enciende el led de acuerdo al color sensado.
 *
 * */

void encenderLed(){
	digitalWrite( rojoLed,  (leds[0])? HIGH : LOW);
  	digitalWrite( verdeLed, (leds[1])? HIGH : LOW);
  	digitalWrite( azulLed,  (leds[2])? HIGH : LOW);
}


/*
 * función principal donde todo se integra.
 *
 * */

void inicio(){
  	analogWrite( motor, (pieza)? 0 : 1200 );
  	Serial.print(" pieza distancia: ");
  	Serial.println( obtenerDistancia());
  	// mientras haya piezas
  	while( obtenerDistancia() < 10){
      	Serial.println(" Pieza detectada, sensando color ... ");
      	detectarColor();
  		encenderLed();
      	Servo1.write( (pieza)? 150 : 0  );
      	delay( (pieza)? 150 : 0 );
    }


  	// el motor avanza si no hay piezas, se detiene si hay una.


  	// el servo mueve una pieza, si hay una

	delay((pieza)? 2000 : 0);
  	mostrarPiezas();
}


void loop(){
 	inicio();
}