4 servo drive CellBot que puede ser controlado remotamente. (8 / 8 paso)

Paso 8: El código de Arduino adaptado para mi uso.

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

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

#define BUFFERSIZE 20
#define InferFPin 0 //Front pin analógico de inferir Sensor
#define InferBPin 1 //Back pin analógico de inferir Sensor
#define SonarPin 2 //sonar pin analógico
#define LeftEarPin 3 //left pin analógico de oído
#define RightEarPin 4 //right pin analógico de oído /
#define VoltPin 5 //analog pin 5 utilizado para la detección de VoltageCheck.
#define LEDBlue 5
#define LEDUFO 6
#define LEDPlataforma 7
#define servoPinHead 8 / / Servo controla el ángulo del teléfono
#define FLpin 9
#define FRpin 10
#define RLpin 11
#define RRpin 12
#define ledPin 13 / / LED se enciende durante la ejecución de servos
#define regulador 20

#define OneDeg 5,65 //setting retraso rotacional por igual a un grado (alfombra de 2 ruedas)
#define OneDeg 9.04 //setting retraso rotacional por igual a un grado (rueda 2 de la madera dura)
Comprobación de tensión de la batería resultó para ser lo más difícil, intenté.  Tener la batería conectada y USB conectado...
da lecturas diferentes y sólo la batería conectada.  De hecho las lecturas analógicas se revirtieron de alguna manera.
#define shutdownVoltageCheck 978 / / este número es único para cada divisor de tensión y batería combo.  Básicamente es el 20% de la mina.
#define analogvoltconstant 144.6969

int iAnVal; entrada de sensor de sonar
int grados;
int InferF = 0;
int InferB = 0;
int soundRT [72];
int soundLF [72];
int distancia [72];
int tempvolt;
unsigned BatteryIndex largo = 0;
unsigned BatteryIndexThreshold largo = 150000; unos 10 minutos.
RegulatorIndex largo sin firmar;
unsigned RegulatorTimerThreshold largo = 150000; intervalo de encender o apagar el regulador de tensión. unos 10 minutos.
int VoltSampleSize = 1000;
char ='s dir'; cero la dirección el bot ha dicho a impulsar -, atrás, derecha, izquierda, parada, establece que deje de funcionar.
int potencia =-1;
Boolean ConservePower = true; Esto determina si el bot utiliza energía modo de ahorro.
calentamiento de int = 1000; retraso para iniciar la instalación en milisegundos.

Ninguna configuración requerida para estos parámetros
Boolean servosActive = false; asumir los servos no se mueve cuando empezamos
stopRegulatorIndex=millis() largo sin firmar; utilizado para calcular el funcionamiento RegulatorIndex para servos
char incomingByte; Entrada serie valores
char mensaje [8]; Para pasar mensajes nuevo serial
inBytes Char [BUFFERSIZE]; Buffer para la serie de mensajes
int serialIndex = 0;
int serialAvail = 0;

** GAMA encontrar ***-ajustes son para Telémetros ultrasónicos. OK para lave como-es si no tiene en su robot
larga dist, microsegundos, cm, pulgadas; Utilizado por el telémetro para calcular distancias
const int rangePinForward = 2; Pin digital de telémetro hacia adelante (para distancia de objeto frente bot)
const int rangeToObjectMargin = 25; (bot parará cuando la distancia está más cerca de este - conjunto a 0 si no hay ningún sensor)
const int rangePinForwardGround = 0; Pin digital de telémetro frente hacia abajo en la parte delantera (para límite de detección de mesa)
const int rangeToGroundMargin = 0; Gama en cm para la tabla (bot se detendrá cuando la distancia es superior a este valor 0 si no hay ningún sensor)
const int rangeSampleCount = 3; Número de lecturas a tomar y para un valor más estable

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

Serial.println ("start setup!");
Delay(WarmUp);

modo de salida entrada set ***
pinMode (LEDBlue, salida);
pinMode (LEDUFO, salida);
pinMode (LEDPlataforma, salida);
pinMode (servoPinHead, salida);
pinMode(FLpin,OUTPUT);
pinMode(FRpin,OUTPUT);
pinMode(RLpin,OUTPUT);
pinMode(RRpin,OUTPUT);
pinMode (ledPin, salida);
pinMode (regulador, salida);

digitalWrite(servoPinHead,0);

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

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

