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

Stap 7: De Arduino Code (ongewijzigd)

Zoekt een reeks van karakters van ASCII in het signaal
opdrachten aan een reeks van servo's aan het rijden van een kleine robot. LED pin #13
blijft branden tijdens servo verkeer en blink voor veranderingen van de snelheid.

Het minimale circuit:
* LED verbonden vanaf pin 13 tot en met de grond (of gebruik ingebouwde LED op de meeste Arduino)
* Servos met signaal draden aangesloten op pin 3 en 5 (5v macht en motivering
servo's kunnen ook worden bedraad in Arduino, of macht kan afkomstig zijn van externe bron)
* Seriële input aangesloten op RX pin 0
* Seriële uitgang aangesloten op TX pin 1

Extra circuits (optioneel):
* Toekomen geconfronteerd met ultrasone Bereikindicatie op digitale pin 7
* Neerwaartse facing ultrasone Bereikindicatie op digitale pin 8

Opmerking: Als u een seriële apparaat te verbinden met nog niet hebt, kunt u de
gebouwd in seriële Monitor in de Arduino software wanneer aansluiten via USB voor het testen.
Moet ook naar onthutsen RX en TX pinnen van andere apparaten wanneer het proberen om te programmeren
de Arduino via USB.

gemaakte 2010
door Tim Heath, Ryan Hickman en Glen Arrowsmith
Bezoek http://www.cellbots.com voor meer informatie
*/

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

#define schrijven 20
#define EEPROM_servoCenterLeft 1
#define EEPROM_servoCenterRight 2
#define EEPROM_speedMultiplier 3
#define EEPROM_servosForcedActive 4
#define EEPROM_lastNeckValue 5

#define DEFAULT_servoCenterLeft 90
#define DEFAULT_servoCenterRight 90
#define DEFAULT_speedMultiplier 5
#define DEFAULT_servosForcedActive false
#define DEFAULT_servosForcedActive false
#define DEFAULT_lastNeckValue 255

** ALGEMENE instellingen **-algemene voorkeursinstellingen
Booleaanse foutopsporing = false; Of uitvoer foutopsporing via seriële brandt door defauly (kan worden omgedraaid met 'h' opdracht)
Const int ledPin = 13; LED wordt ingeschakeld terwijl het runnen van servo's
char * driveType = "servo"; Gebruik van de "motor" als bots beschikt DC motor stuurprogramma of een "servo" voor servo's voeden van de wielen

** SERVO instellingen **-configureerbare waarden gebaseerd op pinnen gebruikt en servo richting
Const int servoPinLeft = 9;
Const int servoPinRight = 10;
Const int servoPinHead = 12; Servo controle op de hoek van de telefoon
Const int servoDirectionLeft = 1; 1 of -1 gebruiken voor reverse
Const int servoDirectionRight = -1; 1 of -1 gebruiken voor reverse
int servoCenterLeft = DEFAULT_servoCenterLeft; PWM instelling voor geen beweging op linker servo
int servoCenterRight = DEFAULT_servoCenterLeft; PWM instelling voor geen beweging op juiste servo
int servoPowerRange = 30; PWM bereik off center die servo's best reageren en (ingesteld op 30 om te werken in het bereik van 60-120 uit midden van 90)
Const lange maxRunTime = 2000; Maximaal bewerkingstijd voor servo's zonder extra opdracht. * Gebruik een opdracht om dit te plaatsen. *
int speedMultiplier = DEFAULT_speedMultiplier; De instelling van de snelheid van de standaard. Maakt gebruik van een scala van 1-10
int lastNeckValue = DEFAULT_lastNeckValue;

** De instellingen van het stuurprogramma van de MOTOR ** - voor gebruik met planken zoals de Pololu motor stuurprogramma (ook gebruik links/rechts servo pin-instellingen hierboven)
int leftMotorPin_1 = 9;
int leftMotorPin_2 = 8;
int rightMotorPin_1 = 10;
int rightMotorPin_2 = 11;
int motor_stby = 12;

** BEREIK vinden ***-de volgende 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 = 7; Digitale pin voor de voorwaartse geconfronteerd met Bereikindicatie (voor object afstand voor bot)
Const int rangeToObjectMargin = 0; Bereik in cm op vooruit-object (bot stopt afstand dichter dan deze - set op 0 als geen sensor)
Const int rangePinForwardGround = 8; 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 reeks lezingen te nemen en het gemiddelde voor een meer stabiele waarde

Servo-objecten om te bepalen van de servo's maken
Servo myservoLeft;
Servo myservoRight;
Servo myservoHead;

Geen configuratie vereist voor deze parameters
Boole servosActive = false; Neem aan servo's zijn niet verplaatsen wanneer we beginnen
Boole servosForcedActive = DEFAULT_servosForcedActive; zal alleen stoppen als als gevaarlijk beschouwd
unsigned long stopTime=millis(); gebruikt voor de berekening van de bewerkingstijd 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;

VOID Setup {}
pinMode (servoPinLeft, OUTPUT);
pinMode (servoPinRight, OUTPUT);
pinMode (servoPinHead, OUTPUT);
pinMode(leftMotorPin_1,OUTPUT);
pinMode(leftMotorPin_2,OUTPUT);
pinMode(rightMotorPin_1,OUTPUT);
pinMode(rightMotorPin_2,OUTPUT);
pinMode (ledPin, OUTPUT);
digitalWrite(servoPinLeft,0);
digitalWrite(servoPinRight,0);
digitalWrite(servoPinHead,0);
digitalWrite(motor_stby,HIGH);
Serial.begin(115200);
servoCenterLeft = readSetting (EEPROM_servoCenterLeft, servoCenterLeft);
servoCenterRight = readSetting (EEPROM_servoCenterRight, servoCenterRight);
speedMultiplier = readSetting (EEPROM_speedMultiplier, speedMultiplier);
servosForcedActive = readSetting (EEPROM_servosForcedActive, servosForcedActive);
lastNeckValue = readSetting (EEPROM_lastNeckValue, lastNeckValue);
Als (lastNeckValue! = DEFAULT_lastNeckValue) {}
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
}
}

