4 servo drive CellBot die op afstand kan worden gecontroleerd. (8 / 8 stap)

Stap 8: De Arduino Code aangepast aan mijn gebruik.

#include < Servo.h >
#include < EEPROM.h >

Servo myservoFR;
Servo myservoFL;
Servo myservoRR;
Servo myservoRL;
Servo myservoHead;

#define schrijven 20
#define InferFPin 0 //Front afgeleid Sensor analoge pin
#define InferBPin 1 //Back afgeleid Sensor analoge pin
#define SonarPin 2 //sonar analoge pin
#define LeftEarPin 3 //left oor analoge pin
#define RightEarPin 4 //right oor analoge pin /
#define VoltPin 5 //analog pin 5 gebruikt voor de detectie van de VoltageCheck.
#define LEDBlue 5
#define LEDGreen 6
#define LEDRed 7
#define servoPinHead 8 / / Servo controle op de hoek van de telefoon
#define FLpin 9
#define FRpin 10
#define RLpin 11
#define RRpin 12
#define ledPin 13 / / LED wordt ingeschakeld terwijl het runnen van servo's
#define regelaar 20

#define OneDeg 5,65 //setting roterende vertraging voor gelijk is aan één graad (2 wielen tapijt)
#define OneDeg 9.04 //setting roterende vertraging voor gelijk is aan één graad (2 wielen hardhout)
Verifiëren van batterijspanning bleek te zijn het moeilijkste wat dat ik geprobeerd.  Dat zowel batterij aangesloten als USB is aangesloten...
verschillende lezingen geeft dan gewoon de accu aangesloten.  In feite werden de analoge lezingen een of andere manier teruggedraaid.
#define shutdownVoltageCheck 978 / / dit nummer is uniek voor elke spanning divider en batterij combo.  Het is eigenlijk 20% van mij.
#define analogvoltconstant 144.6969

int iAnVal; sonar sensor input
int deg;
int InferF = 0;
int InferB = 0;
int soundRT [72];
int soundLF [72];
int afstand [72];
int tempvolt;
unsigned long BatteryIndex = 0;
unsigned long BatteryIndexThreshold = 150000; ongeveer 10 minuten.
unsigned long RegulatorIndex;
unsigned long RegulatorTimerThreshold = 150000; interval inschakelen of uitschakelen van de spanningsregelaar. ongeveer 10 minuten.
int VoltSampleSize = 1000;
char dir ='s'; Zero's de richting de bot heeft verteld om te rijden - vooruit, achteruit, rechts, links, stoppen, ingesteld om te stoppen.
int macht =-1;
Boole ConservePower = waar; Dit bepaalt als de bot leegraakt besparingswijze.
int warmup = 1000; om setup te starten in milliseconden de wachttijd.

Geen configuratie vereist voor deze parameters
Boole servosActive = false; Neem aan servo's zijn niet verplaatsen wanneer we beginnen
unsigned long stopRegulatorIndex=millis(); gebruikt voor de berekening van de run RegulatorIndex voor servo 's
char incomingByte; Binnenkomende seriële waarden bevatten
char msg [8]; Voor het doorgeven van terug seriële berichten
char inBytes [schrijven]; Buffer voor serial in berichten
int serialIndex = 0;
int serialAvail = 0;

** BEREIK vinden ***-instellingen zijn voor ultrasone range finders. Klik op OK om lave als-is als u hen op uw robot hoeft
lange dist, microseconden, cm, inches; Door de range finder gebruikt voor de berekening van afstanden
Const int rangePinForward = 2; Digitale pin voor de voorwaartse geconfronteerd met Bereikindicatie (voor object afstand voor bot)
Const int rangeToObjectMargin = 25; (bot stopt afstand dichter dan deze - set op 0 als geen sensor)
Const int rangePinForwardGround = 0; Digitale pin voor neerwaartse geconfronteerd met Bereikindicatie aan de voorkant (voor rand van tabel detectie)
Const int rangeToGroundMargin = 0; Variëren in cm naar de tafel (bot stopt wanneer de afstand groter is dan deze set op 0 als geen sensor)
Const int rangeSampleCount = 3; Aantal van reeks lezingen te nemen en voor een meer stabiele waarde