//***************************** Clear Sensor Map ************************
para (grados = 0; grados < 72 grados = grados + 1) {}
soundRT [grados] = 0;
soundLF [grados] = 0;
distancia [grados] = 0;
}

por defecto para servos que apague ***
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();

LEDOff();

pinMode (regulador, salida); configurar pin 20 a modo de salida para que el regulador de VoltageCheck se puede activar y desactivar.
digitalWrite (regulador, bajo); iniciar el regulador de VoltageCheck en la posición de apagado.

Serial.println ("final setup!");

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

Responde a en serie y maneja la pausa y descarga los datos para hacer frente a Android Comunicación serial
{} void serialReply (char * sensorname, char * tmpmsg)
Serial3.Print(sensorname);
Serial3.Print(":");
Serial3.println(tmpmsg); Enviar el mensaje nuevamente hacia afuera de la línea serie
Espere a que el depurador serial a callarse
Delay(200); Este es un número mágico
Serial3.Flush(); borra todos los datos de entrada
}

Compruebe si ha transcurrido suficiente RegulatorIndex para detener el bot y si es seguro continuar
Boolean checkIfStopBot() {}
int tempfrontsonar = Averagefive(SonarPin);
Serial.Print ("frente de sonar:");
Serial.println(tempfrontsonar);
int tempfrontinfer = Averagefive(InferFPin);
Serial.Print ("frente de inferir:");
Serial.println(tempfrontinfer);
int tempbackinfer = Averagefive(InferBPin);
Serial.Print ("back inferir:");
Serial.println(tempbackinfer);
Serial.println();

Si ((dir == 'f') & & ((tempfrontsonar < rangeToObjectMargin) || (tempfrontinfer < 200))) {
STOP();
servosActive = false;
verdaderas;
} else if (dir == 'b') {}
Si (! () (tempbackinfer < 650) & & (tempbackinfer > 450))) {}
STOP();
servosActive = false;
verdaderas;
}
}
devuelven el valor false;
}

Enviar comando al dispositivo Bluetooth conectado para iniciar la sincronización
void pairBluetooth() {}
Serial3.Print("\r\n+INQ=1\r\n"); Se trata de Seeedstudio maestro/esclavo (modificar según sea necesario para su modelo)
}

Lee entrada en serie si está disponible y analiza el comando cuando se ha enviado el comando completo.
void readSerialInput() {}
serialAvail = Serial3.available();
Leer lo que está disponible
para (int i = 0; i < serialAvail; i ++) {}
Almacenar en búfer.
inBytes [+ serialIndex] = Serial3.read();
Busque el final del comando.

Si (inBytes [+ serialIndex] == '\n' || inBytes [+ serialIndex] == ';' || inBytes [+ serialIndex] == ' >') {//Use; cuando se utiliza Monitor Serial
inBytes [+ serialIndex] = '\0'; final de caracteres de cadena
parseCommand(inBytes);
serialIndex = 0;
}
Else {}
esperaba más del comando para venir más tarde.
serialIndex += serialAvail;
}
}
}

Limpia y analiza el comando
void parseCommand(char* com) {}
Si (com [0] == '\0') {vuelta}; //bit de comprobación de errores
int Inicio = 0;
comenzar del comando
mientras que (com [Inicio]! = ' <') {}
Inicio ++;
Si (com [Inicio] == '\0') {}
no hay. Debe ser versión antigua
Inicio = -1;
rotura;
}
}
Inicio ++;
Cambiar de puesto al principio
int i = 0;
mientras que (com [i + start - 1]! = '\0') {}
com [i] = com [start + i];
i ++;
}
performCommand(com);
}

void LEDWhite() {}
digitalWrite (7, bajo);
digitalWrite (6, bajo);
digitalWrite (5, bajo);
}

void LEDOff() {}
digitalWrite (7, alto);
digitalWrite (6, alto);
digitalWrite (5, alto);
}

void Detach() {}
Serial.println ("separar servos!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
Delay(150);
}

void Attach() {}
Serial.println ("fije servos!");
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
digitalWrite (regulador, alta);
}

void Forew() {}
Serial.println ("*** adelante!");
dir = 'f';
Si (no checkIfStopBot()) {}
Attach();
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
RegulatorIndex = 0;
servosActive = true;
}
}

