Kaninöron på en mössa (6 / 6 steg)
Steg 6: Sista steget
Vi kombinerat både servo kod och accelerometer kod. Från värdena hittade vi tidigare, vi lagt till den i koden för att göra motorerna sväng vid rätt värde.Värdena är inte mycket exakt. Vi fann det svårt att peka ut ett exakt värde, det är fortfarande inte en perfekt uppsättning "öron" som det inte vända varje gång vi slår våra huvuden.
Slutliga kod:
===================================================
#include < Servo.h >
int i;
CONST int xPin = 0;
CONST int yPin = 1;
CONST int zPin = 2;
int minVal = 265;
int maxVal = 402;
dubbel x;
dubbla y;
dubbel z;
Servo myservoLeft;
Rutinen installationsprogrammet körs en gång när du trycker på reset:
void setup() {
Serial.BEGIN(9600);
myservoLeft.attach(9);
}
loop rutinen körs för evigt om och om igen:
void loop() {
int xRead = analogRead(xPin);
int yRead = analogRead(yPin);
int zRead = analogRead(zPin);
konvertera Läs värden till grader -90 till 90 - behövs för ARCTAN2
int xAng = karta (xRead, minVal, maxVal, -90, 90);
int yAng = karta (yRead, minVal, maxVal, -90, 90);
int zAng = karta (zRead, minVal, maxVal, -90, 90);
Caculate 360deg värden som så: ARCTAN2 (-yAng, -zAng)
ARCTAN2 utgångar värdet av - π π (radianer)
Vi sedan konvertera radianer till grader
x = RAD_TO_DEG * (ARCTAN2 (-yAng, -zAng) + PI);
y = RAD_TO_DEG * (ARCTAN2 (-xAng, - zAng) + PI);
z = RAD_TO_DEG * (ARCTAN2 (-yAng, -xAng) + PI);
Serial.Print ("x:");
Serial.Print(x);
Serial.Print("| y: ");
Serial.Print(y);
Serial.Print("| z: ");
Serial.println(z);
Delay(100);
om (z > = 76) {//right
Delay(100);
för (jag = 0; jag < 90; i += 10)
{
myservoLeft.write(i);
Delay(100);
}
för (jag = 90; jag > 0; i-= 10)
{
myservoLeft.write(i);
Delay(100);
}
}
om ((z > = 50) & & (z < = 75)) {//middle
myservoLeft.write(60);
Delay(100);
}
om (z < = 49) {//left
Delay(100);
för (jag = 90; jag < 180; i += 10)
{
myservoLeft.write(i);
Delay(100);
}
för (jag = 180, jag > 90; i-= 10)
{
myservoLeft.write(i);
Delay(100);
}
}
}
===================================================
Det är gjort!