Hacka en Rumble Robot

Målet är att skapa en sökare robot. Jag satte två ping sensorer på detta. Vänster kommer ping, sedan höger, sedan vänster, sedan höger. Dessa värden lagras i en matris. Detta ger 4 värden att använda för analys. Målet är att upptäcka rörelse. Jag hittade att det finns viss variation i ping mätningarna och dessutom sensorer är inte exakt i linje, så det finns ett värde för tolerans. Om det finns en värdeförändring sedan rörelsen uppstod och roboten kommer att gå mot rörelsen.

Här är skissen. Jag har så att det kommer att cirkla och söka. Men jag har inte en robot att jaga, som ska komma nästa vecka. Ledsen koden är inte super ren. Men det är ett pågående arbete.

++++++++++++++++++++++++++++++++++++++++

Detta är att testa hur man gör funktioner fungerar. Bra.
Lysdioder blinkar alldeles utmärkt.
Två pingers ping och returnerar avståndet i cm.
Detta har en skapande minnesmatrisen.
Nu att använda den mätning av avstånd och har beslut baserat på det.

int vcount = 0;
int vSonarTime = 0; Detta kommer värdet av tidpunkten för sonar ping, vilket är avståndet
int arrSonarPin [] = {
4,2}. detta lämnas sedan rätt sonar
int svar = 0; variabel för något
int sonarLeft = 0;
int sonarRight = 0;
int vTolerate = 10; Detta är mikrosekunder för fel i ping som blir ignorerade
int arrScanz [96]. Hit går beslut om comparePing beslut
int arrScanzEnd = 95. slutet Elementnummer för array arrScanz
int vScanz = 0; globala var används till rekordresultat för scan analys
int vMovementRight = 3; initiera med 3 för att se om det finns ett fel senare
int vMovementLeft = 3; initiera med 3 för att se om det finns ett fel senare
int vCloserRight = 3;
int vCloserLeft = 3;
int vStayPut = 3;

int aPoints [48]. vill göra array poäng och avstånd
int PointsEnd = 47;
int vPoint = 0; Detta är init skulle användas som ökande matriselementen där avstånd få lagras.
Eftersom detta är globala vi kan usitilize det i funktioner. Men bara StorePoint() kommer att ändra vPoint
int i; för att räkna för loopar
int LedA = 13. ombord
int LedB = 12. röd
int LedC = 9; röd
int ledde = 8. grön
int greenLED = 8; lättare att minnas

int arrLed [] = {LedA, LedB, LedC, ledde};
int arrLedEnd = 3; array arrLed har 4 medlemmar, så sista är #3

int motorpinright = 10; PIN-koden för rätt motor röd
int motorpinleft = 11; PIN-koden för vänster motor framåt gul
int motorpinrevright = 5; PIN-koden för rätt motor omvänd svart
int motorpinrevleft = 6; PIN-koden för rätt motor framåt gröna
int vMoveTime = 200. Detta är var för den tiden flyttade
int vDelayTime = 250;

void setup() {
Serial.BEGIN(9600);
pinMode(LedA,OUTPUT);
pinMode(LedB,OUTPUT);
pinMode(LedC,OUTPUT);
pinMode(LedD,OUTPUT);
pinMode (motorpinright, produktionen); Motor enheter---
pinMode (motorpinleft, produktionen); //------------------------
pinMode (motorpinrevright, produktionen); //------------------------
pinMode (motorpinrevleft, produktionen); //------------------------
för (jag = 0; jag < PointsEnd; i ++) {/ / måste initiera arrayen avstånd aPoints]
aPoints [i] = 0;
} / / End den for-loop
för (jag = 0; jag < arrScanzEnd; i ++) {/ / måste initiera arrayen arrScanz]
arrScanz [i] = 0;
} / / End den for-loop

} / / end ogiltig loop