anular el remanso () {}
int Tempdist = Averagefive(InferFPin);
Serial.Print ("distancia al revés:");
Serial.println(Tempdist);
Serial.println();
dir = 'b';
Si (no checkIfStopBot()) {}
Serial.println ("*** hacia atrás!");
Attach();
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
RegulatorIndex = 0;
servosActive = true;
}
}

void Stop () {}
Serial.println ("*** Stop!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
dir = de ';
RegulatorIndex = 0;
serialReply ("i", "st"); Decirle al teléfono que el robot parado ###
servosActive = false;
}

vacío dejado {})
Serial.println ("*** gire izquierda!");
Attach();
myservoFR.write(180);
myservoFL.write(180);
myservoRR.write(180);
myservoRL.write(180);
dir = 'l';
RegulatorIndex = 0;
servosActive = true;
}

anular (derecha) {}
Serial.println ("*** gire derecho!");
Attach();
myservoFR.write(0);
myservoFL.write(0);
myservoRR.write(0);
myservoRL.write(0);
dir = 'r';
RegulatorIndex = 0;
servosActive = true;
}

void CircleRt () {}
Serial.println ("*** círculo derecho!");
Attach();
myservoFR.write(95);
myservoFL.write(0);
myservoRR.write(95);
myservoRL.write(0);
RegulatorIndex = 0;
}

void CircleLf () {}
Serial.println ("*** círculo izquierda!");
Attach();
myservoFR.write(180);
myservoFL.write(85);
myservoRR.write(180);
myservoRL.write(85);
RegulatorIndex = 0;
}

void mydelay (int grados) {}
Serial.println ("mydelay Inicio");
retardo (int(OneDeg * degress));
Serial.println ("mydelay final");
}

int Averagefive (int listenpin) {}
int promedio = 12;
escuchar [media]; int
index1 int;
int highindex = 0;
int alto = 0;
int lowindex = 0;
baja int = 1000;
int total = 0;

para (index1 = 0; index1 < media; index1 index1 = 1) {}
escuchar [index1] = analogRead(listenpin);
Serial.println(Listen[index1]);
}

para (index1 = 0; index1 < media; index1 index1 = 1) {}
Si (escuchar [index1] > alto) {}
alta = escuchar [index1];
highindex = index1;
}
}

para (index1 = 0; index1 < media; index1 index1 = 1) {}
Si (escuchar [index1] < bajo) {}
bajo = escuchar [index1];
lowindex = index1;
}
}

para (index1 = 0; index1 < media; index1 index1 = 1) {}
Si ((index1! = highindex) & & (index1! = lowindex)) {}
total = total + escuchar [index1];
}
}

total = int (total / (promedio-2));
Serial.Print ("promedio:");
Serial.println(total);
retorno total;
}

{} void de Listen()
Serial.println ("=== escuchar ===!");
para (grados = 0; grados < 71; deg = grados + 1) {}
soundRT [grados] = Averagefive(RightEarPin); escuchar a la oreja derecha y asignar valor a la matriz
soundLF [grados] = Averagefive(LeftEarPin); escuchar a la oreja izquierda y asignar valor a la matriz
distancia [grados] = Averagefive(SonarPin); leer un valor del sensor
Serial.println(Distance[DEG]); valor de indicación escrita en la matriz de distancia para esta dirección
Right();
mydelay(5);
STOP();
}
STOP();
}

void Course() {}
int BestDistDeg = 0;
int BestDist = 0;
para (grados = 0; grados < 71; deg = grados + 1) {}
soundRT [grados] = analogRead(RightEarPin); escuchar a la oreja derecha y asignar valor a la matriz
soundLF [grados] = analogRead(LeftEarPin); escuchar a la oreja izquierda y asignar valor a la matriz
Si (distancia [grados + 1] > distance[BestDistDeg]) {}
BestDistDeg = grados + 1;
}
Serial.Print("BestDist=");
Serial.println(Distance[BestDistDeg]);
Serial.println(BestDistDeg); valor de indicación escrita en la matriz de distancia para esta dirección
}
Left();
mydelay((71-BestDistDeg) * 5); Girar derecha hasta curso de coincidencia más larga distancia.
STOP();
Forew();
}

void ForewardBackward() {}
Serial.println ("*** prueba de atrás hacia adelante comienzo!");
Forew();
Delay(2000);
STOP();
Delay(2000);
Backw();
Delay(2000);
STOP();
Delay(2000);
Serial.println ("*** prueba de atrás hacia adelante final!");
}

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 ("envío de 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 final");
}

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

