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

Steg 7: Arduino koden (oförändrad)

Ser ut för en uppsättning av ASCII-tecken i signal-skicka
kommandon till en uppsättning av servon för att köra en liten robot. LED stift #13
förblir tänd under servo rörelse och blink för hastighet ändras.

Den minsta kretsen:
* LED från stift 13 till marken (eller använda inbyggda LED på de flesta Arduino)
* Servon med signal ledningar anslutna till stift 3 och 5 (5v power och marken för
servon kan också kopplas till Arduino, eller makt kan komma från extern källa)
* Seriell in ansluten till RX stift 0
* Seriell produktion ansluten till TX stift 1

Ytterligare kretsar (valfritt):
* Framåt inför ultraljud avståndsmätare på digital stift 7
* Nedåt inför ultraljud avståndsmätare på digital stift 8

Obs: Om du inte ännu har en seriell enhet ansluta med, kan du använda den
inbyggd Serial Monitor i programvaran Arduino när ansluta via USB för att testa.
Också noga med att bringa ur fattningen RX & TX pins från andra enheter när du försöker programmera
Arduinoen via USB.

skapad 2010
av Tim Heath, Ryan Hickman och Glen Arrowsmith
Besök http://www.cellbots.com för mer information
*/

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

#define BUFFERSIZE 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 falskt
#define DEFAULT_servosForcedActive falskt
#define DEFAULT_lastNeckValue 255

** ALLMÄNNA inställningar **-allmänna inställningar
booleska felsökning = false; Om felsökning utdata över följetong är på av defauly (kanna bli nonchalansen med "h" kommando)
CONST int ledPin = 13. Tänds när du kör servon
char * driveType = "servo"; Använd "motor" när robotar har en DC motor drivrutin eller "servo" för servon driver hjulen

** SERVO inställningar **-konfigurerbara värden baserade på stift används och servo riktning
CONST int servoPinLeft = 9;
CONST int servoPinRight = 10;
CONST int servoPinHead = 12; Servo kontrollera vinkeln på telefonen
CONST int servoDirectionLeft = 1; Använd antingen 1 eller -1 för omvänd
CONST int servoDirectionRight = -1; Använd antingen 1 eller -1 för omvänd
int servoCenterLeft = DEFAULT_servoCenterLeft; PWM inställning för ingen rörelse på vänster servo
int servoCenterRight = DEFAULT_servoCenterLeft; PWM inställning för ingen rörelse på rätt servo
int servoPowerRange = 30. PWM utbud av center som servon svarar bäst (ställa till 30 att fungera i intervallet 60-120 från mitten av 90)
CONST long maxRunTime = 2000. Maximal körtid för servon utan ytterligare kommando. * Bör använda ett kommando till sätta den här. *
int speedMultiplier = DEFAULT_speedMultiplier; Standardinställningen för hastighet. Använder ett spektrum från 1-10
int lastNeckValue = DEFAULT_lastNeckValue;

** MOTOR DRIVRUTINSINSTÄLLNINGAR ** - för användning med styrelser som Pololu motor föraren (också använder vänster/höger servo pin-inställningar ovan)
int leftMotorPin_1 = 9;
int leftMotorPin_2 = 8;
int rightMotorPin_1 = 10;
int rightMotorPin_2 = 11;
int motor_stby = 12;

** UTBUD att hitta ***-följande inställningar finns 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 = 7. Digital pin för framåt inför range finder (för objektet avstånd framför bot)
CONST int rangeToObjectMargin = 0; Intervall i cm framåt objekt (bot slutar när avstånd närmare än denna - satt till 0 om ingen sensor)
CONST int rangePinForwardGround = 8; 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; Antalet olika värden att ta och i genomsnitt för ett mer stabilt värde

Skapa servo objekt för att kontrollera servon
Servo myservoLeft;
Servo myservoRight;
Servo myservoHead;

Ingen config krävs för dessa parametrar
booleska servosActive = false; anta servon inte rör sig när vi börjar
booleska servosForcedActive = DEFAULT_servosForcedActive; kommer bara sluta när anses vara farliga
osignerade långa stopTime=millis(); används för att beräkna bearbetningstid 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;

void setup() {
pinMode (servoPinLeft, produktionen);
pinMode (servoPinRight, produktionen);
pinMode (servoPinHead, produktionen);
pinMode(leftMotorPin_1,OUTPUT);
pinMode(leftMotorPin_2,OUTPUT);
pinMode(rightMotorPin_1,OUTPUT);
pinMode(rightMotorPin_2,OUTPUT);
pinMode (ledPin, produktionen);
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);
om (lastNeckValue! = DEFAULT_lastNeckValue) {
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
}
}