//=============================================================================
VOID Setup {}

Serial.println ("setup starten!");
delay(Warmup);

set input-output modus ***
pinMode (LEDBlue, uitvoer);
pinMode (LEDGreen, uitvoer);
pinMode (LEDRed, uitvoer);
pinMode (servoPinHead, OUTPUT);
pinMode(FLpin,OUTPUT);
pinMode(FRpin,OUTPUT);
pinMode(RLpin,OUTPUT);
pinMode(RRpin,OUTPUT);
pinMode (ledPin, OUTPUT);
pinMode (regelgever, OUTPUT);

digitalWrite(servoPinHead,0);

Serial.begin(9600);
Serial3.begin(115200);

lastNeckValue = readSetting (EEPROM_lastNeckValue, lastNeckValue);
Als (lastNeckValue! = DEFAULT_lastNeckValue) {}
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
// }

//***************************** Clear Sensor Map ************************
voor (deg = 0; deg < 72; deg = deg + 1) {}
soundRT [deg] = 0;
soundLF [deg] = 0;
afstand [deg] = 0;
}

standaard servo's uitgeschakeld ***
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();

LEDOff();

pinMode (regelgever, OUTPUT); ingesteld pin 20 op uitvoermodus zodat de regulator van de VoltageCheck kan worden omgezet in- en uitschakelen.
digitalWrite (regelgever, laag); Start de regulator van de VoltageCheck in de uit-stand.

Serial.println ("einde setup!");

}
//====================================================================================

Antwoorden uit over seriële en behandelt onderbreken en spoelen van de gegevens om te gaan met Android seriële comms
VOID serialReply (char * sensorname, char * tmpmsg) {}
Serial3.Print(sensorname);
Serial3.Print(":");
Serial3.println(tmpmsg); De boodschap weer uit de seriële lijn
Wachten op de serial debugger te snoeren
delay(200); Dit is een magische getal
Serial3.Flush(); Hiermee wist u alle binnenkomende gegevens
}

Controleer als er voldoende RegulatorIndex is verstreken om te stoppen met de bot en als het veilig is om verder te gaan
Booleaanse checkIfStopBot() {}
int tempfrontsonar = Averagefive(SonarPin);
Serial.Print ("front sonar:");
Serial.println(tempfrontsonar);
int tempfrontinfer = Averagefive(InferFPin);
Serial.Print ("voorkant concluderen:");
Serial.println(tempfrontinfer);
int tempbackinfer = Averagefive(InferBPin);
Serial.Print ("rug concluderen:");
Serial.println(tempbackinfer);
Serial.println();

Als ((dir == 'f') & & ((tempfrontsonar < rangeToObjectMargin) || (tempfrontinfer < 200))) {
Stop();
servosActive = false;
return true;
} else if (dir == 'b') {}
Als (! () (tempbackinfer < 650) & & (tempbackinfer > 450))) {}
Stop();
servosActive = false;
return true;
}
}
return false;
}

Opdracht verzenden met gekoppelde Bluetooth-apparaat om te starten met het in paren rangschikken
ongeldig pairBluetooth() {}
Serial3.Print("\r\n+INQ=1\r\n"); Dit is voor Seeedstudio master/slave eenheid (wijzigen zoals nodig voor uw model)
}

Ingang voor seriële leest indien beschikbaar en ontleedt opdracht wanneer volledige opdracht is verstuurd.
ongeldig readSerialInput() {}
serialAvail = Serial3.available();
Lees wat er beschikbaar is
for (int i = 0; ik < serialAvail; i ++) {}
Opslaan in buffer.
inBytes [i + serialIndex] = Serial3.read();
Controleer voor het einde van de opdracht.