void forwardinfertest() {}
Delay(500);
Serial.Print ("infiere adelante senor:");
Serial.println(analogRead(InferFPin));
umbral de peligro < 200
}

void backwardinfertest() {}
Delay(500);
Serial.Print ("Señor infiere al revés:");
Serial.println(analogRead(InferBPin));
umbral de peligro < 300
}

void sonartest() {}
Delay(500);
Serial.Print ("Señor de Sonar:");
Serial.println(analogRead(SonarPin));
umbral de peligro < 30
}

int VoltageCheck() {}

index1 int = 0;
doble voltsum = 0;
int analogvolt = 0;

para (index1 = 0; index1 < VoltSampleSize; index1 index1 = 1) {}
voltsum = voltsum + analogRead(VoltPin);
}
analogvolt = int (voltsum / VoltSampleSize);
Serial.Print ("VoltageCheck:");
Serial.Print ("analag voltios:");
Serial.println(analogvolt);
Serial.Print ("voltios reales:");
Serial.println ((analogvolt / analogvoltconstant));
Serial.println();
volver analogvolt;
}

void BatteryCheckLED()
{
Si (VoltageCheck() > shutdownVoltageCheck) {}
digitalWrite (LEDPlataforma, bajo);
digitalWrite (LEDUFO, alto);
} else {}
digitalWrite (LEDPlataforma, alto);
digitalWrite (LEDUFO, LOW);
}
BatteryIndex = 0;
}

void RegulatorControlToggle()
{
energía = potencia * -1;
Si (potencia == 1) {}
digitalWrite (regulador, alta);
}
Si (potencia == -1) {}
digitalWrite (regulador, bajo);
}
RegulatorIndex = 0;
}

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

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

void performCommand(char* com) {}
voltios de flotador = 0;

Si (strcmp (com, "f") == 0) {/ / adelante
Forew();
} else if (strcmp (com, "r") == 0) {/ / derecha
Right();
} else if (strcmp (com, "l") == 0) {/ / izquierda
Left();
} else if (strcmp (com, "b") == 0) {/ / al revés
Backw();
} else if (strcmp (com, "s") == 0) {/ / Stop
STOP();
} else if (strcmp (com, "fr") == 0 || strcmp (com, "fz") == 0 || strcmp (com, "x") == 0) {/ impresión de revestimiento delantero sensor de distancia
Dist = Averagefive(SonarPin);
itoa (dist, msg, 10); Vuelven lo dist int a char
serialReply ("x", msg); Enviar la distancia a la línea serie
} else if (strcmp (com, "z") == 0) {/ / lectura y tierra impresión frente a distancia
Dist = Averagefive(SonarPin); Dist = getDistanceSensor(rangePinForwardGround);
itoa (dist, msg, 10); Vuelven lo dist int a char
serialReply ("z", msg); Enviar la distancia a la línea serie
Serial.println(Dist);
} else if (strcmp (com, "v") == 0) {/ / leer e imprimir VoltageCheck
voltios = VoltageCheck();
itoa (voltios, msg, 10); Convertir el int voltios en un char
Serial.println(Volt);
serialReply ("v", msg); Enviar la distancia a la línea serie
} else if (strcmp (com, "h") == 0) {/ / ayuda modo - depuración de palanca
Signalblink();
} else if (com [0] == ' n ') {/ / mover la cabeza hacia arriba
sscanf (com, «n %d» & lastNeckValue); Analizar la contribución de varios valores
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
EEPROM.write (EEPROM_lastNeckValue, lastNeckValue);
Si {} (depuración)
Serial.Print ("cuello a");
Serial.println(lastNeckValue);
//}
} else if (com [0] == 'p') {/ / inicia Bluetooth emparejamiento así que otro dispositivo se puede conectar
pairBluetooth();
} else {}
serialReply ("e", com); / / Echo comando desconocido nuevo
Si {} (depuración)
Serial.Print ("comando desconocido:");
Serial.println(com);
// }

0-9, w, n, w, i, c, h, d de demostración,
}
}

=== Principal lazo en RegulatorIndexs todos ===
void loop()
{
readSerialInput();
checkIfStopBot();

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

Si Miser() (ConservePower);
}