Veilig leest EEPROM
int readSetting (int memoryLocation, int defaultValue) {}
int-waarde = EEPROM.read(memoryLocation);
Als (waarde 255 ==) {}
EEPROM.write (memoryLocation, standaardwaarde (DefaultValue));
}
retourwaarde;
}

Hiermee stelt u de EEPROM-instellingen op de standaardwaarden
ongeldig setEepromsToDefault() {}
servosForcedActive = DEFAULT_servosForcedActive;
speedMultiplier = DEFAULT_speedMultiplier;
servoCenterRight = DEFAULT_servoCenterRight;
servoCenterLeft = DEFAULT_servoCenterLeft;
lastNeckValue = DEFAULT_lastNeckValue;
EEPROM.write (EEPROM_servosForcedActive, DEFAULT_servosForcedActive);
EEPROM.write (EEPROM_speedMultiplier, DEFAULT_speedMultiplier);
EEPROM.write (EEPROM_servoCenterRight, DEFAULT_servoCenterRight);
EEPROM.write (EEPROM_servoCenterLeft, DEFAULT_servoCenterLeft);
EEPROM.write (EEPROM_lastNeckValue, DEFAULT_lastNeckValue);
Als (DEBUGGING) {}
Serial.println ("alle EEPROM waarden ingesteld op de standaardwaarden.');
}
}

Directionele tekstopdrachten ("vooruit"- of "Terugspoelen") converteren naar berekende servo snelheid
int directionValue (char * directionCommand, int servoDirection) {}
Als (directionCommand == "vooruit") {}
retourneren (10 * speedMultiplier * servoDirection);
}
else if (directionCommand == "achteruit") {}
retourneren (-10 * speedMultiplier * servoDirection);
}
else {}
Als (DEBUGGEN) {Serial.println ("Houston, we hebben een probleem!");}
keren 0; Poging om in te stellen waarde naar het midden - dit moet niet nodig zijn
}
}

Tekstopdrachten vertalen naar PWM waarden voor de bot te verplaatsen (linker servo command, juiste servo-opdracht)
unsigned long moveBot (char * commandLeft, char * commandRight) {}
int valueLeft = directionValue (commandLeft, servoDirectionLeft) + servoCenterLeft;
int valueRight = directionValue (commandRight, servoDirectionRight) + servoCenterRight;
driveWheels (valueLeft, valueRight);
}