Als (inBytes [i + serialIndex] == '\n' || inBytes [i + serialIndex] == ";" || inBytes [i + serialIndex] == ' >') {//Use; bij het gebruik van seriële Monitor
inBytes [i + serialIndex] = '\0'; einde van de tekenreeks char
parseCommand(inBytes);
serialIndex = 0;
}
else {}
verwacht meer van de opdracht later te komen.
serialIndex += serialAvail;
}
}
}

Reinigt en ontleedt de opdracht
ongeldig parseCommand(char* com) {}
Als (com [0] == '\0') {terugkeer;} //bit voor de foutcontrole
int begin = 0;
beginnen van opdracht
terwijl (com [start]! = ' <') {}
Start ++;
Als (com [start] == '\0') {}
zijn er niet. Oude versie moet worden
Start = -1;
breken;
}
}
Start ++;
Verschuiven naar begin
int i = 0;
terwijl (com [i + start - 1]! = '\0') {}
com [i] = com [start + i];
i ++;
}
performCommand(com);
}

VOID LEDWhite() {}
digitalWrite (7, laag);
digitalWrite (6, laag);
digitalWrite (5, laag);
}

VOID LEDOff() {}
digitalWrite (7, hoge);
digitalWrite (6, hoge);
digitalWrite (5, hoge);
}

VOID Detach() {}
Serial.println ("detach servos!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
delay(150);
}

VOID Attach() {}
Serial.println ("bijvoegen servos!");
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
digitalWrite (regelaar, HIGH);
}

VOID Forew() {}
Serial.println ("*** vooruit!");
dir = 'f';
Als (niet checkIfStopBot()) {}
Attach();
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
RegulatorIndex = 0;
servosActive = true;
}
}

VOID Backw () {}
int Tempdist = Averagefive(InferFPin);
Serial.Print ("achterwaarts afstand:");
Serial.println(Tempdist);
Serial.println();
dir = "b";
Als (niet checkIfStopBot()) {}
Serial.println ("*** achterwaarts!");
Attach();
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
RegulatorIndex = 0;
servosActive = true;
}
}

VOID Stop () {}
Serial.println ("*** stoppen!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
dir = de ';
RegulatorIndex = 0;
serialReply ('i', 'ste'); De telefoon die de robot gestopt vertellen ###
servosActive = false;
}

leegte achtergelaten () {}
Serial.println ("*** linksom!");
Attach();
myservoFR.write(180);
myservoFL.write(180);
myservoRR.write(180);
myservoRL.write(180);
dir = 'l';
RegulatorIndex = 0;
servosActive = true;
}

void (juiste) {}
Serial.println ("*** rechts draaien!");
Attach();
myservoFR.write(0);
myservoFL.write(0);
myservoRR.write(0);
myservoRL.write(0);
dir = 'r';
RegulatorIndex = 0;
servosActive = true;
}

VOID CircleRt () {}
Serial.println ("*** cirkel recht!");
Attach();
myservoFR.write(95);
myservoFL.write(0);
myservoRR.write(95);
myservoRL.write(0);
RegulatorIndex = 0;
}

VOID CircleLf () {}
Serial.println ("*** cirkel links!");
Attach();
myservoFR.write(180);
myservoFL.write(85);
myservoRR.write(180);
myservoRL.write(85);
RegulatorIndex = 0;
}

VOID mydelay (int degress) {}
Serial.println ("mydelay start");
vertraging (int(OneDeg * degress));
Serial.println ("mydelay-end");
}

int Averagefive (int listenpin) {}
int gemiddelde = 12;
int luisteren [gemiddelde];
int index1;
int highindex = 0;
int hoge = 0;
int lowindex = 0;
int laag = 1000;
int totaal = 0;

