Arduino, Mini Sonar Robot (3 / 3 steg)
Steg 3: kod
Jag har försökt att skriva koden lätt att vara vanligt för nybörjare också. Men om du har några problem, frågor och får förslag, jag vet.
Några tips:
- Minsta avstånd är stoppsträckan att förhindra kraschen. Bästa avståndet från varje block att stoppa roboten, är beror på hur växellådan snabbt.
- Robot kommer tillbaka till max avstånd.
- backDist och turnDist bör anpassas beroende av robot hastigheten.
- En slumpmässiga beteende inklusive stopp, har melodi och slumpmässiga rörelsen använt.
//******************************
* Av: http://blog.mshams.ir *
//******************************
PIN-koder
CONST byte pinMotor1 = 4, pinMotor2 = 5, pinMotor3 = 6, pinMotor4 = 7.
CONST pinLed byte = 8, pinTone = 10, pinSonar1 = 11, pinSonar2 = 12;
sund freq
CONST int toneFreq = 800;
sonar avstånd
CONST byte minDistance = 45, maxDistance = 1000;
motor: s
CONST byte M_STOP = 0, M_GO = 1 M_BACK = 2, M_RIGHT = 3, M_LEFT = 4, M_RIGHT2X = 5, M_LEFT2X = 6;
robot stater
CONST byte R_START = 0, R_DRIVE = 1 R_BLOCK = 2, R_TURN = 3;
upptäcka baksmälla
CONST int hangoverWait = 5000;
blocket upptäcka gå tillbaka
CONST int backDist = 1000, turnDist = 300;
slumpmässiga beteende
CONST int randomChance = 300, randomStopWait = 10000, randomTurnWait = 200.
byte state = R_START;
int i, räknare = 0;
int dist;
void setup() {
randomSeed(analogRead(1));
init motor pins
pinMode (pinMotor1, produktionen);
pinMode (pinMotor2, produktionen);
pinMode (pinMotor3, produktionen);
pinMode (pinMotor4, produktionen);
pinMode (pinSonar1, produktionen);
pinMode (pinSonar2, ingång);
init LED stift
pinMode (pinLed, produktionen);
}
void loop() {
Växla (staten) {
fall R_START:
Motor(M_GO);
Melody();
State = R_DRIVE;
bryta;
fall R_DRIVE:
dist = Pingu();
om (dist < minDistance) {
Motor(M_STOP);
State = R_BLOCK;
}
annars om (random (0, randomChance) == 5) {
RandomTone();
Motor(M_STOP);
Delay(randomStopWait);
Motor (random (M_BACK, M_LEFT2X + 1));
Delay(randomTurnWait);
Motor(M_GO);
}
bryta;
fall R_BLOCK:
Beep();
Motor(M_BACK);
Delay(backDist);
Motor (random (M_RIGHT2X, M_LEFT2X + 1));
Delay(turnDist);
State = R_TURN;
bryta;
fall R_TURN:
dist = Pingu();
om (dist > = maxDistance) {
Motor(M_STOP);
Motor(M_GO);
Beep();
State = R_DRIVE;
}
annat {
räknaren + = 1;
om (counter > = hangoverWait) {
räknare = 0;
Motor(M_STOP);
RandomTone();
State = R_BLOCK;
}
}
bryta;
}
}
Annullera Blink (byte state) {
digitalWrite (pinLed, statligt);
}
void Beep() {
Blink(1);
SpeakTone(2);
SpeakTone(1);
Blink(0);
}
void Melody() {
Blink(1);
för (int jag = 1; jag < 10; i ++) {
SpeakTone(i);
}
Blink(0);
}
void RandomTone() {
Blink(1);
för (int jag = 1; jag < 10; i ++) {
SpeakTone (random(1, 10));
}
Blink(0);
}
void SpeakTone(byte note) {
tonen (pinTone, toneFreq * Obs);
Delay(100);
noTone(pinTone);
}
Annullera Motor (byte state) {
byte m1, m2, m3, m4;
Växla (staten) {
fall M_STOP:
M1 = m2 = m3 = m4 = låg;
bryta;
fall M_GO:
M1 = hög;
m2 = låg;
M3 = hög;
M4 = låg;
bryta;
fall M_BACK:
M1 = låg;
m2 = hög;
M3 = låg;
M4 = hög;
bryta;
fall M_RIGHT:
M1 = hög;
m2 = låg;
M3 = låg;
M4 = låg;
bryta;
fall M_LEFT:
M1 = låg;
m2 = låg;
M3 = hög;
M4 = låg;
bryta;
fall M_RIGHT2X:
M1 = hög;
m2 = låg;
M3 = låg;
M4 = hög;
bryta;
fall M_LEFT2X:
M1 = låg;
m2 = hög;
M3 = hög;
M4 = låg;
bryta;
}
digitalWrite (pinMotor1, m1);
digitalWrite (pinMotor2, m2);
digitalWrite (pinMotor3, m3);
digitalWrite (pinMotor4, m4);
}
int Pingu() {
digitalWrite (pinSonar1, låg);
delayMicroseconds(5);
digitalWrite (pinSonar1, hög);
delayMicroseconds(5);
digitalWrite (pinSonar1, låg);
delayMicroseconds(2);
lång varaktighet = pulseIn (pinSonar2, hög);
tillbaka (längd / 29 /2);
}