Säkert läser EEPROM
int readSetting (int memoryLocation, int defaultValue) {
int värde = EEPROM.read(memoryLocation);
om (värde == 255) {
EEPROM.write (memoryLocation, standardvärde);
}
returnera värdet;
}

Uppsättningar EEPROM inställningarna till standardvärdena
void 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);
om (felsökning) {
Serial.println ("alla EEPROM värden anges till standardvärden.");
}
}

Konvertera riktad textkommandon ("framåt" / "bakåt") till beräknade servo hastighet
int directionValue (char * directionCommand, int servoDirection) {
om (directionCommand == "vidarebefordra") {
tillbaka (10 * speedMultiplier * servoDirection);
}
annat if (directionCommand == "bakåt") {
tillbaka (-10 * speedMultiplier * servoDirection);
}
annat {
om (felsökning) {Serial.println ("Houston, vi har ett problem!");}
Return 0; Försöket att sätta värde på mittpunkten - detta ska inte behövas
}
}

Översätta textkommandon till PWM värden för bot att flytta (vänster servo kommandot, rätt servo kommando)
osignerade långa moveBot (char * commandLeft, char * commandRight) {
int valueLeft = directionValue (commandLeft, servoDirectionLeft) + servoCenterLeft;
int valueRight = directionValue (commandRight, servoDirectionRight) + servoCenterRight;
driveWheels (valueLeft, valueRight);
}

Enheten servo eller DC motorer om du vill flytta roboten med värden i intervallet -100 till 100 för vänster och höger
osignerade långa driveWheels (int valueLeft, int valueRight) {
Lossa både servo stift som kommer att sluta gnälla och vitalisera de motorerna så de inte döda den kompass behandlingen
om (valueLeft == 0 och valueRight == 0) {
myservoLeft.detach();
myservoRight.detach();
}
Driva hjulen baserat på "servo" driveType
om (driveType == "servo") {
valueLeft = valueLeft * servoDirectionLeft; Flip positiv till negativ om det behövs baserat på servo riktning värde inställningen
valueRight = valueRight * servoDirectionRight;
Karta "w" värden till de snävt intervall som servon svarar på
valueLeft = karta (valueLeft, -100, 100, (servoCenterLeft - servoPowerRange), (servoCenterLeft + servoPowerRange));
valueRight = karta (valueRight, -100, 100, (servoCenterRight - servoPowerRange), (servoCenterRight + servoPowerRange));
digitalWrite (ledPin, hög); ställa in lysdioden på
Starta om servo PWM och skicka dem kommandon
myservoLeft.attach(servoPinLeft);
myservoRight.attach(servoPinRight);
myservoLeft.write(valueLeft);
myservoRight.write(valueRight);
Spotta ut lite diagnos info över följetong
om (felsökning) {
Serial.Print ("röra vänster servo");
Serial.Print (valueLeft, DEC);
Serial.Print (och rätt servo");
Serial.println (valueRight, DEC);
}
}
Driva hjulen baserat på "motor" driveType
annat {
Ange vänster motor stift att vända i önskad riktning
om (valueLeft < 0) {
digitalWrite(leftMotorPin_1,LOW);
digitalWrite(leftMotorPin_2,HIGH);
}
annat {
digitalWrite(leftMotorPin_1,HIGH);
digitalWrite(leftMotorPin_2,LOW);
}
Ange rätt motor stift att vända i önskad riktning
om (valueRight < 0) {
digitalWrite(rightMotorPin_1,LOW);
digitalWrite(rightMotorPin_2,HIGH);
}
annat {
digitalWrite(rightMotorPin_1,HIGH);
digitalWrite(rightMotorPin_2,LOW);
}
Kartor "w" värden till den bredare utbud som motorn svarar på
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; Ställ in tid att stoppa körs baserat på tillåtna körtid
återvända stopTime;
}

Stoppa bot
void stopBot() {
driveWheels(0,0);
digitalWrite (ledPin, låg); Inaktivera LED
om (felsökning) {Serial.println ("stoppa båda hjulen");}
serialReply ("i", "st"); Berätta för den telefon som roboten stannade
}

Läsa och behandla värden från en Ultraljuds avståndsmätare (du kan lämna denna kod i även om du inte har ett)
lång getDistanceSensor(int ultrasonicPin) {
Ta flera avläsningar och genomsnittlig dem
mikrosekunder = 0;
för (int prov = 1; prova < = rangeSampleCount; prova ++) {
Parallaxen PING))) utlöses av en hög puls av 2 eller fler mikrosekunder.
Ge en kort låg puls förhand för att säkerställa en ren hög puls:
Maxsonar verkar inte behöver denna del men det gör inte ont heller
pinMode (ultrasonicPin, produktionen);
digitalWrite (ultrasonicPin, låg);
delayMicroseconds(2);
digitalWrite (ultrasonicPin, hög);
delayMicroseconds(5);
digitalWrite (ultrasonicPin, låg);

Samma PIN-koden används för att läsa signalen från den ultrasonic detektorn: 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 (ultrasonicPin, ingång);
mikrosekunder += pulseIn (ultrasonicPin, hög);
delayMicroseconds(5); Mycket kort paus mellan behandlingarna
}
mikrosekunder = mikrosekunder / rangeSampleCount;
Konvertera genomsnitt sensorn läser till centimeter och lämna tillbaka den
cm = microsecondsToCentimeters(microseconds);
tum = microsecondsToInches(microseconds);
om (felsökning) {
Serial.Print ("Micro:"); Serial.Print(microseconds);
Serial.Print ("Inches:"); Serial.Print(inches);
Serial.Print ("cm:"); Serial.println(cm);
}
returnera cm;
}