Station servo of DC motoren als wilt verplaatsen de robot met behulp van waarden in het bereik-100 tot 100 voor links en rechts
unsigned long driveWheels (int valueLeft, int valueRight) {}
Loskoppelen van beide servo pinnen die stoppen met zal zeuren en -energize de motoren zodat ze niet de kompas lezingen doden
Als (valueLeft == 0 en valueRight == 0) {}
myservoLeft.detach();
myservoRight.detach();
}
De wielen op basis van "servo" driveType rijden
Als (driveType == "servo") {}
valueLeft = valueLeft * servoDirectionLeft; Spiegelen positief hebben gereageerd op negatieve indien nodig op basis van de instelling van de waarde van de richting van de servo
valueRight = valueRight * servoDirectionRight;
"W" waarden toewijzen aan de smalle waaier die de servo's reageren op
valueLeft = kaart (valueLeft,-100, 100, (servoCenterLeft - servoPowerRange), (servoCenterLeft + servoPowerRange));
valueRight = kaart (valueRight,-100, 100, (servoCenterRight - servoPowerRange), (servoCenterRight + servoPowerRange));
digitalWrite (ledPin, hoge); de LED aangezet
Opnieuw opstarten van de servo van de PWM en stuur ze opdrachten
myservoLeft.attach(servoPinLeft);
myservoRight.attach(servoPinRight);
myservoLeft.write(valueLeft);
myservoRight.write(valueRight);
Spugen sommige diagnose-info over seriële
Als (DEBUGGING) {}
Serial.Print ("Moving links servo");
Serial.Print (valueLeft, DEC);
Serial.Print ("en juiste servo");
Serial.println (valueRight, DEC);
}
}
De wielen op basis van de "motor" driveType rijden
else {}
Instellen van de linker motor pinnen om te zetten in de gewenste richting
Als (valueLeft < 0) {}
digitalWrite(leftMotorPin_1,LOW);
digitalWrite(leftMotorPin_2,HIGH);
}
else {}
digitalWrite(leftMotorPin_1,HIGH);
digitalWrite(leftMotorPin_2,LOW);
}
Instellen van rechts motor pinnen om te zetten in de gewenste richting
Als (valueRight < 0) {}
digitalWrite(rightMotorPin_1,LOW);
digitalWrite(rightMotorPin_2,HIGH);
}
else {}
digitalWrite(rightMotorPin_1,HIGH);
digitalWrite(rightMotorPin_2,LOW);
}
"W" waarden toegewezen aan de bredere waaier die de motor reageert op
valueLeft = map(abs(valueLeft), 0, 100, 0, 255);
valueRight = map(abs(valueRight), 0, 100, 0, 255);
analogWrite(servoPinLeft,valueLeft);
analogWrite(servoPinRight,valueRight);
}

stopTime=millis() + maxRunTime; Tijd om te stoppen op basis van toegestane lopende tijd instellen
retourneren van stilstaan;
}

Stop de bot
ongeldig stopBot() {}
driveWheels(0,0);
digitalWrite (ledPin, laag); Het uitschakelen van de LED
Als (DEBUGGING) {Serial.println ("stoppen beide wielen");}
serialReply ('i', 'ste'); Vertellen van de telefoon die de robot gestopt
}

Lezen en verwerken van de waarden uit een ultrasone Bereikindicatie (u kunt deze code in laten zelfs als u geen één hebt)
lange getDistanceSensor(int ultrasonicPin) {}
Neem meerdere lezingen en gemiddelde hen
microseconden = 0;
voor (int monster = 1; proeven < = rangeSampleCount; proeven ++) {}
De Parallax PING))) wordt geactiveerd door een hoge pols van 2 of meer microseconden.
Geef een korte lage puls vooraf om een schoon hoog puls:
De Maxsonar lijkt niet nodig dit deel maar het geen kwaad ofwel
pinMode (ultrasonicPin, OUTPUT);
digitalWrite (ultrasonicPin, laag);
delayMicroseconds(2);
digitalWrite (ultrasonicPin, hoge);
delayMicroseconds(5);
digitalWrite (ultrasonicPin, laag);

Dezelfde pincode wordt gebruikt voor het lezen van het signaal van de ultrasone detector: een hoog
Pulse waarvan de duur de tijd (in microseconden) van de verzending is
voor de ping naar de receptie van de echo off van een object.
pinMode (ultrasonicPin, INPUT);
microseconden += pulseIn (ultrasonicPin, hoge);
delayMicroseconds(5); Zeer korte pauze tussen lezingen
}
microseconden = microseconden / rangeSampleCount;
De gemiddelde sensor lezen naar centimeters converteren en terug te sturen
cm = microsecondsToCentimeters(microseconds);
duim = microsecondsToInches(microseconds);
Als (DEBUGGING) {}
Serial.Print ("Micro:"); Serial.Print(microseconds);
Serial.Print ("inch:"); Serial.Print(inches);
Serial.Print ("cm: '); Serial.println(cm);
}
retourneren cm;
}

