4 servo drive CellBot som kan fjärrstyras. (8 / 8 steg)

Steg 8: Arduino koden anpassade till min användning.

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

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

#define BUFFERSIZE 20
#define InferFPin 0 //Front sluta sig till analoga givarstaven
#define InferBPin 1 //Back sluta sig till analoga givarstaven
#define SonarPin 2 //sonar analog pin
#define LeftEarPin 3 //left öra analoga pin
#define RightEarPin 4 //right öra analoga pin /
#define VoltPin 5 //analog pin 5 används för VoltageCheck upptäckt.
#define LEDBlue 5
#define LEDGreen 6
#define LEDRed 7
#define servoPinHead 8 / / Servo kontrollera vinkeln på telefonen
#define FLpin 9
#define FRpin 10
#define RLpin 11
#define RRpin 12
#define ledPin 13 / / tänds när du kör servon
#define regulator 20

#define OneDeg 5,65 //setting roterande fördröjning för motsvarar en grad (2 rullar mattan)
#define OneDeg 9.04 //setting roterande fördröjning för motsvarar en grad (2 hjul lövträ)
Kontrollera batteriets spänning visade sig vara det svåraste som jag försökt. Med både batteri och USB-ansluten...
ger olika mätvärden sedan bara batteriet anslutet. I själva verket var de analoga avläsningarna omvända på något sätt.
#define shutdownVoltageCheck 978 / / detta nummer är unikt för varje spänningsavdelare och batteri combo. Det är i grunden 20% till mig.
#define analogvoltconstant 144.6969

int iAnVal; sonar sensor input
int deg;
int InferF = 0;
int InferB = 0;
int soundRT [72].
int soundLF [72].
int avstånd [72].
int tempvolt;
osignerade långa BatteryIndex = 0;
osignerade långa BatteryIndexThreshold = 150000; ca 10 minuter.
osignerade långa RegulatorIndex;
osignerade långa RegulatorTimerThreshold = 150000; intervallet slå på eller stänga av spänningsregulatorn. ca 10 minuter.
int VoltSampleSize = 1000;
char dir ='s'; Zeros riktning bot har fått höra att driva - framåt, bakåt, höger, vänster, stoppa, anger att den stannar.
int power =-1;
booleska ConservePower = sant; Detta avgör om bot använder makten räddningen sätt.
int warmup = 1000; dröjsmål att starta installationsprogrammet i millisekunder.

Ingen config krävs för dessa parametrar
booleska servosActive = false; anta servon inte rör sig när vi börjar
osignerade långa stopRegulatorIndex=millis(); används för att beräkna den köra RegulatorIndex för servon
char incomingByte; Innehar inkommande serievärden
char msg [8]. För att skicka tillbaka seriell meddelanden
char inBytes [BUFFERSIZE]; Buffert för följetong i meddelanden
int serialIndex = 0;
int serialAvail = 0;

** UTBUD att hitta ***-inställningar gäller för ultraljud utbud finders. OK att lave som-om du inte har dem på din robot
långa dist, mikrosekunder, cm, inches; Används av range finder för att beräkna avstånd
CONST int rangePinForward = 2; Digital pin för framåt inför range finder (för objektet avstånd framför bot)
CONST int rangeToObjectMargin = 25; (bot slutar när avstånd närmare än denna - satt till 0 om ingen sensor)
CONST int rangePinForwardGround = 0; Digital pin för nedåt inför avståndsmätare på framsidan (för kanten av bordet upptäckt)
CONST int rangeToGroundMargin = 0; Varierar i cm till tabellen (bot kommer sluta när avståndet är större än denna satt till 0 om ingen sensor)
CONST int rangeSampleCount = 3; Antal intervall avläsningar ta och för ett mer stabilt värde