voor (index1 = 0; index1 < gemiddelde; index1 = index1 + 1) {}
Luister [index1] = analogRead(listenpin);
Serial.println(listen[Index1]);
}

voor (index1 = 0; index1 < gemiddelde; index1 = index1 + 1) {}
Als (Luister [index1] > hoge) {}
hoog = luisteren [index1];
highindex = index1;
}
}

voor (index1 = 0; index1 < gemiddelde; index1 = index1 + 1) {}
Als (Luister [index1] < lage) {}
laag = luisteren [index1];
lowindex = index1;
}
}

voor (index1 = 0; index1 < gemiddelde; index1 = index1 + 1) {}
Als ((index1! = highindex) & & (index1! = lowindex)) {}
totaal = totaal + luisteren [index1];
}
}

totaal = int (totaal / (gemiddelde-2));
Serial.Print ("gemiddelde:");
Serial.println(Total);
retourneren totaal;
}

VOID Listen() {}
Serial.println ("=== luisteren ===!");
voor (deg = 0; deg < 71; deg = deg + 1) {}
soundRT [deg] = Averagefive(RightEarPin); Luister naar rechter oor en waarde toewijzen aan matrix
soundLF [deg] = Averagefive(LeftEarPin); Luister naar linker oor en waarde toewijzen aan matrix
afstand [deg] = Averagefive(SonarPin); een waarde van de sensor leest
Serial.println(Distance[DEG]); weergegeven waarde geschreven naar afstand matrix voor deze richting
Rechts();
mydelay(5);
Stop();
}
Stop();
}

VOID Course() {}
int BestDistDeg = 0;
int BestDist = 0;
voor (deg = 0; deg < 71; deg = deg + 1) {}
soundRT [deg] = analogRead(RightEarPin); Luister naar rechter oor en waarde toewijzen aan matrix
soundLF [deg] = analogRead(LeftEarPin); Luister naar linker oor en waarde toewijzen aan matrix
Als (afstand [deg + 1] > distance[BestDistDeg]) {}
BestDistDeg = deg + 1;
}
Serial.Print("BestDist=");
Serial.println(Distance[BestDistDeg]);
Serial.println(BestDistDeg); weergegeven waarde geschreven naar afstand matrix voor deze richting
}
Datumwaarde;
mydelay((71-BestDistDeg) * 5); Rechts draaien totdat cursus bijpassende langste afstand wordt bereikt.
Stop();
Forew();
}

VOID ForewardBackward() {}
Serial.println ("*** Foreward achterwaarts test Start!");
Forew();
delay(2000);
Stop();
delay(2000);
Backw();
delay(2000);
Stop();
delay(2000);
Serial.println ("*** Foreward achterwaarts test einde!");
}

ongeldig base() {}
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
delay(2500);
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
Serial.println ('verzenden 110');
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
delay(2500);
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
delay(2500);
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
Serial.println('sending70');
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
delay(2500);
}

ongeldig degreetest() {}
Serial.println ("degreetest start");
Stop();
delay(5000);
Rechts();
mydelay(360);
Serial.println ("degreetest-end");
}

VOID Signalblink() {}
LEDWhite();
delay(500);
LEDOff();
delay(500);
}

ongeldig forwardinfertest() {}
delay(500);
Serial.Print ("voorwaartse infered senor:");
Serial.println(analogRead(InferFPin));
gevaar drempel < 200
}

ongeldig backwardinfertest() {}
delay(500);
Serial.Print ("achterwaartse infered senor:");
Serial.println(analogRead(InferBPin));
gevaar drempel < 300
}

ongeldig sonartest() {}
delay(500);
Serial.Print ("Sonar senor:");
Serial.println(analogRead(SonarPin));
gevaar drempel < 30
}

int VoltageCheck() {}

int index1 = 0;
dubbele voltsum = 0;
int analogvolt = 0;