lång microsecondsToCentimeters(long microseconds) {
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;
}

lång microsecondsToInches(long microseconds) {
Enligt Parallaxs datablad för PING))), det finns
73.746 mikrosekunder per tum (dvs. ljud resor på 1130 fot per
det andra). Detta ger vägsträcka som ping, utgående
och tillbaka, så vi delar med 2 att få distansera av hindret.
Se: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
Samma sak gäller för MaxSonar av MaxBotix
återvända mikrosekunder / 74 / 2;
}

Svar ut över följetong och hanterar pausa och spola data att hantera Android seriell kommunikation
void serialReply (char * sensorname, char * tmpmsg) {
Serial.Print(sensorname);
Serial.Print(":");
Serial.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
Serial.flush(); rensar alla inkommande data
}

Kontrollerar utbud finders att se om det är säkert att fortsätta flytta (* måste lägga till sätt att veta vilken riktning vi är rörliga *)
booleska safeToProceed() {
booleska safe = false; Anta det är inte säkert att fortsätta
Kontrollera avståndet till det närmaste objektet framför bot och stoppa om för nära
om (rangeToObjectMargin! = 0) {/ / bryr sig inte skicka om marginalen anges till noll eftersom det låser sig när ingen sensor närvarande
dist = getDistanceSensor(rangePinForward);
om (dist > rangeToObjectMargin) {
säker = sant;
}
annars om (felsökning) {Serial.print ("objekt för nära i front -");}
}
Kontrollera avståndet till marken framför bot att se till att bordet är fortfarande kvar
om (rangeToGroundMargin! = 0) {/ / bryr sig inte skicka om marginalen anges till noll eftersom det låser sig när ingen sensor närvarande
dist = getDistanceSensor(rangePinForwardGround);
om (dist > rangeToGroundMargin) {
säker = sant;
}
annars om (felsökning) {Serial.print ("slutet av ytan nått -");}
}
om (rangeToGroundMargin == 0 & & rangeToObjectMargin == 0) {return true;}
återvända säkert;
}

Kontrollera om tillräckligt med tid har förflutit för att stoppa bot och om det är säkert att gå vidare
void checkIfStopBot() {
om (inte servosForcedActive och servosActive och (stopTime < millis() eller inte safeToProceed())) {
stopBot();
servosActive = false;
} else om (inte safeToProceed()) {
stopBot();
servosActive = false;
}
}