lange microsecondsToCentimeters(long microseconds) {}
De snelheid van het geluid is 340 m/s of 29 microseconden per vierkante centimeter.
De ping reist uit en terug, zo vind je de afstand van de
we de helft van de afgelegde afstand nemen-object.
retourneren van microseconden / 29 / 2;
}

lange microsecondsToInches(long microseconds) {}
Volgens de Parallax gegevensblad voor de PING))), er zijn
73.746 microseconden per inch (dwz geluid reist aan 1130 voeten per
seconde). Dit geeft de afstand die is afgelegd door de ping, uitgaande
en terug te keren, zodat wij door 2 delen te krijgen van de afstand van het obstakel.
Zie: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
Hetzelfde geldt voor de MaxSonar door MaxBotix
retourneren van microseconden / 74 / 2;
}

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) {}
Serial.Print(sensorname);
Serial.Print(":");
Serial.println(tmpmsg); De boodschap weer uit de seriële lijn
Wachten op de serial debugger te snoeren
delay(200); Dit is een magische getal
Serial.Flush(); Hiermee wist u alle binnenkomende gegevens
}

Controleert range finders om te zien of het veilig is om te blijven bewegen (* moet toevoegen manier om te weten welke richting we gaan *)
Booleaanse safeToProceed() {}
Booleaanse kluis = false; Neem aan dat het niet veilig om door te gaan
Controleer de afstand tot het dichtstbijzijnde object voor de bot en stoppen als te dicht
Als (rangeToObjectMargin! = 0) {/ / Don't bother verzenden als marge ingesteld op nul, omdat het loopt vast wanneer geen sensor aanwezig
dist = getDistanceSensor(rangePinForward);
Als (dist > rangeToObjectMargin) {}
veilig = true;
}
anders als (DEBUGGING) {Serial.print ("Object te dicht in de front -");}
}
Controleer de afstand tot de grond voor de bot om ervoor te zorgen dat de tabel is er nog steeds
Als (rangeToGroundMargin! = 0) {/ / Don't bother verzenden als marge ingesteld op nul, omdat het loopt vast wanneer geen sensor aanwezig
dist = getDistanceSensor(rangePinForwardGround);
Als (dist > rangeToGroundMargin) {}
veilig = true;
}
anders als (DEBUGGING) {Serial.print ("End of oppervlak bereikt -");}
}
Als (rangeToGroundMargin == 0 & & rangeToObjectMargin == 0) {terugkeer waar;}
terugkeer veilig;
}

Controleer als er voldoende tijd is verstreken om te stoppen met de bot en als het veilig is om verder te gaan
ongeldig checkIfStopBot() {}
Als (niet servosForcedActive en servosActive en (stilstaan < millis() of niet safeToProceed())) {}
stopBot();
servosActive = false;
} else if (niet safeToProceed()) {}
stopBot();
servosActive = false;
}
}

Opdracht verzenden met gekoppelde Bluetooth-apparaat om te starten met het in paren rangschikken
ongeldig pairBluetooth() {}
Serial.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 = Serial.available();
Lees wat er beschikbaar is
for (int i = 0; ik < serialAvail; i ++) {}
Opslaan in buffer.
inBytes [i + serialIndex] = Serial.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);
}

