ArduBot (7 / 8 steg)

Steg 7: Program

Vi kommer att behöva definiera vissa matriser för att lagra de polära och kartesiska koordinater samlades för att representera planritningen av oavsett rum roboten är i. Vi behöver också en annan array att ange fördröjningstiden mellan pingar för att beräkna theta.

CONST int pingPin = 7.
CONST int DIRA = 13. tabellriktningen motor en
CONST int DIRB = 12; tabellriktningen motor b
CONST int PWMA = 11; puls bredd modulering för motor en
CONST int PWMB = 3; Pulse bredd modulering för motor b
int n = 16. datapunkter för scan (); -1
int ping [16]. lagrar PING data för fylla
int theta [16]. lagrar polar data för data punkt n
int x [16]. butiker kartesiska x-värde för data pekar Nilsson
int y [16]. lagrar kartesiska y-värde för data punkt n
int t [16]. används för att beräkna theta; butiker förändring i tid från början av scan för data punkt n
int spindelay = 3200; tid i ms att det tar roboten att snurra ett helt varv
int delta = 3200/16. delta = spindelay/n; tid för en rotation
osignerade långa varaktighet, tum, cm, dir; längd mäter tiden mellan pingar
lång minsafe = 15. minsta säkerhetsavstånd framför robot
int antal; används för att identifiera när roboten är i ett hörn
int omega = 2*3.1416/3.2; vinkelformig hastighet av roterande robot = 2pi/spindelay

void setup() {
pinMode (DIRA, OUTPUT); //Sets riktning av motor A
pinMode (DIRB, OUTPUT); //Sets riktning av motor B
pinMode (PWMA, OUTPUT); //Turns A på och av
pinMode (PWMB, OUTPUT); //Turns B på och av

/ * Om DIRA = låg, Motor A spins FWD
Om DIRB = låg, Motor B spins FWD
*/

initiera seriell kommunikation:
Serial.BEGIN(9600);
}

void (FWD) {//sets motors att vidarebefordra
digitalWrite (DIRA, hög);
digitalWrite (PWMA, hög);
digitalWrite (DIRB, hög);
digitalWrite (PWMB, hög);
}

void REV () {//sets motors att vända
digitalWrite (DIRA, låg);
digitalWrite (PWMA, hög);
digitalWrite (DIRB, låg);
digitalWrite (PWMB, hög);
}

Annullera rätt () {//turns rätta på plats
digitalWrite (DIRA, hög);
digitalWrite (PWMA, hög);
digitalWrite (DIRB, låg);
digitalWrite (PWMB, hög);
}

annullera (vänster) {//turns kvar på plats
digitalWrite (DIRA, låg);
digitalWrite (PWMA, hög);
digitalWrite (DIRB, hög);
digitalWrite (PWMB, hög);
}

void STOP () {
digitalWrite (PWMA, låg);
digitalWrite (PWMB, låg);
}

osignerade långa PING() {
pinMode (pingPin, produktionen); Göra Pingpin att mata
digitalWrite (pingPin, låg); Skicka en låg puls
delayMicroseconds(2); vänta två mikrosekunder
digitalWrite (pingPin, hög); Skicka en hög puls
delayMicroseconds(5); vänta i 5 sekunder för mikro
digitalWrite (pingPin, låg); Skicka en låg puls
pinMode(pingPin,INPUT); Växla Pingpin att mata in
längd = pulseIn (pingPin, hög); lyssna efter echo

/ * Konvertera mikro sekunder till Inches
*/

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

returnera cm;

}

lång microsecondsToInches(long microseconds) {
återvända mikrosekunder / 74 / 2;
}
lång microsecondsToCentimeters(long microseconds) {
återvända mikrosekunder / 29 / 2;
}

osignerade långa leftping () {//records avstånd till vänster om roboten och returnerar det till mitten
int kvar;
(VÄNSTER);
fördröjning (420);
STOPPA ();
Delay(10);

Serial.Print ("vänster:");
vänster = PING ();
fördröjning (420);

(RÄTT);
fördröjning (420);
STOPPA ();
returnera vänster;
}

osignerade långa rightping () {//records avstånd till höger om roboten och returnerar det till mitten
int rätt;
(RÄTT);
fördröjning (420);
STOPPA ();
fördröjning (10).

Serial.Print ("rätten:");
höger = PING ();
fördröjning (420);

(VÄNSTER);
fördröjning (420);
STOP();
returnera rätt;
}

osignerade långa forwardping () {//essentiallhy PING
int mitten;
STOPPA ();
fördröjning (10).
Serial.Print ("mellersta:");
mittersta = PING ();
returnera mellersta;

}

osignerade långa Välj () {
leftping();
forwardping();
rightping();
IF(leftping > rightping)
om (leftping > forwardping)
(VÄNSTER);
fördröjning (420);
STOP();
dir = forwardping ();
om (rightping > leftping)
om (rightping > forwardping) {
(RÄTT);
fördröjning (420);
STOP();
dir = forwardping ();
} annat
STOPPA ();
dir = forwardping ();

}

void scan () {
delta = spindelay/n;
int c;
int i;
int t;

för (t = 1; t < 17; t ++) {
theta [t] = t/8 * 3.1416;
}

för (c = 0; c < 16; c ++) {
(VÄNSTER);
Delay(delta);
STOPPA ();
fördröjning (200);

ping [c] = PING ();

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

för (jag = 0; jag < 16; i ++) {
Serial.println(theta[i]);
}
}

Ogiltig loop () {
Scan();
Delay(10000);
}