ArduBot (7 / 8 stap)

Stap 7: programma

We zullen moeten om te bepalen sommige arrays slaan de polaire en cartesiaanse coördinaten verzameld te vertegenwoordigen het vloerplan voor welke kamer van de robot is in. Ook zullen we een andere array om aan te geven van de vertragingstijd tussen pings om te berekenen theta.

Const int pingPin = 7;
Const int DIRA = 13;   Hiermee stelt u de richting van de motor een
Const int DIRB = 12;   Hiermee stelt u de richting van motor b
Const int PWMA = 11;   puls breedte modulatie voor motor een
Const int PWMB = 3;    puls breedte modulatie voor motor b
int n = 16;            aantal gegevenspunten opgenomen voor scan (); -1
int ping [16];          PING-gegevens worden opgeslagen voor voltooien
int theta [16];         de polar gegevens winkels voor gegevens punt n
int x [16];             winkels Cartesiaanse x-waarde voor gegevens punt n
int y [16];             Cartesiaanse y-waarde voor gegevens punt n wordt opgeslagen
int t [16];             gebruikt om te berekenen theta; winkels veranderen in de tijd vanaf begin van scan voor gegevens punt n
int spindelay = 3200;  tijd in ms dat robot duurt te draaien een volledige omwenteling
int delta = 3200/16;             Delta = spindelay/n; hoeveelheid tijd voor één omwenteling
niet-ondertekende lange duur, inch, cm, dir;   duur meet de tijd tussen PINGs
lange minsafe = 15;                         minimale veiligheidsafstand tegenover robot
int count;                                 gebruikt om te helpen identificeren wanneer de robot in een hoek is
int omega = 2*3.1416/3.2;             hoeksnelheid van het omzetten van robot = 2pi/spindelay

VOID Setup {}
pinMode (DIRA, OUTPUT); -//Sets richting van motor A
pinMode (DIRB, OUTPUT); -//Sets richting van motor B
pinMode (PWMA, OUTPUT); //Turns A in- en uitschakelen
pinMode (PWMB, OUTPUT); //Turns B in- en uitschakelen

/ * Als DIRA = laag, Motor A draaiingen FWD
Als DIRB = laag, Motor B draaiingen FWD
*/

seriële communicatiepoorten te initialiseren:
Serial.begin(9600);
}

VOID FWD () {//sets motoren toekomen
digitalWrite (DIRA, hoge);
digitalWrite (PWMA, hoge);
digitalWrite (DIRB, hoge);
digitalWrite (PWMB, hoge);
}

leegte (REV) {//sets motoren om te keren
digitalWrite (DIRA, laag);
digitalWrite (PWMA, hoge);
digitalWrite (DIRB, laag);
digitalWrite (PWMB, hoge);
}

VOID juiste () {//turns recht op plek
digitalWrite (DIRA, hoge);
digitalWrite (PWMA, hoge);
digitalWrite (DIRB, laag);
digitalWrite (PWMB, hoge);
}

VOID links () {//turns liet ter plaatse
digitalWrite (DIRA, laag);
digitalWrite (PWMA, hoge);
digitalWrite (DIRB, hoge);
digitalWrite (PWMB, hoge);
}

VOID STOP () {}
digitalWrite (PWMA, laag);
digitalWrite (PWMB, laag);
}

unsigned long PING() {}
pinMode (pingPin, OUTPUT); Maken van de Pingpin voor de uitvoer van
digitalWrite (pingPin, laag); Stuur een lage puls
delayMicroseconds(2); Wacht twee microseconden
digitalWrite (pingPin, hoge); Stuur een hoge pols
delayMicroseconds(5); wacht 5 micro seconden
digitalWrite (pingPin, laag); Stuur een lage puls
pinMode(pingPin,INPUT); Schakel de Pingpin in te voeren
duur = pulseIn (pingPin, hoge); luisteren voor de echo

/ * Micro seconden omzetten in Inches
*/

duim = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.Print (cm);
Serial.Print ("CM");
Serial.println ();

retourneren cm;

}

lange microsecondsToInches(long microseconds) {}
retourneren van microseconden / 74 / 2;
}
lange microsecondsToCentimeters(long microseconds) {}
retourneren van microseconden / 29 / 2;
}

unsigned long leftping () {//records afstand aan de linkerkant van de robot en wordt hersteld naar het midden
int links;
LINKS ();
vertraging (420);
STOPPEN ();
delay(10);

Serial.Print ("links:");
links = PING ();
vertraging (420);

RECHTS ();
vertraging (420);
STOPPEN ();
terug links;
}

unsigned long rightping () {//records afstand tot het recht van de robot en wordt hersteld naar het midden
int recht;
RECHTS ();
vertraging (420);
STOPPEN ();
vertraging (10);

Serial.Print ("recht:");
rechts = PING ();
vertraging (420);

LINKS ();
vertraging (420);
STOP();
terug gelijk te hebben;
}

unsigned long forwardping () {//essentiallhy PING
int midden;
STOPPEN ();
vertraging (10);
Serial.Print ("Midden:");
Midden = PING ();
keren midden;

}

niet-ondertekende lang kiezen () {}
leftping();
forwardping();
rightping();
if(leftping > rightping)
Als (leftping > forwardping)
LINKS ();
vertraging (420);
STOP();
dir = forwardping ();
Als (rightping > leftping)
Als (rightping > forwardping) {}
RECHTS ();
vertraging (420);
STOP();
dir = forwardping ();
} else
STOPPEN ();
dir = forwardping ();

}

VOID scan () {}
Delta = spindelay/n;
int c;
int i;
int t;

voor (t = 1; t < 17; t ++) {}
Theta [t] = t/8 * 3.1416;
}


voor (c = 0; c < 16; c ++) {}
LINKS ();
delay(Delta);
STOPPEN ();
vertraging (200);

ping [c] = PING ();

x [c] = ping[c]*sin(theta[c]);
y [c] = ping[c]*cos(theta[c]);
}

voor (ik = 0; ik < 16; ik ++) {}
Serial.println(theta[i]);
}
}

void loop () {}
Scan();
delay(10000);
}