ongeldig performCommand(char* com) {}
Als (killerid (com, "f") == 0) {/ / vooruit
stopTime = driveWheels (speedMultiplier * 10, speedMultiplier * 10);
servosActive = true;
} else if (killerid (com, "r") == 0) {/ / rechts
stopTime = driveWheels (speedMultiplier * 10, speedMultiplier * -10);
servosActive = true;
} else if (killerid (com, "l") == 0) {/ / links
stopTime = driveWheels (speedMultiplier * -10, speedMultiplier * 10);
servosActive = true;
} else if (killerid (com, "b") == 0) {/ / achterwaarts
stopTime = driveWheels (speedMultiplier * -10, speedMultiplier * -10);
servosActive = true;
} else if (killerid (com, "s") == 0) {/ / Stop
stopBot();
servosActive = false;
} anders als (killerid (com, "fr") == 0 || killerid (com, "fz") == 0 || killerid (com, "x") == 0) {/ / lezen en afdrukken naar voren geconfronteerd met afstandssensor
dist = getDistanceSensor(rangePinForward);
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 = getDistanceSensor(rangePinForwardGround);
Itoa (dist, msg, 10); De dist int omzetten in een char
serialReply ("z", msg); Stuur de afstand uit de seriële lijn
} else if (killerid (com, "h") == 0) {/ / mode - toggle debugging Help
Sommige basisinstructies uitprinten wanneer eerst inschakelen Foutopsporing
Als (geen DEBUGGING) {}
Serial.println ("klaar om te luisteren naar opdrachten! Ome van deze proberen: ");
Serial.println ("F (voorwaarts), B (neerwaarts), L (links), R (rechts), S (stop), D (demo).");
Serial.println ("ook gebruik nummers 1-9 als u wilt aanpassen van de snelheid (0 = langzaam, 9 = snel).");
}
FOUTOPSPORING =! FOUTOPSPORING;
} anders als (killerid (com, "1") == 0 || killerid (com, "2") == 0 || killerid (com, "3") == 0 || killerid (com, "4") == 0 || killerid (com, "5") == 0 || killerid (com, "6") == 0 || killerid (com, "7") == 0 || killerid (com, "8") == 0 || killerid (com, "9") == 0 || killerid (com, "0") == 0) {}
Ik weet dat de voorgaande voorwaarde is gehaaid, maar het zal snel veranderen
Als (DEBUGGING) {Serial.print ("Changing snelheid op");}
int i = com [0];
speedMultiplier = i - 48; De snelheid-multiplier ingesteld op een bereik 1-10 van ASCII ingangen 0-9
EEPROM.write (EEPROM_speedMultiplier, speedMultiplier);
Als (DEBUGGING) {Serial.println(speedMultiplier);}
De LED te bevestigen de nieuwe instelling van de netwerkoverdrachtssnelheid knipperen
voor (int speedBlink = 1; speedBlink < = speedMultiplier; speedBlink ++) {}
digitalWrite (ledPin, hoge); de LED aangezet
delay(100);
digitalWrite (ledPin, laag); verrekening van de LED
delay(100);
}
} else if (com [0] == 'c') {/ / kalibreren centreren PWM instellingen voor beide servo's ex: "c 90 90"
int valueLeft = 90, valueRight = 90;
sscanf (com, "c %d van %d", valueLeft, & valueRight); De inbreng van meerdere waarden parseren
servoCenterLeft = valueLeft;
servoCenterRight = valueRight;
stopTime = driveWheels(0,0); De servo's met 0 waarde die moet leiden tot geen beweging heeft plaatsgevonden wanneer correct gekalibreerd rijden
servosActive = true;
EEPROM.write (EEPROM_servoCenterLeft, servoCenterLeft);
EEPROM.write (EEPROM_servoCenterRight, servoCenterRight);
Als (DEBUGGING) {}
Serial.Print ("geijkte servo centers");
Serial.Print(servoCenterLeft);
Serial.Print ("en");
Serial.println(servoCenterRight);
}
} else if (killerid (com, "i") == 0) {/ / servo naar oneindige actieve modus in-/ uitschakelen zodat het niet automatisch time-out
servosForcedActive =! servosForcedActive; Stop alleen als gevaarlijk
EEPROM.write (EEPROM_servosForcedActive, servosForcedActive);
Als (DEBUGGING) {}
Serial.Print ("oneindige rotatie om van een knevel gevoorzid");
Als (servosForcedActive){Serial.println("on");}
else {Serial.println("off");}
}
} else if (com [0] == 'w') {/ / "wiel" opdracht te verwerken en te vertalen naar PWM waarden ex: "w-100 100" [bereik loopt van-100 tot 100]
int valueLeft = 90, valueRight = 90;
sscanf (com, "w %d van %d", valueLeft, & valueRight); De inbreng van meerdere waarden parseren
stopTime = driveWheels (valueLeft, valueRight);
servosActive = true;
} else if (killerid (com, "reset") == 0) {/ / reset de eeprom-instellingen
setEepromsToDefault();
} 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);
}
}
}

Hoofdlus uitgevoerd te allen tijde
void loop
{
readSerialInput();
checkIfStopBot();
}