void loop() {
digitalWrite(13,LOW); lågt att starta
Serial.println ("Start i huvudloop");

blinxAll(arrLedEnd); Anropa fancy Ass funktion. Inte tilldela det en yttre värde

ShowArrayContents();

BlinxOne(greenLED,100,6);

DetectMovement(); Gör ping i 3 riktningar

DecideWhichWay(); om ingen rörelse upptäcks då flytta och söka
slut beslutet att flytta

vMoveTime = 200.
Serial.Print ("strax utanför sonarx. vSonarTime = ");
Serial.println(vSonarTime);
Serial.println ("av funktionen blinxAll");
arrLedEnd = 3;

int vMovementRight = 3; initiera med 3 för att se om det finns ett fel senare
int vMovementLeft = 3; initiera med 3 för att se om det finns ett fel senare
int vStayPut = 3;

} / / end slingan
//+++++++++++++++++++++++++++++++++++++++++++
*** Förklara funktioner ***
//+++++++++++++++++++++++++++++++++++++++++++
int sonarx (int vpin) {/ / denna funktion kommer att kalla en sonar modul för att pinga och returnerar ett värde av ping
int x = 5;
int vPingDist = 0; ingen ängre behövs
int vDuration; , tum, cm;
x = vpin; Detta är den pin-kod
Serial.Print ("i sonarx och värde vpin är:");
Serial.Print(x);
pinMode (vpin, produktionen);
digitalWrite (vpin, låg);
delayMicroseconds(2);
digitalWrite (vpin, hög);
delayMicroseconds(5);
digitalWrite (vpin, låg);

Samma PIN-koden används för att läsa signalen från PING))): en hög
puls vars längd är tid (i mikrosekunder) från den sändande
ping till mottagning av dess eko av ett objekt.
pinMode (vpin, ingång);
vDuration = pulseIn (vpin, hög);

cm = microsecondsToCentimeters(vDuration);

Serial.Print ("vDuration:");
Serial.println(vDuration);

återvända vDuration;
}
//+++++++++++++++++++++++++++++++++++++++++++

int microsecondsToCentimeters (int mikrosekunder)
{
Ljudets hastighet är 340 m/s eller 29 mikrosekunder per centimeter.
Ping reser ut och tillbaka, så för att hitta distansera av den
objekt vi tar hälften av den tillryggalagda.
återvända mikrosekunder / 29 / 2;
}