Skicka kommando till anslutna Bluetooth enheten att initiera ihopkopplingen
void pairBluetooth() {
Serial.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 = Serial.available();
Läsa vad som finns
för (int jag = 0; jag < serialAvail; i ++) {
Lagra i buffert.
inBytes [jag + serialIndex] = Serial.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 performCommand(char* com) {
om (strcmp (com, "f") == 0) {/ / framåt
stopTime = driveWheels (speedMultiplier * 10, speedMultiplier * 10);
servosActive = sant;
} else om (strcmp (com, "r") == 0) {/ / höger
stopTime = driveWheels (speedMultiplier * 10, speedMultiplier * -10);
servosActive = sant;
} else om (strcmp (com, "l") == 0) {/ / vänster
stopTime = driveWheels (speedMultiplier * -10, speedMultiplier * 10);
servosActive = sant;
} else om (strcmp (com, "b") == 0) {/ / bakåt
stopTime = driveWheels (speedMultiplier * -10, speedMultiplier * -10);
servosActive = sant;
} else om (strcmp (com, "s") == 0) {/ / stopp
stopBot();
servosActive = false;
} else om (strcmp (com, "fr") == 0 || strcmp (com, "fz") == 0 || strcmp (com, "x") == 0) {/ / Läs och skriv ut framåt mot avstånd sensor
dist = getDistanceSensor(rangePinForward);
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 = 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
} else om (strcmp (com, "h") == 0) {/ / hjälp-läge - felsökning växla
Skriva ut några grundläggande instruktioner när du först slår på felsökning
om (inte felsökning) {
Serial.println ("redo att lyssna på kommandon! Försök ågra av dessa: ");
Serial.println ("F (framåt), B (bakåt), L (vänster), R (höger), S (stop), D (demo)");
Serial.println ("också använda siffrorna 1-9 för att justera hastighet (0 = långsam, 9 = fast).");
}
FELSÖKNING =! FELSÖKNING.
} else om (strcmp (com, "1") == 0 || strcmp (com, "2") == 0 || strcmp (com, "3") == 0 || strcmp (com, "4") == 0 || strcmp (com, "5") == 0 || strcmp (com, "6") == 0 || strcmp (com, "7") == 0 || strcmp (com, "8") == 0 || strcmp (com, "9") == 0 || strcmp (com, "0") == 0) {
Jag vet att det föregående villkoret är knepigt men det kommer att ändras snart
om (felsökning) {Serial.print ("ändra hastighet till");}
int jag = com [0];
speedMultiplier = i - 48. Ställa in hastighet multiplikatorn till en rad 1-10 från ASCII-ingångar 0-9
EEPROM.write (EEPROM_speedMultiplier, speedMultiplier);
om (felsökning) {Serial.println(speedMultiplier);}
Blinka lysdioden att bekräfta den nya inställningen hastighet
för (int speedBlink = 1; speedBlink < = speedMultiplier; speedBlink ++) {
digitalWrite (ledPin, hög); ställa in lysdioden på
Delay(100);
digitalWrite (ledPin, låg); iväg för LED
Delay(100);
}
} else om (com [0] == "c") {/ / kalibrera centrera PWM inställningar för båda servon ex: "c 90 90"
int valueLeft = 90, valueRight = 90.
sscanf (com, "c %d %d", & valueLeft, och valueRight); Tolka indata till flera värden
servoCenterLeft = valueLeft;
servoCenterRight = valueRight;
stopTime = driveWheels(0,0); Driva servon med 0 värde som bör leda till ingen rörelse när kalibreras korrekt
servosActive = sant;
EEPROM.write (EEPROM_servoCenterLeft, servoCenterLeft);
EEPROM.write (EEPROM_servoCenterRight, servoCenterRight);
om (felsökning) {
Serial.Print ("kalibrerad servo centers till");
Serial.Print(servoCenterLeft);
Serial.Print ("och");
Serial.println(servoCenterRight);
}
} else om (strcmp (com, "i") == 0) {/ / växla servo till oändlig aktivt läge så det inte tid till automatiskt
servosForcedActive =! servosForcedActive; Stoppa bara när farliga
EEPROM.write (EEPROM_servosForcedActive, servosForcedActive);
om (felsökning) {
Serial.Print ("oändliga rotation växlas till");
om (servosForcedActive){Serial.println("on");}
annat {Serial.println("off");}
}
} else om (com [0] == "w") {/ / hanterar "hjulet" kommando och översätta till PWM värden ex: "w-100 100" [intervall är från -100 till 100]
int valueLeft = 90, valueRight = 90.
sscanf (com, "w %d %d", & valueLeft, och valueRight); Tolka indata till flera värden
stopTime = driveWheels (valueLeft, valueRight);
servosActive = sant;
} else om (strcmp (com, "reset") == 0) {/ / återställer eeprom
setEepromsToDefault();
} 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);
}
}
}

Huvudloop vid alla tider
void loop()
{
readSerialInput();
checkIfStopBot();
}