Stap 3: Programar Arduino
#include < Servo.h >
Servo sonar; maken van servo-object om te bepalen van een servo
Servo timon; objeto controle timon
Servomotor;
int configPin = 13;
VOID Setup
{
sonar.attach(9);
Timon.attach(8);
motor.attach(11);
Serial.begin(9600);
pinMode(configPin,OUTPUT);
inicializo motor
int VelMin = 60;
int VelMax = 100;
for (int i = 60; ik < 100; i ++)
{
motor.write(i);
delay(100);
}
incializo timon
int TimMin = 0;
int TimMax = 90;
for (int i = TimMin; ik < TimMax; i ++)
{
Timon.write(i);
delay(10);
}
inicializo sonar
int SonarMin = 0;
int SonarMax = 90;
for (int i = SonarMin; ik < SonarMax; i ++)
{
sonar.write(i);
delay(10);
}
}
////////////////
Funciones / /
//////////////
int Detecto_Objeto (int pos)
{
Pongo servo nl 0,90,180
sonar.write(POS);
delay(250);
Mido la distancia
digitalWrite(configPin,HIGH); verhogen van de resetpin hoge
delay(250); begin van kalibratie ring
zweven sensorvalue = analogRead(5); analoge sensor waarde krijgen van pin 5
zweven cmvalue = (1,25 * sensorvalue + 3,75);
Serial.Print("distancia="); inch tekst afdrukken
Serial.println(cmvalue); inch waarde afdrukken
delay(15); optionele vertraging van 1 seconde
digitalWrite(configPin,LOW); Kalibratie ring en sensor uitschakelen
/*Serial.Print ("pos/Distancia =");
Serial.Print(POS);
Serial.Print("/");
Serial.Print(cmvalue);
Serial.Print("\n"); * /
Return cmvalue;
}
Funciones de movimiento
VOID Aceleracion(int vel)
{
int i = 0;
Serial.Print ("Aceleracion");
Serial.Print(vel);
Serial.Print("\n");
voor (ik = 60; ik < vel; i ++)
{
motor.write(i);
delay(100);
Serial.Print ("pos/vel");
Serial.Print(i);
Serial.Print("/");
Serial.Print(vel);
Serial.Print("\n");
}
delay(1000);
}
VOID VehiculoCTL(int pos)
{
int Adelante = 90;
int Derecha = 0;
int Izquierda = 180;
int Vel;
Timon.write(POS);
Aceleracion(70);
delay(150);
Serial.Print ("pos/vel");
Serial.Print(POS);
Serial.Print("/");
Serial.Print(vel);
Serial.Print("\n");
delay(100);
}
/////////////////////////
Programa Principal / /
///////////////////////
void loop
{
Detecto objeto
int pos = 0;
int DistChoque = 30;
int Adelante = 90;
int Derecha = 0;
int Izquierda = 180;
Als (Detecto_Objeto(Adelante) > DistChoque)
VehiculoCTL(Adelante);
anders
Als (Detecto_Objeto(Derecha) > DistChoque)
VehiculoCTL(Izquierda);
anders
Als (Detecto_Objeto(Izquierda) > DistChoque)
VehiculoCTL(Derecha);
}