voor (index1 = 0; index1 < VoltSampleSize; index1 = index1 + 1) {}
voltsum = voltsum + analogRead(VoltPin);
}
analogvolt = int (voltsum / VoltSampleSize);
Serial.Print ("VoltageCheck:");
Serial.Print ("analag volt:");
Serial.println(analogvolt);
Serial.Print ("werkelijke volt:");
Serial.println ((analogvolt / analogvoltconstant));
Serial.println();
Return analogvolt;
}

VOID BatteryCheckLED()
{
Als (VoltageCheck() > shutdownVoltageCheck) {}
digitalWrite (LEDRed, laag);
digitalWrite (LEDGreen, hoge);
} else {}
digitalWrite (LEDRed, hoge);
digitalWrite (LEDGreen, laag);
}
BatteryIndex = 0;
}

VOID RegulatorControlToggle()
{
macht = macht * -1;
Als (macht == 1) {}
digitalWrite (regelaar, HIGH);
}
Als (macht == -1) {}
digitalWrite (regelgever, laag);
}
RegulatorIndex = 0;
}

VOID Miser()
{
RegulatorIndex = RegulatorIndex + 1;
Als (RegulatorIndex == RegulatorTimerThreshold) RegulatorControlToggle();
}

//********************************************* end mine *******************************************

ongeldig performCommand(char* com) {}
float volt = 0;

Als (killerid (com, "f") == 0) {/ / vooruit
Forew();
} else if (killerid (com, "r") == 0) {/ / rechts
Rechts();
} else if (killerid (com, "l") == 0) {/ / links
Datumwaarde;
} else if (killerid (com, "b") == 0) {/ / achterwaarts
Backw();
} else if (killerid (com, "s") == 0) {/ / Stop
Stop();
} anders als (killerid (com, "fr") == 0 || killerid (com, "fz") == 0 || killerid (com, "x") == 0) {/ / print vooruit tegenover afstandssensor
dist = Averagefive(SonarPin);
Itoa (dist, msg, 10); De dist int omzetten in een char
serialReply ("x", msg); Stuur de afstand uit de seriële lijn
} else if (killerid (com, "z") == 0) {/ / lezen en afdrukken grond geconfronteerd met afstandssensor
dist = Averagefive(SonarPin); dist = getDistanceSensor(rangePinForwardGround);
Itoa (dist, msg, 10); De dist int omzetten in een char
serialReply ("z", msg); Stuur de afstand uit de seriële lijn
Serial.println(dist);
} else if (killerid (com, "v") == 0) {/ / lezen en afdrukken van VoltageCheck
volt = VoltageCheck();
Itoa (volt, msg, 10); De volt int omzetten in een char
Serial.println(volt);
serialReply ("v", msg); Stuur de afstand uit de seriële lijn
} else if (killerid (com, "h") == 0) {/ / mode - toggle debugging Help
Signalblink();
} else if (com [0] == 'n') {/ / hoofd omhoog
sscanf (com, "n %d", & lastNeckValue); De inbreng van meerdere waarden parseren
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
EEPROM.write (EEPROM_lastNeckValue, lastNeckValue);
Als (DEBUGGING) {}
Serial.Print ("nek verhuisde naar");
Serial.println(lastNeckValue);
//}
} else if (com [0] == 'p') {/ / initieert Bluetooth dus een ander apparaat in paren rangschikken kan connect
pairBluetooth();
} else {}
serialReply ("e", com); / / Echo onbekende opdracht terug
Als (DEBUGGING) {}
Serial.Print ("Unknown command:");
Serial.println(com);
// }

0-9, w n w, ik, c, h, d voor demo
}
}

=== Hoofdlus draait op alle RegulatorIndexs ===
void loop
{
readSerialInput();
checkIfStopBot();

BatteryIndex = BatteryIndex + 1;
Als (BatteryIndex == BatteryIndexThreshold) BatteryCheckLED();

Als (ConservePower) Miser();
}