Animatronic Wolf head (4 / 5 steg)

Steg 4: kodning

Grunden för min kod kom från en annan instructable:

Motion efter Robot

Jag har mestadels tweaked den för att inkludera en annan servo och har den göra olika rörelser, som andra servo bär ut huvudet rörelsen. Jag vill arbeta med det ytterligare att arbeta med en pan och tilt servo men det är längre ner linjen.

KOD:

#include

Servo yservo; Servo xservo; //servos för x och y

CONST int Lin = 10, Rin = 12, Lout = 11, Rout = 13, yserv = 9, xserv = 8; inställningen sensor stift och servo pin

fastställa variabler för varaktighet / / och avståndet resultera i inches lång Rduration, Lduration, Rinches, Linches;

int ythreshold = 24. Sensor tröskel i tum

int xthreshold = 12; int vinkel = 80. Första vinkeln int xangle = 90.

int counter1 = 0; int counter2 = 0; booleska höger = false;

int maxcounter1 = 45; int maxcounter2 = 3;

booleska debug = false; Seriell kommunikation för debuging. Värdet true för seriell kommunikation.

void setup() {/ / initiera seriell kommunikation: om (debug) {Serial.begin(9600);} yservo.attach(9); //attach servo till pin 9 xservo.attach(8); / / koppla servo till stift 8}

void loop() {//Most i sensor-koden har tagits från David Mellis PING sensor kod //I modifierat den för en 4-pin sensor som oppsed till 3 stift sensorn / / ge en kort låg puls förhand för att säkerställa en ren hög puls: pinMode (Rout, utgång), digitalWrite (Rout, låg); delayMicroseconds(2), digitalWrite (Rout, hög), delayMicroseconds(5), digitalWrite (Rout, låg);

Rduration = pulseIn (Rin, hög); pinMode (Lout, OUTPUT); digitalWrite (Lout, låg); delayMicroseconds(2); digitalWrite (Lout, hög); delayMicroseconds(5); digitalWrite (Lout, låg);

Lduration = pulseIn (Lin, hög);

konvertera tiden till ett avstånd Rinches = microsecondsToInches(Rduration); Linches = microsecondsToInches(Lduration); om (debug) {Serial.print ("vänster:"); Serial.Print(Linches); Serial.println ("i"); Serial.Print ("rätten:"); Serial.Print(Rinches); Serial.println ("i"); } follow(); }

långa microsecondsToInches (lång mikrosekunder) {/ / enligt Parallaxs datablad för PING))), det finns / / 73.746 mikrosekunder per tum (dvs. ljud resor på 1130 fot per / / sekund). Detta ger vägsträcka som ping, utgående / / och retur, så vi delar med 2 att få distansera av hindret. Se: http://www.parallax.com/dl/docs/prod/acc/28015-PI... återvända mikrosekunder / 74 / 2; }

void follow() {om (Linches < = ythreshold || Rinches < = ythreshold) {om (Linches + 2 < Rinches) {vinkel = vinkel - 2;} om (Rinches + 2 < Linches) {vinkel = vinkel + 2;}} om (vinkel > 160) {vinkel = 160;} om (vinkel < 0) {vinkel = 0;} yservo.write(angle);

/ * om (Linches < = xthreshold || Rinches < = xthreshold) {om (Linches + 2 < Rinches) {xangle = xangle - 2;} om (Rinches + 2 < Linches) {xangle = xangle + 2;} om (xangle >} annat {xangle = 90. * / om (Linches < = xthreshold || Rinches < = xthreshold) {om (counter1 == maxcounter1 || counter1 == 0) {counter2 ++; om (counter2 > = maxcounter2) {counter2 = 0; if(counter1 == maxcounter1) counter1--, annat counter1 ++; höger =! rätt;}} annat if(right) {xangle = xangle + 2; counter1--;} annat {xangle = xangle-2; counter1 ++;}} annat xangle = 90.

xservo.write(xangle); }