MANUELL / AUTONOM kontroll ROBOT (med SENSOR FUSION teknik) (8 / 12 steg)
Steg 8: Koden: - för HC-SR04 sensorn
CONST int serialPeriod = 250; en period av 250ms = en frekvens av 4 Hz
osignerade långa timeSerialDelay = 0;
CONST int UltraloopPeriod = 20; en period av 20ms = en frekvens på 50 Hz
osignerade UltraLoopDelay = 0;
CONST int sensor_1_in = 10; indata/utdata från sensor_1
CONST int sensor_1_out = 13.
CONST int sensor_2 = 11;
CONST int sensor_3 = 12;
int motorL1 = 6; utgång för motorn förarens stift 2
int motorL2 = 7. utgång för motorn förarens stift 7
int motorR1 = 8; utgång för motor driver pin 15
int motorR2 = 9; utgång för motorn förarens stift 10
int d0 = 2; ingång från DTMF stift D0
int d1 = 3; ingång från DTMF stift D1
int d2 = 4; ingång från DTMF stift D2
int d3 = 5; ingång från DTMF stift D3
int ultrasonicTime; variabel för att spara tid
int ultrasonicDistance; variabel att lagra avståndet beräknas
void setup()
{
Serial.BEGIN(9600); inställningen för seriell kommunikationshastighet
pinMode (motorL1, produktionen);
pinMode (motorL2, produktionen);
pinMode (motorR1, produktionen);
pinMode (motorR2, produktionen);
pinMode (d0, ingång);
pinMode (d1, ingång);
pinMode (d2, ingång);
pinMode (d3, ingång);
}
void loop()
{
int z = digitalRead(d0);
int y = digitalRead(d1);
int x = digitalRead(d2);
int w = digitalRead(d3);
/*----------------------------------------- M A N U A L M O D E ---------------------------------------*/
IF(w == low)
{
om ((w == LOW) & &(x == LOW) & &(y == HIGH) & &(z == LOW))
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, hög);
digitalWrite (motorR2, låg);
}
om ((w == LOW) & &(x == HIGH) & &(y == LOW) & &(z == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
om ((w == LOW) & &(x == HIGH) & &(y == LOW) & &(z == LOW))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, hög);
digitalWrite (motorR2, låg);
}
om ((w == LOW) & &(x == HIGH) & &(y == HIGH) & &(z == LOW))
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
om ((w == LOW) & &(x == HIGH) & &(y == HIGH) & &(z == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, låg);
}
}
/*----------------------------------- A U T O N O M O U S M O D E -----------------------------------*/
IF(w == High)
{
viewDistance();
IF((Millis() - UltraLoopDelay) > = UltraloopPeriod)
{
readUltrasonicsensor_1();
motorStart();
UltraLoopDelay = millis();
}
}
}
void readUltrasonicsensor_1() / / förändrat
{
digitalWrite (sensor_1_out, låg);
Delay(2);
digitalWrite (sensor_1_out, hög);
Delay(10);
digitalWrite (sensor_1_out, låg);
ultrasonicTime = pulseIn (sensor_1_in, hög);
ultrasonicDistance = (ultrasonicTime/2) / 29; beräkningen att mäta distansera av hinder från ultrasonic sensor
}
void motorStart() //function för att driva motorn enligt kände avståndet
{
int irsL = digitalRead(sensor_2); IR-sensorn som vänster
int irsR = digitalRead(sensor_3); IR-sensorn som rätt
om (ultrasonicDistance > 10) //this del kod arbetar för att förhindra sida kollisioner även
{//when det är inget hinder framför roboten
om ((irsL == LOW) & &(irsR== LOW))
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, hög);
digitalWrite (motorR2, låg);
}
om ((irsL == HIGH) & &(irsR == LOW))
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
om ((irsL == LOW) & &(irsR == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, hög);
digitalWrite (motorR2, låg);
}
om ((irsL == HIGH) & &(irsR == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
}
om (ultrasonicDistance < 10) //this del av koden fungerar när det finns några
{//obstacle är infront av roboten och även
om ((irsL == HIGH) & &(irsR == LOW)) //to förhindra sidokollision
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
om ((irsL == LOW) & &(irsR == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, hög);
digitalWrite (motorR2, låg);
}
om ((irsL == HIGH) & &(irsR == HIGH))
{
digitalWrite (motorL1, låg);
digitalWrite (motorL2, hög);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
om ((irsL == LOW) & &(irsR == LOW))
{
digitalWrite (motorL1, hög);
digitalWrite (motorL2, låg);
digitalWrite (motorR1, låg);
digitalWrite (motorR2, hög);
}
}
}
/ *---KONTROLLERA ULTRSONIC SENSORN---* /
void viewDistance() //function att Visa avstånd på serial monitor
{//to kontrollera om ultra sonic sensor koden fungerar
IF((Millis() - timeSerialDelay) > = serialPeriod)
{
Serial.Print("Distance");
Serial.Print(ultrasonicDistance);
Serial.Print("cm");
Serial.println();
timeSerialDelay = millis();
}
}