Paso 12: El código...
/ * Arduino evita el obstáculo y seguidor. Meta en la vida... Se mueve hacia adelante en busca de obstáculos para evitar o mantener lejos de ellos (fuerza) escrito por Scott Beasley - 2015 gratis para usar o modificar. Disfrutar. * / / * Utiliza la biblioteca L9110S. Funciona con el puente h L9110S. Vea el paso 1 para el código de biblioteca. * / #include < L9110Driver.h > #include < Servo.h > / / define la distancia en función del bot de la lectura. ¿#define turn() (left_dist > = right_dist)? (go_left): go_right (#define) microsecondsToCentimeters(microseconds) (largo sin signo) microsegundos / 29,1 y 2.0 #define MIN_ACTION_DIST 20 / / #define MIN_AWAY_DIST 20 de 20 cm / 20 cm de bot / / cambio estos define si utilizas diferentes pernos #define ECHO_PIN 8 / / Digital pin 2 #define TRIGGER_PIN 3 / / Digital pin 3 #define SERVO_LR 4 / / Pin de exploración izquierda servo #define pinAIN1 5 / / definir I1 interfaz #define pinAIN2 6 / / definir I2 interfaz #define pinBIN1 9 / / definir I3 interfaz #define pinBIN2 11 / / definir interfaz de I4 #define MODE_PIN 2 / / modos de Bot #define MODE_AVOID 0 #define MODE_FORCE 1 / / velocidad define #define MAXFORWARDSPEED 225 / / máxima velocidad queremos moviendo adelante #define MAXBACKWARDSPEED 225 / reversa máxima velocidad #define TOPSPEED 255 / usados para ayudar a girar mejor en alfombras y superficies más ásperas. Vario tiempo demoras para conducción y servo #define TURNDELAY 475 #define BACKUPDELAY 400 #define SERVOMOVEDELAY 200 #define SERVOSEARCHDELAY 85 #define DEBOUNCE_TM 20 / / 20ms / * área de Globals. * / / / Crear los objetos servo motor, interfaz con L9110_Motor motor_left (pinAIN1, pinAIN2); Crear objeto motor izquierdo L9110_Motor motor_right (pinBIN1, pinBIN2); Crear derecho motor objeto Servo sensor_servo; Crea un objeto servo para nuestro sensor de distancia / / Global variables byte sweep_pos = 0; Posición actual de la sensor servo byte pos_index = 90; modo de octeto = MODE_AVOID; byte mode_change = true; last_pin_change largo sin firmar; unsigned left_dist largo, right_dist; Distancia medida izquierda y derecha / / función para manejar el selector de modo con debounce void checkMode () {byte mode_val = digitalRead (MODE_PIN); si ((largo)(millis () - last_pin_change) > = DEBOUNCE_TM) {last_pin_change = millis (); / / cambia el modo del bot es pulg si (mode_val == baja) modo = MODE_AVOID; si (mode_val == HIGH) modo = MODE_FORCE; mode_change == true;}} void setup () {/ / descomentar si necesita depurar. //Serial.begin (9600); / monitor sistema Serial a 9600 baudios //Serial.println ("mi seguimiento, evitar bot es puesta en marcha!"); Asegúrese que los motores estén apagados en principio detener (); sensor_servo.Attach (SERVO_LR); Conecte el servo SERVO_LR sensor_servo.write (90); Fijar el servo a la posición media (neutra) / modos de ajuste de distancia sensor pines pinMode (ECHO_PIN, entrada); Configurar pin de eco como la entrada pinMode (TRIGGER_PIN, salida); Configurar pin de gatillo como salida pinMode (MODE_PIN, entrada); Pin de modo conjunto como entrada / retardo para dar tiempo a hacer los ajustes del usuario. Quitar después. retardo (30000); } void loop () {checkMode (); / leer valor de interruptor actual / / si ha cambiado el modo, volver a centrar el servo sensor si (mode_change == true) {mode_change = false; pos_index = 90; sweep_pos = 0; sensor_servo.write (pos_index);} si (modo == MODE_AVOID) evitar (); / / trate de no golpear nada! si (modo == MODE_FORCE) fuerza (); / / mantener lejos de la mano!} vacío evitar () {dist_fwd largo sin firmar; / / gire el sensor de la distancia que nos conduzca a lo largo de rotate_sensor (); / dará el tiempo servo a la posición para conseguir colocado demora (SERVOSEARCHDELAY); / / Obtener una lectura de la dist_fwd de dirección de sensor actual = (ping); Serial.Print ("distancia de lectura del sensor:"); Serial.println (dist_fwd); Mientras no está en los sensores de distancia información sobre si (dist_fwd > MIN_ACTION_DIST || dist_fwd == 0) {go_forward ();} otro / / hay algo en los sensores de información {detener (); / / Stop! go_backward (); / / copia de seguridad de un poco de retraso (BACKUPDELAY), detener (); / / Stop! sensor_read (); Distancia de lectura izquierda y derecha gire (); Gire hacia el retraso de ruta más claro (TURNDELAY); detener (); Restablecer posición media antes de pasar otra vez pos_index = 90; sweep_pos = 0; sensor_servo.Write (pos_index); }} void fuerza () {obj_dist largo sin firmar; obj_dist = ping (); //Serial.print ("distancia de lectura del sensor:"); Serial.println (obj_dist); Mientras no está en los sensores de distancia información sobre si (obj_dist > = (MIN_AWAY_DIST + 20)) {detener (); go_forward ();} else if (obj_dist < = (MIN_AWAY_DIST + 5)) {detener (); go_backward (); / / Reset a posición media antes de pasar otra vez pos_index = 90; sweep_pos = 0; sensor_servo.write (pos_index);} delay (250); } / / Leer el sensor después encontramos algo en el camino. Esto ayuda a encontrar un nuevo / / ruta anular (sensor_read) {//Serial.println ("servidor en 40 DEG..."); sensor_servo.write (40); delay (SERVOMOVEDELAY); right_dist = ping (); //Look a la //Serial.println derecha ("Servo en 140 grados..."); sensor_servo.write (140); delay (SERVOMOVEDELAY); left_dist = ping (); / / mira a la izquierda / / establecer el servo en el centro pos //Serial.println ("Servo a 90 deg..."); sensor_servo.write (90);} / / girar el servo del sensor a 45 º incrementos void rotate_sensor () {si (sweep_pos < = 0) {pos_index = 45;} else if (sweep_ POS > = 180) {pos_index = -45;} //Serial.print ("pos_index ="); Serial.println (pos_index); sweep_pos += pos_index; Serial.Print ("sweep_pos ="); Serial.println (sweep_pos); sensor_servo.Write (sweep_pos); } / / Leer el uSonic HC-SR04 sensor sin firmar largo ping () {/ / activar el sensor uSonic (HC-SR04) para enviar un ping digitalWrite (TRIGGER_PIN, bajo) delayMicroseconds (5); digitalWrite (TRIGGER_PIN, alto); delayMicroseconds (10); digitalWrite (TRIGGER_PIN, LOW); / / medir cuánto tiempo tomó el ping y convertir al regreso de cm (microsecondsToCentimeters (pulseIn (ECHO_PIN alto)));} void go_forward () {//Serial.println ("va adelante..."); Los motores hasta la velocidad de la rampa. Ayuda con spining hacia fuera en algunas superficies y mercancías y Tara en ramp_it_up de la GM (MAXFORWARDSPEED, adelante, atrás); Conjunto a toda la velocidad fijada sólo en caso la rampa IVA pasado no fue todo de / / se. motor_left.SETSPEED (MAXFORWARDSPEED); motor_right.SETSPEED (MAXFORWARDSPEED); motor_left.Run (FORWARD| COMUNICADO); motor_right.Run (BACKWARD| COMUNICADO); } void go_backward () {//Serial.println ("ir hacia atrás..."); Los motores hasta la velocidad de la rampa. Ayuda con spining hacia fuera en algunas superficies y mercancías y Tara en ramp_it_up de la GM (MAXBACKWARDSPEED, atrás, adelante); Conjunto a toda la velocidad fijada sólo en caso la rampa IVA pasado no fue todo de / / se. motor_left.SETSPEED (MAXBACKWARDSPEED); motor_right.SETSPEED (MAXBACKWARDSPEED); motor_left.Run (BACKWARD| COMUNICADO); motor_right.Run (FORWARD| COMUNICADO); } void go_left () {//Serial.println ("ir izquierdo..."); Los motores hasta la velocidad de la rampa. Ayuda con spining hacia fuera en algunas superficies y mercancías y Tara en ramp_it_up de GM (TOPSPEED, al revés, al revés); Conjunto a toda la velocidad fijada sólo en caso la rampa IVA pasado no fue todo de / / se. motor_left.SETSPEED (TOPSPEED); motor_right.SETSPEED (TOPSPEED); motor_left.Run (BACKWARD| COMUNICADO); motor_right.Run (BACKWARD| COMUNICADO); } void go_right () {//Serial.println ("va derecho..."); Los motores hasta la velocidad de la rampa. Ayuda con spining hacia fuera en algunas superficies y mercancías y Tara en ramp_it_up de GM (TOPSPEED, adelante, adelante); Conjunto a toda la velocidad fijada sólo en caso la rampa IVA pasado no fue todo de / / se. motor_left.SETSPEED (TOPSPEED); motor_right.SETSPEED (TOPSPEED); motor_left.Run (FORWARD| COMUNICADO); motor_right.Run (FORWARD| COMUNICADO); } (alto vacío) {//Serial.println ("alto!"); motor_left.setSpeed (0); motor_right.setSpeed (0); motor_left.run (freno); motor_right.run (freno);} void ramp_it_up (uint8_t velocidad, uint8_t lf_dir, uint8_t rt_dir) {uint8_t ramp_val = 0, step_val = 0; step_val = abs (velocidad / 4); para (uint8_t i = 0; i < 4; i ++) {ramp_val += step_val; motor_left.setSpeed (ramp_val); motor_right.setSpeed (ramp_val), motor_left.run (lf_dir| COMUNICADO); motor_right.Run (rt_dir| COMUNICADO); retraso (25); } }