//+++++++++++++++++++++++++++++++++++++++++++
void blinxAll (int arrLedEndi) {
int ivar1;

Serial.Print ("i funktionen blinxAll värde är:");
Serial.println(arrLedEndi);
Delay(100);

för (int jag = 0; jag < = arrLedEndi; i ++) {
ivar1 = arrLed [i]; / * jag är platsen i matrisen, och detta ändrar varje slinga.
Värdet av arrayen tilldelas. Detta är den pin-kod av LED
det blir outputted.* /
Serial.Print ("värdet på ivar1 är:");
Serial.Print(ivar1);
digitalWrite(ivar1,HIGH);
Delay(100);
digitalWrite(ivar1,LOW);
Delay(100);
} / / End för cykeln genom array

Serial.println ("slutet av funktionen blinxAll");

} / / End funktion
//+++++++++++++++++++++++++++++++++++++++++++
void BlinxOne (int vLED, int vDelay, int numBlinks) {/ / vLED har val från ovan. vDelay är
int vLED;
vDelay = 100;
Serial.Print ("i BlinxOne funktion, värdet är:");
Serial.println(vLED);
Delay(vDelay);
för (int jag = 0; jag < = numBlinks; i ++) {
digitalWrite(vLED,HIGH);
Delay(vDelay);
digitalWrite(vLED,LOW);
Delay(vDelay);
} //End av for-loop
Serial.println ("slutet av funktionen BlinxOne");

} / / End funktionen BlinxOne
//+++++++++++++++++++++++++++++++++++++++++++
void MoveAhead() {
int ivar1 = skyldiga;
digitalWrite(ivar1,HIGH);

int v2MoveTime = vMoveTime;
Serial.Print ("vMoveTime MoveAhead är värt:");
Serial.println(v2MoveTime);
Detta kommer att fortsätta bot för en tid skickas till funktion
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor
analogWrite (motorpinleft, 255); stoppa vänster motor
analogWrite (motorpinright, 255); stoppa rätt motor
Delay(vMoveTime);
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor

digitalWrite(ivar1,LOW); stänga av grönt ljus
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}
//+++++++++++++++++++++++++++++++++++++++++++
void MoveBack() {
Serial.println ("flytta tillbaka OK!");
int v2MoveTime = vMoveTime;
Serial.Print ("vMoveTime är värt:");
Serial.println(v2MoveTime);

analogWrite (motorpinrevleft, 255); stoppa vänster motor
analogWrite (motorpinrevright, 255); stoppa rätt motor
Delay(vMoveTime);
analogWrite (motorpinrevleft, 0); stoppa vänster motor
analogWrite (motorpinrevright, 0); stoppa rätt motor
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}
//+++++++++++++++++++++++++++++++++++++++++++
void TurnRight90() {
int v2MoveTime = vMoveTime;
Serial.Print ("vMoveTime TurnRight90 är värt:");
Serial.println(v2MoveTime);

analogWrite (motorpinrevleft, 255); stoppa vänster motor
analogWrite (motorpinright, 255); stoppa rätt motor
Delay(vMoveTime);
analogWrite (motorpinrevleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}
//+++++++++++++++++++++++++++++++++++++++++++

void TurnLeft45() {
int vMoveTime2 = (vMoveTime/2);
Serial.Print ("vMoveTime TurnLeft45 är värt:");
Serial.println(vMoveTime2);

analogWrite (motorpinleft, 0); Starta vänster motor
Delay(vMoveTime2); Detta är hälften att göra turn
analogWrite (motorpinright, 255); Starta rätt motor
Delay(vMoveTime);
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}
//+++++++++++++++++++++++++++++++++++++++++++

void TurnRight45() {
int vMoveTime2 = (vMoveTime/2);
Serial.Print ("vMoveTime TurnRight45 är värt:");
Serial.println(vMoveTime2);

analogWrite (motorpinleft, 255); Starta vänster motor
Delay(vMoveTime);
analogWrite (motorpinright, 0); Starta rätt motor
Delay(vMoveTime2); Detta är hälften att göra turn
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}

//+++++++++++++++++++++++++++++++++++++++++++
void TurnLeft90() {
int v2MoveTime = vMoveTime;
Serial.Print ("vMoveTime TurnLeft90 är värt:");
Serial.println(v2MoveTime);

analogWrite (motorpinleft, 255); stoppa vänster motor
analogWrite (motorpinrevright, 255); stoppa rätt motor
Delay(vMoveTime);
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinrevright, 0); stoppa rätt motor
vMoveTime = 0; förhindra oändliga loopar ++ runaways som är
}
//+++++++++++++++++++++++++++++++++++++++++++
void AllStop() {
analogWrite (motorpinleft, 0); stoppa vänster motor
analogWrite (motorpinright, 0); stoppa rätt motor
analogWrite (motorpinrevright, 0); stoppa rätt rev motor
analogWrite (motorpinrevleft, 0); stoppa vänster rev motor

}
//+++++++++++++++++++++++++++++++++++++++++++
funktion StorePoint lagrar poängvärden i en matris
void StorePoint () {
om (vPoint < = PointsEnd) {
aPoints [vPoint] = sonarx(svar); gör detta en funktion någon gång
vPoint ++; Increment vPoint variabel
}
annat {
vPoint = 0;
aPoints [vPoint] = sonarx(svar); gör detta en funktion någon gång
}
}
//+++++++++++++++++++++++++++++++++++++++++++
funktion StoreScanz
void StoreScanz (int vfScan) {
om (vScanz < = arrScanzEnd) {
arrScanz [vScanz] = vfScan;
vScanz ++; öka vScanz räknaren
} annat
{
vScanz = 0;
arrScanz [vScanz] = vfScan;
}
}
//+++++++++++++++++++++++++++++++++++++++++++
void get4pings() {
Få 2 uppsättningar av vänster och höger värden.
för (int f = 0; f < = 1; f ++) {
Göra funktionen ping, första vänster sedan höger.
för (int b = 0; b < = 1; b ++) {
svar = arrSonarPin [b]; Välj vänster sedan höger ping pin: 4 som är kvar nu
vSonarTime = sonarx(svar); Anropa funktionen sonarx för att mäta avstånd då ping.
tilldela vSonarTime detta värde är sannolikt värdelös
StorePoint(); sonarx funktionen anropas funktionen vPoint är en Global Var

det nuvarande avståndet lagras i ett array-element
IF(b==0) {
Serial.Print ("sonarx tilldelats vänster Hand vPoint värde =");
Serial.println(vPoint); vPoint är den globala variabeln att använda.
}
annat {
Serial.Print ("sonarx högra vPoint värde anges =");
Serial.println(vPoint); vPoint är den globala variabeln att använda.
} / / end if annat
} / / slutet av for-loop. Två värden som tilldelas till minne array. 2 uppsättningar av vänster och höger värden.
} / / End den for-slingan 4 värden hämtas
} //End funktion get4 värden
//++++++++++++++++++++++++++++++++++++++++++++
En funktion för att skriva ut array innehållet.

