Created
August 6, 2014 08:33
-
-
Save ctring/bd3a0e5c3736a42b247b to your computer and use it in GitHub Desktop.
Source code of my obstacle avoidance robot for Tiva C Launchpad
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <Servo.h> | |
// Obstacle avoiding program - Tiva C Launchpad | |
// Cuong T. Nguyen - 4/15/2014 | |
/* Code for controling an obstacle avoidance robot, which has two DC drivers | |
interface with a L298N modul and a HC-SR04 range sensor mounted on a servo motor. | |
Range sensor | |
VCC: VBus | |
GND: GND | |
TrigPin: PC4 37 | |
EchoPin: PC5 36 | |
L298N: | |
In1: PE1 27 | |
In2: PE2 28 | |
In3: PE3 29 | |
In4: PE4 5 | |
Servo: | |
VCC: VBus | |
GND: GND | |
Signal: PB3 38 | |
*/ | |
//======================================== | |
#define TrigPin 37 | |
#define EchoPin 36 | |
#define ServoSignal 38 | |
#define DCIn1 27 | |
#define DCIn2 28 | |
#define DCIn3 29 | |
#define DCIn4 5 | |
const int MAX_DISTANCE = 35; // cm | |
const int TURN_LEFT_DELAY = 320; // ms | |
const int TURN_RIGHT_DELAY = 320; // ms | |
const int RED = 0; | |
const int GREEN = 1; | |
const int BLUE = 2; | |
const int YELLOW = 3; | |
const int NOCOLOR = 4; | |
Servo ServoController; | |
//============LED Driver================= | |
void turnOnOne(int x) { | |
digitalWrite(RED_LED, LOW); | |
digitalWrite(GREEN_LED, LOW); | |
digitalWrite(BLUE_LED, LOW); | |
if (x == RED) digitalWrite(RED_LED, HIGH); | |
if (x == GREEN) digitalWrite(GREEN_LED, HIGH); | |
if (x == BLUE) digitalWrite(BLUE_LED, HIGH); | |
if (x == YELLOW) { | |
digitalWrite(RED_LED, HIGH); | |
digitalWrite(GREEN_LED, HIGH); | |
} | |
} | |
//======================================= | |
//============Motor Driver=============== | |
void stop() { | |
turnOnOne(RED); | |
digitalWrite(DCIn1, LOW); | |
digitalWrite(DCIn2, LOW); | |
digitalWrite(DCIn3, LOW); | |
digitalWrite(DCIn4, LOW); | |
} | |
void goStraight() { | |
turnOnOne(GREEN); | |
digitalWrite(DCIn1, LOW); | |
digitalWrite(DCIn2, HIGH); | |
digitalWrite(DCIn3, LOW); | |
digitalWrite(DCIn4, HIGH); | |
} | |
void turnLeft() { | |
turnOnOne(BLUE); | |
digitalWrite(DCIn1, HIGH); | |
digitalWrite(DCIn2, LOW); | |
digitalWrite(DCIn3, LOW); | |
digitalWrite(DCIn4, HIGH); | |
} | |
void turnRight() { | |
turnOnOne(YELLOW); | |
digitalWrite(DCIn1, LOW); | |
digitalWrite(DCIn2, HIGH); | |
digitalWrite(DCIn3, HIGH); | |
digitalWrite(DCIn4, LOW); | |
} | |
//======================================= | |
//=============Servo Driver============== | |
void lookFront() { | |
ServoController.write(70); | |
delay(500); | |
} | |
void lookLeft() { | |
ServoController.write(180); | |
delay(500); | |
} | |
void lookRight() { | |
ServoController.write(0); | |
delay(500); | |
} | |
//======================================= | |
//===========Range Sensor================ | |
double getDistance() { // in centimeter | |
double distance; | |
int duration; | |
digitalWrite(TrigPin, HIGH); | |
delayMicroseconds(1000); | |
digitalWrite(TrigPin, LOW); | |
duration = pulseIn(EchoPin, HIGH); | |
distance = (duration/2) / 29.1; | |
return distance; | |
} | |
//======================================= | |
void setup() | |
{ | |
pinMode(EchoPin, INPUT); | |
pinMode(TrigPin, OUTPUT); | |
pinMode(DCIn1, OUTPUT); | |
pinMode(DCIn2, OUTPUT); | |
pinMode(DCIn3, OUTPUT); | |
pinMode(DCIn4, OUTPUT); | |
pinMode(RED_LED, OUTPUT); | |
pinMode(GREEN_LED, OUTPUT); | |
pinMode(BLUE_LED, OUTPUT); | |
ServoController.attach(ServoSignal); | |
tooClose(); | |
} | |
int tooClose() { | |
lookFront(); | |
int d = getDistance(); | |
return d <= MAX_DISTANCE; | |
} | |
void decideAndTurn() { | |
double dLeft, dRight, dFront; | |
lookLeft(); | |
dLeft = getDistance(); | |
lookRight(); | |
dRight = getDistance(); | |
lookFront(); | |
dFront = getDistance(); | |
if (dLeft > dRight && dLeft > dFront) { | |
turnLeft(); | |
delay(TURN_LEFT_DELAY); | |
stop(); | |
} | |
else if (dRight > dFront) { | |
turnRight(); | |
delay(TURN_RIGHT_DELAY); | |
stop(); | |
} | |
else { | |
goStraight(); | |
} | |
} | |
void loop() | |
{ | |
delay(5); | |
if (tooClose()) { | |
digitalWrite(RED_LED, HIGH); | |
stop(); | |
decideAndTurn(); | |
} | |
else { | |
digitalWrite(RED_LED, LOW); | |
goStraight(); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Can you add your digitalwrite() function code