Étape 3: Code
J’ai essayé d’écrire le code assez facile d’être clair pour les débutants aussi. Mais si vous avez des problèmes, questions et peut-être des suggestions, je voudrais savoir.
Quelques conseils :
- Distance minimale est la distance d’arrêt afin d’éviter l’accident. Est la meilleure distance de n’importe quel bloc pour arrêter le robot, dépend de la vitesse de la boîte de vitesses.
- Robot revient à distance max.
- backDist et turnDist doivent être personnalisés dépendent de la vitesse du robot.
- Un comportement aléatoire, y compris un arrêt, mélodie et Mouvement aléatoire a utilisé.
//******************************
* Par : http://blog.mshams.ir *
//******************************
codes PIN
const byte pinMotor1 = 4, pinMotor2 = 5, pinMotor3 = 6, pinMotor4 = 7 ;
const byte pinLed = 8, pinTone = 10 pinSonar1 = 11, pinSonar2 = 12 ;
freq sonore
const int toneFreq = 800 ;
distances de sonar
const byte minDistance = 45, maxDistance = 1000 ;
États de moteurs
const byte M_STOP = 0, M_GO = 1, M_BACK = 2, M_RIGHT = 3, M_LEFT = 4, M_RIGHT2X = 5, M_LEFT2X = 6 ;
États de robot
const byte R_START = 0, R_DRIVE = 1, R_BLOCK = 2, R_TURN = 3 ;
détecter la gueule de bois
const int hangoverWait = 5000 ;
bloc de détection aller retour
const int backDist = 1000, turnDist = 300 ;
comportement aléatoire
const int randomChance = 300, randomStopWait = 10000, randomTurnWait = 200 ;
État de l’octet = R_START ;
int i, compteur = 0 ;
int dist ;
void setup() {}
randomSeed(analogRead(1)) ;
init broches moteur
pinMode (pinMotor1, sortie) ;
pinMode (pinMotor2, sortie) ;
pinMode (pinMotor3, sortie) ;
pinMode (pinMotor4, sortie) ;
pinMode (pinSonar1, sortie) ;
pinMode (pinSonar2, entrée) ;
init pin de LED
pinMode (pinLed, sortie) ;
}
void loop() {}
interrupteur (État) {}
affaire R_START :
Motor(M_GO) ;
Melody() ;
État = R_DRIVE ;
rupture ;
affaire R_DRIVE :
Dist = Pingu() ;
Si (dist < minDistance) {}
Motor(M_STOP) ;
État = R_BLOCK ;
}
ElseIf (aléatoire (0, randomChance) == 5) {}
RandomTone() ;
Motor(M_STOP) ;
Delay(randomStopWait) ;
Moteur (aléatoire (M_BACK, M_LEFT2X + 1)) ;
Delay(randomTurnWait) ;
Motor(M_GO) ;
}
rupture ;
affaire R_BLOCK :
Beep() ;
Motor(M_BACK) ;
Delay(backDist) ;
Moteur (aléatoire (M_RIGHT2X, M_LEFT2X + 1)) ;
Delay(turnDist) ;
État = R_TURN ;
rupture ;
affaire R_TURN :
Dist = Pingu() ;
Si (dist > = maxDistance) {}
Motor(M_STOP) ;
Motor(M_GO) ;
Beep() ;
État = R_DRIVE ;
}
else {}
Counter += 1 ;
Si (compteur > = hangoverWait) {}
compteur = 0 ;
Motor(M_STOP) ;
RandomTone() ;
État = R_BLOCK ;
}
}
rupture ;
}
}
Sub {Blink (État octets)
digitalWrite (pinLed, État) ;
}
Sub {Beep()
Blink(1) ;
SpeakTone(2) ;
SpeakTone(1) ;
Blink(0) ;
}
void Melody() {}
Blink(1) ;
pour (int i = 1; i < 10; i ++) {}
SpeakTone(i) ;
}
Blink(0) ;
}
void RandomTone() {}
Blink(1) ;
pour (int i = 1; i < 10; i ++) {}
SpeakTone (random(1, 10)) ;
}
Blink(0) ;
}
void SpeakTone(byte note) {}
ton (pinTone, toneFreq * Remarque) ;
Delay(100) ;
noTone(pinTone) ;
}
Sub {moteur (État octets)
octet m1, m2, m3, m4 ;
interrupteur (État) {}
cas M_STOP :
M1 = m2 = m3 = m4 = faible ;
rupture ;
affaire M_GO :
M1 = élevé ;
m2 = faible ;
m3 = élevé ;
M4 = faible ;
rupture ;
affaire M_BACK :
M1 = faible ;
m2 = élevé ;
m3 = faible ;
M4 = élevé ;
rupture ;
affaire M_RIGHT :
M1 = élevé ;
m2 = faible ;
m3 = faible ;
M4 = faible ;
rupture ;
affaire M_LEFT :
M1 = faible ;
m2 = faible ;
m3 = élevé ;
M4 = faible ;
rupture ;
affaire M_RIGHT2X :
M1 = élevé ;
m2 = faible ;
m3 = faible ;
M4 = élevé ;
rupture ;
affaire M_LEFT2X :
M1 = faible ;
m2 = élevé ;
m3 = élevé ;
M4 = faible ;
rupture ;
}
digitalWrite (pinMotor1, m1) ;
digitalWrite (pinMotor2, m2) ;
digitalWrite (pinMotor3, m3) ;
digitalWrite (pinMotor4, m4) ;
}
int Pingu() {}
digitalWrite (pinSonar1, basse) ;
delayMicroseconds(5) ;
digitalWrite (pinSonar1, HIGH) ;
delayMicroseconds(5) ;
digitalWrite (pinSonar1, basse) ;
delayMicroseconds(2) ;
longue durée = pulseIn (pinSonar2, HIGH) ;
retour (durée / 29 2) ;
}