void ShowArrayContents() {
Detta används för diagnostik och felsökning.
för (jag = 0; jag < = PointsEnd; jag ++) / / skriva ut utbudet av uppmätta avstånd
{
Serial.Print ("jag =");
Serial.Print(i);
Serial.Print ("aPoints värdet är =");
Serial.Print(aPoints[i]);
Serial.println();
} / / End den for-loop.
} / / End funktionen ShowArrayContents
//+++++++++++++++++++++++++++++++++++++++++++++
int comparePings() / / om vi vet positionen för det sista array element skrivit, kan vi jämföra fotboll
{/ / start av funktion
int s = 1; Detta kommer bara gå igenom en gång.
int t = 2;
int vComparePingsLD = abs ((aPoints[vPoint-1]) - (aPoints[vPoint-3])); Detta visar "D'epth rörelse
int vComparePingsRD = abs ((aPoints[vPoint-0]) - (aPoints[vPoint-2]));
int vComparePingsRC = abs ((aPoints[vPoint-0]) - (aPoints[vPoint-1])); Detta visar vilken sida har ' C'losest ping
int vComparePingsLC = abs ((aPoints[vPoint-3]) - (aPoints[vPoint-4]));

Serial.Print ("vComparePingsLD värde är:");
Serial.println (vComparePingsLD);
Serial.Print ("vComparePingsRD värde är:");
Serial.println (vComparePingsRD);
Serial.Print ("vComparePingsRC värde är:");
Serial.println (vComparePingsRC);
Serial.Print ("vComparePingsLC värde är:");
Serial.println (vComparePingsLC);

medan (s < t) {/ / resaon att titta på där är ett tag loop fungerade endast en gång.
om ((aPoints [vPoint-1] == aPoints[vPoint-3]) || (vComparePingsLD < = vTolerate))
{//if detta är eqivalent så finns det ingen rörelse mot eller bort från pinger
arrScanz [vScanz] = 0; ingen rörelse
Serial.Print ("vänster sida djup poäng är likvärdiga:");
Serial.Print (aPoints[vPoint-3]);
Serial.Print ("");
Serial.println (aPoints[vPoint-1]);
vScanz ++;
vScanz ++;
}
annat {
vMovementLeft = 1;
arrScanz [vScanz] = 1; Det fanns rörlighet
vScanz ++;
vScanz ++;
}

om (((aPoints[vPoint]) == (aPoints[vPoint-2])) || (vComparePingsRD < = vTolerate))
{//if detta är eqivalent så finns det ingen rörelse mot eller bort från pinger
arrScanz [vScanz] = 0; ingen rörelse
Serial.Print ("höger sida djup poäng är likvärdiga:");
Serial.Print (aPoints[vPoint-2]);
Serial.Print ("");
Serial.println (aPoints[vPoint]);
vScanz ++;
}
annat {
vMovementRight = 1;
arrScanz [vScanz] = 1; Det fanns rörlighet
vScanz ++;
vScanz ++;
}

om (((aPoints[vPoint]) == (aPoints[vPoint-1])) || (vComparePingsRC < = vTolerate))
{//check att se vilken väg har närmare objektet
Serial.Print ("den nyaste två distanserar av vänster och höger är kvar:");
Serial.Print (aPoints[vPoint-1]);
Serial.Print ("sedan höger:");
Serial.println (aPoints[vPoint]);
vScanz ++;
}
annat {
om ((aPoints [vPoint - 1]) < (aPoints[vPoint])) {/ / starta om
vCloserLeft = 1;
vCloserRight = 0;
} / / end om
annat
{
vCloserLeft = 0;
vCloserRight = 1;
arrScanz [vScanz] = 1; Det fanns rörlighet
vScanz ++;
vScanz ++;
} / / end annars
} / / end annars

om (((aPoints[vPoint-3]) == (aPoints[vPoint-4])) || (vComparePingsLC < = vTolerate))
{//check att se vilken väg har närmare objektet
Tror inte jag behöver detta för närvarande. Hmmmm
Serial.Print ("den äldre två distanserar av vänster och höger är kvar:");
Serial.Print (aPoints[vPoint-4]);
Serial.Print ("sedan höger:");
Serial.println (aPoints[vPoint-3]);
vScanz ++;
}
annat
{
arrScanz [vScanz] = 1; Det fanns rörlighet
vScanz ++;
}
slut om
s + +; öka tag loop
} //end den medan funktion
Delay(500); Att låta mig läsa följetong bildskärmen. Ta bort detta senare.
} / / end av funktion
//++++++++++++++++++++++++++++++++++++++++++++
void DetectMovement() {//Does ping i 3 direcctions
gå left45 ping sedan gå right45 (som är center) sedan right45 och besluta om det finns rörelse.
int s = 0;
int t = 2; Detta kan justeras för fler skanningar
int fDelay = 100; internt nummer för förseningen mellan skanningar
vMoveTime = 500. Detta är en global variabel som är förändrats mycket
TurnLeft45(); Detta är den inledande godtycklig åtgärd. Gör sortering Skanna data lättare.
Serial.println (tur left45, hämta värden");

medan (s < t) {
get4pings();
Delay(fDelay);
comparePings(); När detta kallas kommer att sedan jämförelser bestämma rörelsen eller ingen rörelse

s = s + +;
TurnRight45();
Serial.println (tur right45, hämta värden");
} / / end medan
s = 0; återställa variabeln funktion

Jämför de insamlade värdena.

Delay(fDelay);

} / / End DetectMovement funktion
//++++++++++++++++++++++++++++++++++++++++++++
void DecideWhichWay() {

om (aPoints [vPoint] > 200) {
vMoveTime = (aPoints[vPoint]/10); ger en skala använda baserat på avstånd
}
annat {/ / för nära så tur bara
vMovementLeft = 0;
vMovementRight = 0;
}

om (vMovementLeft == 1) {
TurnLeft45 ();
MoveAhead ();
} / / end om
annat if (vMovementRight == 1) {
TurnRight45 ();
MoveAhead ();
} //
annat if (vStayPut == 0) {
vMoveTime = vMoveTime;
MoveAhead();
} //

annat {/ / bo på plats, men rotera höger 90 om du vill skanna nytt område.
TurnRight90();
AllStop();
} / / end annars

} / / End funktionen DecideWhichWay
//+++++++++++++++++++++++++++++++++++++++++++++