111 lines
1.8 KiB
Plaintext
111 lines
1.8 KiB
Plaintext
const int fire = 10;
|
|
|
|
const int m1a = 3;
|
|
const int m1b = 5;
|
|
|
|
const int m2a = 6;
|
|
const int m2b = 9;
|
|
|
|
unsigned long time=0;
|
|
|
|
|
|
//int sensor[6] = { 0,0,0,0,0,0};
|
|
|
|
int average(int anaPin)
|
|
{
|
|
long total = 0;
|
|
long average = 0;
|
|
int count = 0;
|
|
int lecturas = 10;
|
|
for(int i=0; i<lecturas; i++)
|
|
{
|
|
total = total + analogRead(anaPin);
|
|
}
|
|
|
|
average = total / lecturas;
|
|
return(average);
|
|
|
|
}
|
|
|
|
void avanza_m1(byte vel)
|
|
{
|
|
//stop_m1();
|
|
analogWrite(m1a, 0);
|
|
analogWrite(m1b, vel);
|
|
}
|
|
|
|
void retrocede_m1(byte vel)
|
|
{
|
|
//stop_m1();
|
|
analogWrite(m1b, 0);
|
|
analogWrite(m1a, vel);
|
|
}
|
|
|
|
void stop_m1()
|
|
{
|
|
analogWrite(m1a, 0);
|
|
analogWrite(m1b, 0);
|
|
}
|
|
|
|
void avanza_m2(byte vel)
|
|
{
|
|
//stop_m2();
|
|
//delay(100);
|
|
analogWrite(m2a, 0);
|
|
analogWrite(m2b, vel);
|
|
}
|
|
|
|
void retrocede_m2(byte vel)
|
|
{
|
|
//stop_m2();
|
|
//delay(1000);
|
|
analogWrite(m2b, 0);
|
|
analogWrite(m2a, vel);
|
|
}
|
|
|
|
void stop_m2()
|
|
{
|
|
analogWrite(m2a, 0);
|
|
analogWrite(m2b, 0);
|
|
}
|
|
|
|
void setup() {
|
|
// initialize serial communications at 9600 bps:
|
|
Serial.begin(9600);
|
|
delay(500);
|
|
pinMode(fire, OUTPUT);
|
|
pinMode(19, INPUT);
|
|
digitalWrite(18, HIGH);
|
|
pinMode(18, INPUT);
|
|
digitalWrite(19, HIGH);
|
|
digitalWrite(m1a, HIGH);
|
|
digitalWrite(m1b, HIGH);
|
|
digitalWrite(m2a, HIGH);
|
|
digitalWrite(m2b, HIGH);
|
|
digitalWrite(fire, LOW);
|
|
Serial.println("RESET!!");
|
|
time=millis();
|
|
}
|
|
|
|
void loop() {
|
|
while (digitalRead(18)) retrocede_m2(60);
|
|
stop_m2();
|
|
|
|
while ((average(0)>100)&&((millis()-time)<10000)) avanza_m1(100);
|
|
if (average(0)<=100) time=millis();
|
|
if ((!digitalRead(19))&&(average(0)<=100))
|
|
{
|
|
digitalWrite(fire, HIGH);
|
|
delay(2000);
|
|
avanza_m2(60);
|
|
delay(6000);
|
|
stop_m2();
|
|
delay(4000);
|
|
digitalWrite(fire, LOW);
|
|
while (digitalRead(18)) retrocede_m2(60);
|
|
stop_m2();
|
|
time=millis();
|
|
}
|
|
else stop_m1();
|
|
}
|