Stap 4: codering
De basis van mijn code kwam vanuit een ander instructable:
Beweging na Robot
Ik heb meestal getweaked te omvatten een ander servo en hebben het doen van verschillende bewegingen, zoals de andere servo verricht het hoofd verkeer. Ik wil werken met het verder te werken met een pan en tilt servo maar dat is verder langs de lijn.
CODE:
#include
Servo yservo; Servo xservo; //servos voor x en y
Const int Lin = 10, Rin = 12, Lout = 11, aftocht = 13, yserv = 9, xserv = 8; instelling sensor pinnen en servo pin
stellen variabelen voor duur / / en de afstand die resulteren in inch lange Rduration, Lduration, Rinches, trouwfeesten;
int ythreshold = 24; Sensor drempel in inch
int xthreshold = 12; int hoek = 80; Eerste hoek int xangle = 90;
int counter1 = 0; int counter2 = 0; Booleaanse rechts = false;
int maxcounter1 = 45; int maxcounter2 = 3;
Booleaanse debug = false; Seriële communicatie voor debugging. Is ingesteld op true voor seriële communicatie.
void setup {/ / seriële communicatiepoorten te initialiseren: als (debug) {Serial.begin(9600)}; yservo.attach(9); //attach servo naar pin 9 xservo.attach(8); / / servo koppelen aan pin 8}
void loop {//Most van het Wetboek van de sensor is genomen van David Mellis van PING sensor code //I gewijzigd voor een 4-pin sensor als oppsed aan de 3-pin sensor / / geven een korte lage puls vooraf om een schoon hoog puls: pinMode (aftocht, OUTPUT); digitalWrite (aftocht, laag); delayMicroseconds(2); delayMicroseconds(5); digitalWrite (aftocht, hoog) en digitalWrite (aftocht, laag);
Rduration = pulseIn (Rin, hoge); pinMode (Lout, uitvoer); digitalWrite (Lout, laag); delayMicroseconds(2); digitalWrite (Lout, hoge); delayMicroseconds(5); digitalWrite (Lout, laag);
Lduration = pulseIn (Lin, hoge);
de tijd omzetten in een afstand Rinches = microsecondsToInches(Rduration); Trouwfeesten = microsecondsToInches(Lduration); Als (debug) {Serial.print ("links:"); Serial.Print(Linches); Serial.println ("in"); Serial.Print ("recht:"); Serial.Print(Rinches); Serial.println ("in"); } follow(); }
lang microsecondsToInches (lange microseconden) {/ / volgens de Parallax gegevensblad voor de PING))), er zijn / / 73.746 microseconden per inch (dwz geluid reizen aan 1130 voeten per / / seconde). Dit geeft de afstand die is afgelegd door de ping, uitgaande / / en terugkeer, dus wij door 2 delen te krijgen van de afstand van het obstakel. Zie: http://www.parallax.com/dl/docs/prod/acc/28015-PI... terug microseconden / 74 / 2; }
VOID follow() {als (trouwfeesten < = ythreshold || Rinches < ythreshold =) {als (trouwfeesten + 2 < Rinches) {hoek = hoek - 2;} als (Rinches + 2 < trouwfeesten) {hoek = hoek + 2;}} als (hoek > 160) {hoek = 160;} als (hoek < 0) {hoek = 0;} yservo.write(angle);
/ * Als (trouwfeesten < = xthreshold || Rinches < xthreshold =) {als (trouwfeesten + 2 < Rinches) {xangle = xangle - 2;} als (Rinches + 2 < trouwfeesten) {xangle = xangle + 2;} als (xangle >} else {xangle = 90; * / als (trouwfeesten < = xthreshold || Rinches < = xthreshold) {als (counter1 == maxcounter1 || counter1 == 0) {counter2 ++; als (counter2 > = maxcounter2) {counter2 = 0; if(counter1 == maxcounter1) counter1--; anders counter1 ++; rechts =! rechts;}} anders if(right) {xangle = xangle + 2; counter1--;} else {xangle = xangle-2; counter1 ++;}} anders xangle = 90;
xservo.write(xangle); }
