Enkla multi-mode 4wd Rover JR-001 (12 / 15 steg)

Steg 12: Koden...

/*
Arduino hinder avoider och efterföljare.

Mål i livet...
Flyttar framåt söker hinder att undvika eller
Håll borta från dem (kraft)

Skriven av Scott Beasley - 2015
Gratis att använda eller ändra. Njuta av.
*/

/*
Använder L9110S-biblioteket. Det fungerar med L9110S h-bron. Se steg 1 för biblioteket koden.
*/

#include < L9110Driver.h >
#include < Servo.h >

Definierar för avståndet behandlingen funktion av bot.
#define turn() (left_dist > = right_dist)? go_left (): go_right)
#define microsecondsToCentimeters(microseconds) (osignerade långa) mikrosekunder / 29,1 / 2,0
#define MIN_ACTION_DIST 20 / / 20 cm
#define MIN_AWAY_DIST 20 / / 20cm från bot

Ändra dessa definierar om du använder olika pins
#define ECHO_PIN 8 / / Digital stift 2
#define TRIGGER_PIN 3 / / Digital stift 3
#define SERVO_LR 4 / / Pin för vänster/höger scan servo
#define pinAIN1 5 / / definiera I1 gränssnitt
#define pinAIN2 6 / / definiera I2 gränssnitt
#define pinBIN1 9 / / definiera I3 gränssnitt
#define pinBIN2 11 / / definiera I4 gränssnitt
#define MODE_PIN 2

Bot lägen
#define MODE_AVOID 0
#define MODE_FORCE 1

Speed definierar
#define MAXFORWARDSPEED 225 / / Max hastighet vi vill flytta fram
#define MAXBACKWARDSPEED 225 / / Max reverse hastighet
#define TOPSPEED 255 / / används för att omvandla bättre på mattan och grövre ytor.

Olika tidsfördröjningar används för körning och servo
#define TURNDELAY 475
#define BACKUPDELAY 400
#define SERVOMOVEDELAY 200
#define SERVOSEARCHDELAY 85
#define DEBOUNCE_TM 20 / / 20ms

/*
Globals område.
*/

Skapa de motor, servo objekt att samverka med
L9110_Motor motor_left (pinAIN1, pinAIN2); Vänster motor objekt skapas
L9110_Motor motor_right (pinBIN1, pinBIN2); Skapa rätt motor objekt
Servo sensor_servo; Skapa ett servo objekt för avstånd sensor

Globala variabler
byte sweep_pos = 0; Aktuella position sensor servo
byte pos_index = 90.
byte läge = MODE_AVOID;
byte mode_change = sant;
osignerade långa last_pin_change;
osignerade långa left_dist, right_dist; Avståndet mätt vänster och höger

funktion för att hantera inställningsreglaget med debounce
void checkMode)
{
byte mode_val = digitalRead (MODE_PIN);

om ((lång)(millis () - last_pin_change) > = DEBOUNCE_TM) {
last_pin_change = millis ();
Förändringar i läge bot är i.
om (mode_val == låg)
läge = MODE_AVOID;

om (mode_val == hög)
läge = MODE_FORCE;

mode_change == true;
}
}

Ogiltiga inställningar)
{
Avkommentera om du behöver felsöka.
Serial.BEGIN (9600); Ange Serial monitor på 9600 baud
Serial.println ("My Följ/undvika bot är startande upp!");

Se till att motorerna är av början
stoppa ();

sensor_servo.attach (SERVO_LR); Bifoga servo SERVO_LR
sensor_servo.write (90). Inställd den mellersta (neutral) pos servo

Ställa in lägen för avstånd sensor stift
pinMode (ECHO_PIN, indata); Ställ in echo pin som indata
pinMode (TRIGGER_PIN, OUTPUT); Ange trigger pin
pinMode (MODE_PIN, indata); Inställda läge pin som indata

Dröjsmål att ge användartid att göra justeringar. Ta bort efter gjort.
fördröjning (30000);
}

Ogiltig loop)
{
checkMode (); Läs aktuella switch-värdet

Om läget förändrats, centrera sensor servo
om (mode_change == true) {
mode_change = false;
pos_index = 90.
sweep_pos = 0;
sensor_servo.write (pos_index);
}

om (läge == MODE_AVOID)
undvika (); Försök att inte slå något!

om (läge == MODE_FORCE)
kraft (); Håll borta från handen!
}

void undvika)
{
osignerade långa dist_fwd;

Rotera avståndet sensorn som vi kör längs
rotate_sensor ();

De servo möjlighet att få placera för att få fast
fördröjning (SERVOSEARCHDELAY);

Få en läsning från nuvarande sensor riktning
dist_fwd = ping ();
Serial.Print ("avstånd sensor läsa:");
Serial.println (dist_fwd);

Gå framåt samtidigt som ingenting är i avstånd sensorerna läser område
om (dist_fwd > MIN_ACTION_DIST || dist_fwd == 0)
{
go_forward ();
}
annat / / det är något i sensorerna läsa område
{
stoppa (); Stanna!
go_backward (); Backa upp lite
fördröjning (BACKUPDELAY);
stoppa (); Stanna!

sensor_read (); Läs avstånd vänster och höger

vända (); Vända mot den tydligaste väg
fördröjning (TURNDELAY);
stoppa ();

Återställa till mellanläge innan du flyttar igen
pos_index = 90.
sweep_pos = 0;
sensor_servo.write (pos_index);
}
}

