Etape 12 : Le Code...
/ * Arduino avoider obstacle et suiveur. But dans la vie... Se déplace vers l’avenir pour obstacles à éviter ou donjon-loin d’eux (Force) écrite par Scott Beasley - 2015 gratuit pour utiliser ou modifier. Profitez. * / / * Utilise la bibliothèque L9110S. Il fonctionne avec le pont en h L9110S. Voir l’étape 1 pour le code de la bibliothèque. * / #include < L9110Driver.h > #include < Servo.h > / / définit la distance de lecture fonction du bot. #define turn() (left_dist > = right_dist) ? go_left (): go_right () #define microsecondsToCentimeters(microseconds) (unsigned long) microsecondes / 29,1 / 2.0 #define MIN_ACTION_DIST 20 / / 20 cm #define MIN_AWAY_DIST 20 / / 20 cm du bot / / changement ces définit si vous utilisez différents pins #define ECHO_PIN 8 / / Digital pin 2 #define TRIGGER_PIN 3 / / Digital pin 3 #define SERVO_LR 4 / / broches pour balayage gauche/droite servo #define pinAIN1 5 / / define I1 interface #define pinAIN2 6 / / define I2 interface #define pinBIN1 9 / / define I3 interface #define pinBIN2 11 / / définir l’interface de I4 #define MODE_PIN 2 / / modes Bot #define MODE_AVOID 0 #define MODE_FORCE 1 / / vitesse définit #define MAXFORWARDSPEED 225 / / Max vitesse nous voulons aller vers l’avant #define MAXBACKWARDSPEED 225 / / inverse Max vitesse #define TOPSPEED 255 / / utilisé pour aider à transformer le mieux sur les tapis et les surfaces plus rugueuses. Divers temps retards utilisés pour conduire et servo #define TURNDELAY 475 #define BACKUPDELAY 400 #define SERVOMOVEDELAY 200 #define SERVOSEARCHDELAY 85 #define DEBOUNCE_TM 20 / / 20ms / * zone Globals. * / / / Créer les objets de servo moteur, pour s’interfacer avec L9110_Motor motor_left (pinAIN1, pinAIN2) ; Créez l’objet de moteur gauche L9110_Motor motor_right (pinBIN1, pinBIN2) ; Créer droit moteur objet Servo sensor_servo ; Créer un objet de servo pour notre capteur de distance / / Global variables byte sweep_pos = 0 ; Position actuelle de la sonde servo octets pos_index = 90 ; mode octets = MODE_AVOID ; mode_change octets = true ; unsigned long last_pin_change ; unsigned long left_dist, right_dist ; Distance mesurée left et right / / fonction pour gérer le commutateur de mode avec debounce (Sub checkMode) {mode_val octets = digitalRead (MODE_PIN); if ((longue)(millis () - last_pin_change) > = DEBOUNCE_TM) {last_pin_change = millis (); / / change le Mode du bot est po si (mode_val == faible) mode = MODE_AVOID ; if (mode_val == HIGH) mode = MODE_FORCE ; mode_change == true;}} void (setup) {/ / décommenter si vous devez déboguer. //Serial.begin (9600); / / Set Serial monitor à 9600 bauds //Serial.println ("My suivre/éviter bot est départ vers le haut! ») ; Veillez à ce que les moteurs sont éteints au début halt () ; sensor_servo.Attach (SERVO_LR) ; Fixer le servo SERVO_LR sensor_servo.write (90) ; Mettre le servo au milieu pos (neutres) / / mode de réglage de distance capteur broches pinMode (entrée, ECHO_PIN) ; Définir broche écho comme entrée pinMode (TRIGGER_PIN, sortie) ; Définir l’ergot comme sortie pinMode (entrée, MODE_PIN) ; Broche de mode réglé comme entrée / / retard pour donner le temps de l’utilisateur de faire des ajustements. Supprimer une fois fait. délai (30000) ; } Sub boucle () {checkMode (); / / lire la valeur courante de commutateur / / si la mode a changé, re-centrer le servo du capteur si (mode_change == true) {mode_change = false ; pos_index = 90 ; sweep_pos = 0; sensor_servo.write (pos_index);} si (mode == MODE_AVOID) éviter (); / / essayer de ne pas frapper n’importe quoi! si (mode == MODE_FORCE) force (); / / keep-loin de la main!} vide éviter () {unsigned long dist_fwd; / / rotation du capteur de distance que nous longeons rotate_sensor (); / / donner le temps de servo pour arriver à positionner pour s’installer à retard (SERVOSEARCHDELAY); / / Obtenir une lecture de la dist_fwd de direction capteur actuel = ping () ; Serial.Print ("lecture de capteur à Distance:") ; Serial.println (dist_fwd) ; Aller de l’avant alors que rien n’est dans les capteurs de distance lue zone si (dist_fwd > MIN_ACTION_DIST || dist_fwd == 0) {go_forward ();} else / / il y a quelque chose dans les capteurs lire la zone {stopper (); / / Stop! go_backward (); / / sauvegarder un peu de retard (BACKUPDELAY); mettre fin (); / / Stop! sensor_read () ; Distance de lecture gauche et droite tourner () ; Tournez vers le retard de chemin d’accès plus clair (TURNDELAY) ; cesser (de) ; Remettre à la position intermédiaire avant de déménager à nouveau pos_index = 90 ; sweep_pos = 0 ; sensor_servo.Write (pos_index) ; }} void force () {unsigned long obj_dist ; obj_dist = ping (); //Serial.print ("lecture de capteur à Distance:") ; Serial.println (obj_dist) ; Aller de l’avant alors que rien n’est dans les capteurs de distance lue zone si (obj_dist > = (MIN_AWAY_DIST + 20)) {stopper (); go_forward ();} ElseIf (obj_dist < = (MIN_AWAY_DIST + 5)) {stopper (); go_backward (); / / Reset à une position intermédiaire avant de passer à nouveau pos_index = 90 ; sweep_pos = 0; sensor_servo.write (pos_index);} retard (250) ; } / / Lire le capteur après nous trouvons quelque chose de la manière. Cela aide à trouver une nouvelle / / chemin void sensor_read () {//Serial.println ("serveur à 40 degrés.."); retard (SERVOMOVEDELAY), sensor_servo.write (40); right_dist = ping (); //Look à la //Serial.println droite ("Servo à 140 degrés.."); sensor_servo.write (140), retard (SERVOMOVEDELAY); left_dist = ping (); / / regarder à gauche / / redéfinir le servo sur le centre pos //Serial.println ("Servo à 90 deg.."); sensor_servo.write (90);} / / rotation du servo de capteur à 45deg incréments Sub rotate_sensor () {si (sweep_pos < = 0) {pos_index = 45;} ElseIf (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) ; } / / Lire la HC-SR04 uSonic capteur unsigned long ping () {/ / déclencher le capteur uSonic (HC-SR04) d’envoyer un ping digitalWrite (TRIGGER_PIN, basse), delayMicroseconds (5); digitalWrite (TRIGGER_PIN, HIGH); delayMicroseconds (10); digitalWrite (TRIGGER_PIN, basse); / / mesurer combien de temps le ping a pris et convertir en retour de cm (microsecondsToCentimeters (pulseIn (ECHO_PIN, élevé)));} void go_forward () {//Serial.println ("aller de l’avant...") ; Les moteurs jusqu'à la vitesse de rampe. Aider avec spining out sur quelques surfaces et la vaisselle et la tare sur ramp_it_up de la GM (MAXFORWARDSPEED, FORWARD, BACKWARD) ; Ensemble à l’ensemble de la vitesse fixée juste au cas où la rampe dernier TVA n’était pas tous / / il. motor_left.setSpeed (MAXFORWARDSPEED) ; motor_right.setSpeed (MAXFORWARDSPEED) ; motor_left.Run (FORWARD| SORTIE) ; motor_right.Run (BACKWARD| SORTIE) ; } void go_backward () {//Serial.println ("en reculant...") ; Les moteurs jusqu'à la vitesse de rampe. Aider avec spining out sur certaines surfaces et la vaisselle et la tare sur ramp_it_up de la GM (MAXBACKWARDSPEED, en arrière, vers l’avant) ; Ensemble à l’ensemble de la vitesse fixée juste au cas où la rampe dernier TVA n’était pas tous / / il. motor_left.setSpeed (MAXBACKWARDSPEED) ; motor_right.setSpeed (MAXBACKWARDSPEED) ; motor_left.Run (BACKWARD| SORTIE) ; motor_right.Run (FORWARD| SORTIE) ; } void go_left () {//Serial.println ("Going gauche...") ; Les moteurs jusqu'à la vitesse de rampe. Aider avec spining out sur certaines surfaces et la vaisselle et la tare sur ramp_it_up de la GM (TOPSPEED, en arrière, vers l’arrière) ; Ensemble à l’ensemble de la vitesse fixée juste au cas où la rampe dernier TVA n’était pas tous / / il. motor_left.setSpeed (TOPSPEED) ; motor_right.setSpeed (TOPSPEED) ; motor_left.Run (BACKWARD| SORTIE) ; motor_right.Run (BACKWARD| SORTIE) ; } void go_right () {//Serial.println ("va droit...") ; Les moteurs jusqu'à la vitesse de rampe. Aider avec spining out sur certaines surfaces et la vaisselle et la tare sur ramp_it_up de la GM (TOPSPEED, vers l’avant, vers l’avant) ; Ensemble à l’ensemble de la vitesse fixée juste au cas où la rampe dernier TVA n’était pas tous / / il. motor_left.setSpeed (TOPSPEED) ; motor_right.setSpeed (TOPSPEED) ; motor_left.Run (FORWARD| SORTIE) ; motor_right.Run (FORWARD| SORTIE) ; } void halt () {//Serial.println ("halte!"); motor_left.setSpeed (0); motor_right.setSpeed (0); motor_left.run (frein); motor_right.run (frein);} void ramp_it_up (vitesse d’uint8_t, uint8_t lf_dir, uint8_t rt_dir) {uint8_t ramp_val = 0, step_val = 0; step_val = abs (vitesse / 4); pour (uint8_t j’ai = 0; j’ai < 4; i ++) {ramp_val += step_val ; motor_left.setSpeed (ramp_val); motor_right.setSpeed (ramp_val), motor_left.run (lf_dir| SORTIE) ; motor_right.Run (rt_dir| SORTIE) ; retard (25) ; } }