//=============================================================================
void setup() {

Serial.println ("startar du installationsprogrammet!");
Delay(WarmUp);

Ställ in input/output mode ***
pinMode (LEDBlue, OUTPUT);
pinMode (LEDGreen, OUTPUT);
pinMode (LEDRed, OUTPUT);
pinMode (servoPinHead, produktionen);
pinMode(FLpin,OUTPUT);
pinMode(FRpin,OUTPUT);
pinMode(RLpin,OUTPUT);
pinMode(RRpin,OUTPUT);
pinMode (ledPin, produktionen);
pinMode (regulator, produktionen);

digitalWrite(servoPinHead,0);

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

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

//***************************** Clear Sensor Map ************************
för (deg = 0; deg < 72; deg = deg + 1) {
soundRT [GRD] = 0;
soundLF [GRD] = 0;
avståndet [GRD] = 0;
}

standard till servon avstängd ***
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();

LEDOff();

pinMode (regulator, produktionen); inställd på stift 20 Utläge så VoltageCheck regulator kan slås på och av.
digitalWrite (regulator, låg); Starta VoltageCheck regulator i läge off.

Serial.println ("avsluta installationsprogrammet!");

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

Svar ut över följetong och hanterar pausa och spola data att hantera Android seriell kommunikation
void serialReply (char * sensorname, char * tmpmsg) {
Serial3.Print(sensorname);
Serial3.Print(":");
Serial3.println(tmpmsg); Skicka meddelandet tillbaka ut den seriella linjen
Vänta för seriell felsökaren att hålla käften
Delay(200); Detta är ett magiskt nummer
Serial3.flush(); rensar alla inkommande data
}

Kontrollera om tillräckligt RegulatorIndex har förflutit för att stoppa bot och om det är säkert att gå vidare
booleska checkIfStopBot() {
int tempfrontsonar = Averagefive(SonarPin);
Serial.Print ("front ekolod:");
Serial.println(tempfrontsonar);
int tempfrontinfer = Averagefive(InferFPin);
Serial.Print ("front dra slutsatsen:");
Serial.println(tempfrontinfer);
int tempbackinfer = Averagefive(InferBPin);
Serial.Print ("back dra slutsatsen:");
Serial.println(tempbackinfer);
Serial.println();

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

Skicka kommando till anslutna Bluetooth enheten att initiera ihopkopplingen
void pairBluetooth() {
Serial3.Print("\r\n+INQ=1\r\n"); Detta är för Seeedstudio master/slav enhet (förändring som behövs för din modell)
}

Läser seriell in om tillgängliga och tolkar kommandot när full kommandot skickades.
void readSerialInput() {
serialAvail = Serial3.available();
Läsa vad som finns
för (int jag = 0; jag < serialAvail; i ++) {
Lagra i buffert.
inBytes [jag + serialIndex] = Serial3.read();
Kontrollera om kommandot slutet.

om (inBytes [jag + serialIndex] == '\n' || inBytes [jag + serialIndex] == ';' || inBytes [jag + serialIndex] == ' >') {//Use; när du använder Serial Monitor
inBytes [jag + serialIndex] = '\0'; slutet av strängen röding
parseCommand(inBytes);
serialIndex = 0;
}
annat {
förväntar sig mer av kommandot att komma senare.
serialIndex += serialAvail;
}
}
}

Rengör och tolkar kommandot
void parseCommand(char* com) {
om (com [0] == '\0') {return;} //bit av felkontroll
int start = 0;
får start av kommandot
medan (com [start]! = ' <') {
börja ++;
om (com [start] == '\0') {
sitt inte där. Måste vara gammal version
Start = -1;
bryta;
}
}
börja ++;
Flytta till början
int jag = 0;
medan (com [i + start - 1]! = '\0') {
com [i] = com [start + i];
i ++;
}
performCommand(com);
}

void LEDWhite() {
digitalWrite (7, låg).
digitalWrite (6, låg).
digitalWrite (5, låg).
}

void LEDOff() {
digitalWrite (7, hög).
digitalWrite (6, hög).
digitalWrite 5, hög,
}

void Detach() {
Serial.println ("ta bort servon!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
Delay(150);
}

void Attach() {
Serial.println ("bifoga servon!");
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
digitalWrite (regulator, hög);
}

void Forew() {
Serial.println ("*** framåt!");
dir = "f";
om (inte checkIfStopBot()) {
Attach();
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
RegulatorIndex = 0;
servosActive = sant;
}
}

utan laga kraft bakåt () {
int Tempdist = Averagefive(InferFPin);
Serial.Print ("bakåt avstånd:");
Serial.println(Tempdist);
Serial.println();
dir = "b";
om (inte checkIfStopBot()) {
Serial.println ("*** bakåt!");
Attach();
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
RegulatorIndex = 0;
servosActive = sant;
}
}

void Stop () {
Serial.println ("*** Stop!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
dir ='s ";
RegulatorIndex = 0;
serialReply ("i", "st"); Berätta för den telefon som roboten stannade ###
servosActive = false;
}

tomrummet () {
Serial.println ("*** rotera vänster!");
Attach();
myservoFR.write(180);
myservoFL.write(180);
myservoRR.write(180);
myservoRL.write(180);
dir = 'l';
RegulatorIndex = 0;
servosActive = sant;
}

void rätt () {
Serial.println ("*** rotera höger!");
Attach();
myservoFR.write(0);
myservoFL.write(0);
myservoRR.write(0);
myservoRL.write(0);
dir = "r";
RegulatorIndex = 0;
servosActive = sant;
}

void CircleRt () {
Serial.println ("*** cirkeln rätt!");
Attach();
myservoFR.write(95);
myservoFL.write(0);
myservoRR.write(95);
myservoRL.write(0);
RegulatorIndex = 0;
}

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

void mydelay (int grader) {
Serial.println ("mydelay start");
fördröjning (int(OneDeg * degress));
Serial.println ("mydelay slutet").
}

int Averagefive (int listenpin) {
int genomsnitt = 12.
int lyssna [genomsnittliga];
int index1;
int highindex = 0;
int hög = 0;
int lowindex = 0;
int låg = 1000;
int totala = 0;

för (index1 = 0; index1 < genomsnittet; index1 = index1 + 1) {
lyssna på [index1] = analogRead(listenpin);
Serial.println(listen[index1]);
}

för (index1 = 0; index1 < genomsnittet; index1 = index1 + 1) {
om (lyssna [index1] > hög) {
hög = lyssna [index1];
highindex = index1;
}
}

för (index1 = 0; index1 < genomsnittet; index1 = index1 + 1) {
om (lyssna [index1] < låg) {
låg = lyssna [index1];
lowindex = index1;
}
}

för (index1 = 0; index1 < genomsnittet; index1 = index1 + 1) {
om ((index1! = highindex) & & (index1! = lowindex)) {
totalt = totalt + lyssna [index1];
}
}

Total = int (totalt / (genomsnitt-2));
Serial.Print ("genomsnittliga:");
Serial.println(total);
avkastning totalt;
}

void Listen() {
Serial.println ("=== lyssna ===!");
för (deg = 0; deg < 71; deg = deg + 1) {
soundRT [GRD] = Averagefive(RightEarPin); lyssna på höger öra och tilldela värde till matris
soundLF [GRD] = Averagefive(LeftEarPin); lyssna på vänster öra och tilldela värde till matris
avståndet [GRD] = Averagefive(SonarPin); läsa ett värde från sensorn
Serial.println(Distance[deg]); Visa värde skrivs till sträcka arrayen för denna riktning
Right();
mydelay(5);
Stop();
}
Stop();
}

void Course() {
int BestDistDeg = 0;
int BestDist = 0;
för (deg = 0; deg < 71; deg = deg + 1) {
soundRT [GRD] = analogRead(RightEarPin); lyssna på höger öra och tilldela värde till matris
soundLF [GRD] = analogRead(LeftEarPin); lyssna på vänster öra och tilldela värde till matris
om (avstånd [deg + 1] > distance[BestDistDeg]) {
BestDistDeg = deg + 1.
}
Serial.Print("BestDist=");
Serial.println(Distance[BestDistDeg]);
Serial.println(BestDistDeg); Visa värde skrivs till sträcka arrayen för denna riktning
}
Left();
mydelay((71-BestDistDeg) * 5); Rotera höger tills kursen på längsta avståndet uppnås.
Stop();
Forew();
}

void ForewardBackward() {
Serial.println ("*** frammåt bakåt test Start!");
Forew();
Delay(2000);
Stop();
Delay(2000);
Backw();
Delay(2000);
Stop();
Delay(2000);
Serial.println ("*** frammåt bakåt test slutet!");
}

void 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 ("Skicka 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);
}

void degreetest() {
Serial.println ("degreetest start");
Stop();
Delay(5000);
Right();
mydelay(360);
Serial.println ("degreetest slutet").
}

void Signalblink() {
LEDWhite();
Delay(500);
LEDOff();
Delay(500);
}

void forwardinfertest() {
Delay(500);
Serial.Print ("framåt infered senor:");
Serial.println(analogRead(InferFPin));
tröskelvärde för fara < 200
}

void backwardinfertest() {
Delay(500);
Serial.Print ("bakåt infered senor:");
Serial.println(analogRead(InferBPin));
tröskelvärde för fara < 300
}

void sonartest() {
Delay(500);
Serial.Print ("ekolod senor:");
Serial.println(analogRead(SonarPin));
tröskelvärde för fara < 30
}

int VoltageCheck() {

int index1 = 0;
Double voltsum = 0;
int analogvolt = 0;

för (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 ("faktiska volt:");
Serial.println ((analogvolt / analogvoltconstant));
Serial.println();
återvända analogvolt;
}

void BatteryCheckLED()
{
om (VoltageCheck() > shutdownVoltageCheck) {
digitalWrite (LEDRed, låg);
digitalWrite (LEDGreen, hög);
} annat {
digitalWrite (LEDRed, hög);
digitalWrite (LEDGreen, låg);
}
BatteryIndex = 0;
}

void RegulatorControlToggle()
{
Power = kraft * -1;
om (power == 1) {
digitalWrite (regulator, hög);
}
om (power == -1) {
digitalWrite (regulator, låg);
}
RegulatorIndex = 0;
}

void Miser()
{
RegulatorIndex = RegulatorIndex + 1.
om (RegulatorIndex == RegulatorTimerThreshold) RegulatorControlToggle();
}

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

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

om (strcmp (com, "f") == 0) {/ / framåt
Forew();
} else om (strcmp (com, "r") == 0) {/ / höger
Right();
} else om (strcmp (com, "l") == 0) {/ / vänster
Left();
} else om (strcmp (com, "b") == 0) {/ / bakåt
Backw();
} else om (strcmp (com, "s") == 0) {/ / stopp
Stop();
} else om (strcmp (com, "fr") == 0 || strcmp (com, "fz") == 0 || strcmp (com, "x") == 0) {/ / print framåtvänd avstånd sensor
dist = Averagefive(SonarPin);
itoa (dist, msg, 10); Förvandla de förd int till en char
serialReply ("x", msg); Skicka avståndet ut den seriella linjen
} else om (strcmp (com, "z") == 0) {/ / Läs och skriv ut marken mot avstånd sensor
dist = Averagefive(SonarPin); dist = getDistanceSensor(rangePinForwardGround);
itoa (dist, msg, 10); Förvandla de förd int till en char
serialReply ("z", msg); Skicka avståndet ut den seriella linjen
Serial.println(dist);
} else om (strcmp (com, "v") == 0) {/ / läsa och skriva ut VoltageCheck
volt = VoltageCheck();
itoa (volt, msg, 10); Förvandla volt int till en char
Serial.println(volt);
serialReply ("v", msg); Skicka avståndet ut den seriella linjen
} else om (strcmp (com, "h") == 0) {/ / hjälp-läge - felsökning växla
Signalblink();
} else om (com [0] == 'n') {/ / flytta huvudet
sscanf (com, n %d", och lastNeckValue); Tolka indata till flera värden
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
EEPROM.write (EEPROM_lastNeckValue, lastNeckValue);
om (felsökning) {
Serial.Print ("halsen flyttade till");
Serial.println(lastNeckValue);
//}
} else om (com [0] == "p") {/ / initierar Bluetooth para ihop så en annan enhet kan ansluta
pairBluetooth();
} annat {
serialReply ("e", com); / / Echo Okänt kommando tillbaka
om (felsökning) {
Serial.Print ("Okänt kommando:");
Serial.println(com);
// }

0-9, w, n, w, jag, c, h, d för demo,
}
}

=== Huvudloop som kör på alla RegulatorIndexs ===
void loop()
{
readSerialInput();
checkIfStopBot();

BatteryIndex = BatteryIndex + 1.
om (BatteryIndex == BatteryIndexThreshold) BatteryCheckLED();

om (ConservePower) Miser();
}