void kraft)
{
osignerade långa obj_dist;

obj_dist = ping ();
Serial.Print ("avstånd sensor läsa:");
Serial.println (obj_dist);

Gå framåt samtidigt som ingenting är i avstånd sensorerna läser område
om (obj_dist > = (MIN_AWAY_DIST + 20)) {
stoppa ();
go_forward ();
} else om (obj_dist < = (MIN_AWAY_DIST + 5)) {
stoppa ();
go_backward ();

Återställa till mellanläge innan du flyttar igen
pos_index = 90.
sweep_pos = 0;
sensor_servo.write (pos_index);
}

fördröjning (250);
}

Läs sensorn efter vi hitta något i vägen. Detta hjälper till att hitta en ny
sökväg
void sensor_read)
{
Serial.println ("Server på 40 deg...");
sensor_servo.write (40);
fördröjning (SERVOMOVEDELAY);
right_dist = ping (); Ser till höger

Serial.println ("Servo på 140 deg...");
sensor_servo.write (140);
fördröjning (SERVOMOVEDELAY);
left_dist = ping (); Titta till vänster

Ställa in servo tillbaka till center pos
Serial.println ("Servo på 90 deg...");
sensor_servo.write (90).
}

Rotera sensor servo på 45deg steg
void rotate_sensor)
{
om (sweep_pos < = 0) {
pos_index = 45;
}
annars om (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);
}

Läs HC-SR04 uSonic sensorn
osignerade långa ping)
{
Utlösa uSonic sensorn (HC-SR04) att skicka ut en ping
digitalWrite (TRIGGER_PIN, låg);
delayMicroseconds (5).
digitalWrite (TRIGGER_PIN, hög);
delayMicroseconds (10).
digitalWrite (TRIGGER_PIN, låg);

Mät hur lång ping tog och konvertera till cm's
Return (microsecondsToCentimeters (pulseIn (ECHO_PIN, hög)));
}

void go_forward)
{
Serial.println ("framöver...");

Ramp motorer upp till hastigheten.
Hjälp med spining ut på vissa ytor och ware och tare på GM: s
ramp_it_up (MAXFORWARDSPEED, framåt, bakåt);

Set för den inställda hastigheten alla bara incase rampen förra moms var inte alla
det.
motor_left.setSpeed (MAXFORWARDSPEED);
motor_right.setSpeed (MAXFORWARDSPEED);
motor_left.Run (FORWARD| UTGÅVA);
motor_right.Run (BACKWARD| UTGÅVA);
}

void go_backward)
{
Serial.println ("går bakåt...");

Ramp motorer upp till hastigheten.
Hjälp med spining ut på vissa ytor och ware och tare på GM: s
ramp_it_up (MAXBACKWARDSPEED, bakåt, framåt);

Set för den inställda hastigheten alla bara incase rampen förra moms var inte alla
det.
motor_left.setSpeed (MAXBACKWARDSPEED);
motor_right.setSpeed (MAXBACKWARDSPEED);
motor_left.Run (BACKWARD| UTGÅVA);
motor_right.Run (FORWARD| UTGÅVA);
}

void go_left)
{
Serial.println ("kommer vänster...");

Ramp motorer upp till hastigheten.
Hjälp med spining ut på vissa ytor och ware och tare på GM: s
ramp_it_up (TOPSPEED, bakåt, bakåt);

Set för den inställda hastigheten alla bara incase rampen förra moms var inte alla
det.
motor_left.setSpeed (TOPSPEED);
motor_right.setSpeed (TOPSPEED);
motor_left.Run (BACKWARD| UTGÅVA);
motor_right.Run (BACKWARD| UTGÅVA);
}

void go_right)
{
Serial.println ("kommer rätt...");

Ramp motorer upp till hastigheten.
Hjälp med spining ut på vissa ytor och ware och tare på GM: s
ramp_it_up (TOPSPEED, framåt, framåt);

Set för den inställda hastigheten alla bara incase rampen förra moms var inte alla
det.
motor_left.setSpeed (TOPSPEED);
motor_right.setSpeed (TOPSPEED);
motor_left.Run (FORWARD| UTGÅVA);
motor_right.Run (FORWARD| UTGÅVA);
}

void stopp)
{
Serial.println ("stopp!");
motor_left.setSpeed (0);
motor_right.setSpeed (0);
motor_left.Run (broms);
motor_right.Run (broms);
}

void ramp_it_up (uint8_t hastighet, uint8_t lf_dir, uint8_t rt_dir)
{
uint8_t ramp_val = 0, step_val = 0;

step_val = abs (hastighet / 4);
för (uint8_t jag = 0; jag < 4; i ++) {
ramp_val += step_val;
motor_left.setSpeed (ramp_val);
motor_right.setSpeed (ramp_val);
motor_left.Run (lf_dir| UTGÅVA);
motor_right.Run (rt_dir| UTGÅVA);
fördröjning (25).
}
}