Stap 6: Laatste stap
We combineren zowel de servo code en de code van de versnellingsmeter. Uit de waarden vonden we eerder dat we toegevoegd aan de code om de motoren draaien op de juiste waarde.De waarden zijn niet erg nauwkeurig. We vonden het moeilijk om een exacte waarde lokaliseren, nog niet een perfecte set "oren" als het niet draaien everytime we onze hoofden draaien.
Definitieve code:
===================================================
#include < Servo.h >
int i;
Const int xPin = 0;
Const int yPin = 1;
Const int zPin = 2;
int minVal = 265;
int maxVal = 402;
dubbele x;
dubbele y;
dubbele z;
Servo myservoLeft;
de setup-routine wordt uitgevoerd zodra wanneer u drukt op reset:
VOID Setup {}
Serial.begin(9600);
myservoLeft.attach(9);
}
de routine van de lus wordt uitgevoerd over en weer forever:
void loop {}
int xRead = analogRead(xPin);
int yRead = analogRead(yPin);
int zRead = analogRead(zPin);
Lees waarden tot-90 90 - graden die nodig zijn voor atan2 converteren
int xAng = kaart (xRead, minVal, maxVal,-90, 90);
int yAng = kaart (yRead, minVal, maxVal,-90, 90);
int zAng = kaart (zRead, minVal, maxVal,-90, 90);
Caculate 360deg waarden als volgt: atan2 (-yAng, -zAng)
ATAN2 uitgangen de waarde van - pi tot pi (radialen)
Wij zijn dan de radialen converteren naar graden
x = RAD_TO_DEG * (atan2 (-yAng, -zAng) + PI);
y = RAD_TO_DEG * (atan2 (-xAng, - zAng) + PI);
z = RAD_TO_DEG * (atan2 (-yAng, -xAng) + PI);
Serial.Print ("x:");
Serial.Print(x);
Serial.Print("| y: ");
Serial.Print(y);
Serial.Print("| z: ");
Serial.println(z);
delay(100);
Als (z > = 76) {//right
delay(100);
voor (ik = 0; ik < 90; ik += 10)
{
myservoLeft.write(i);
delay(100);
}
voor (ik = 90; ik > 0; ik-= 10)
{
myservoLeft.write(i);
delay(100);
}
}
Als ((z > = 50) & & (z < = 75)) {//middle
myservoLeft.write(60);
delay(100);
}
Als (z < = 49) {//left
delay(100);
voor (ik = 90; ik < 180; ik += 10)
{
myservoLeft.write(i);
delay(100);
}
voor (ik = 180; ik > 90; ik-= 10)
{
myservoLeft.write(i);
delay(100);
}
}
}
===================================================
Het gedaan!