Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /* Pin Constants (editable) */
- //Pins for the ultrasonic sensor
- const int pingPin = 12;
- const int echoPin = 11;
- //Wheels
- const int leftWheelEnable = 9;
- const int leftWheelForward = 10;
- const int leftWheelBackward = 8;
- const int rightWheelEnable = 7;
- const int rightWheelForward = 5;
- const int rightWheelBackward = 6;
- //White line detector
- const int whiteSensor0 = A0;
- const int whiteSensor1 = A1;
- const int whiteSensor2 = A2;
- //Pins for the binary distance sensors on the side
- const int leftFrontSensor = 4;
- const int leftBackSensor = 2;
- const int rightFrontSensor = A3;
- const int rightBackSensor = 3;
- /* Structures */
- enum State {
- SEARCHING,
- CHASING,
- ON_BORDER,
- AVOIDING
- };
- enum Direction {
- FORWARD,
- BACKWARD
- };
- /* Logic Constants */
- //When sensor reads over this value, it interprets it as a white line (border)
- const int whiteLineThreshold0 = 700;
- const int whiteLineThreshold1 = 700;
- const int whiteLineThreshold2 = 700;
- //When ultrasonic sensor detects object closer than this value then it will interpret it as an enemy;
- const int enemySightDistance = 10;
- //Globals
- State state = SEARCHING;
- Direction direction = FORWARD;
- unsigned long time;
- void setup() {
- // put your setup code here, to run once:
- Serial.begin(9600);
- pinMode(leftWheelForward, OUTPUT);
- pinMode(leftWheelBackward, OUTPUT);
- pinMode(rightWheelForward, OUTPUT);
- pinMode(rightWheelBackward, OUTPUT);
- pinMode(leftWheelEnable, OUTPUT);
- pinMode(rightWheelEnable, OUTPUT);
- digitalWrite(leftWheelEnable, HIGH);
- digitalWrite(rightWheelEnable, HIGH);
- pinMode(pingPin, OUTPUT);
- pinMode(echoPin, INPUT);
- time = millis();
- }
- void loop() {
- //Update time
- unsigned long deltaTime = millis() - time;
- time = millis();
- int serialState = 0;
- if(Serial.available() > 0) {
- serialState = Serial.read();
- }
- //if(serialState == '1') { Do bluetooth (potem sie zrobi)
- if(!avoidBorder()) {
- if(!avoidEnemiesFromSide()) {
- searchAndChase();
- }
- }
- //}
- }
- void searchAndChase() {
- if(state == SEARCHING) {
- if(enemyInFront()) {
- state = CHASING;
- direction = FORWARD;
- move();
- }
- else {
- rotateLeft();
- }
- }
- else if(state == CHASING) {
- if(!enemyInFront()) {
- state = SEARCHING;
- }
- else {
- move();
- }
- }
- }
- bool avoidEnemiesFromSide() {
- if(enemyOnSide(true, true) || enemyOnSide(true, false) || enemyOnSide(false, false) || enemyOnSide(false, true)) {
- direction = BACKWARD;
- state = AVOIDING;
- move();
- return true;
- }
- else {
- if(state = AVOIDING) {
- state = SEARCHING;
- }
- return false;
- }
- }
- bool avoidBorder() {
- if(state == ON_BORDER) {
- if(onBorder()) {
- move();
- return true;
- }
- else {
- state = SEARCHING;
- return false;
- }
- }
- else {
- if(onBorder()) {
- state = ON_BORDER;
- switchDirection();
- move();
- return true;
- }
- else {
- return false;
- }
- }
- }
- void switchDirection() {
- direction = FORWARD ? BACKWARD : FORWARD;
- }
- void move() {
- if(direction == FORWARD) {
- forward();
- }
- else {
- backward();
- }
- }
- void rotateLeft() {
- digitalWrite(leftWheelForward, HIGH);
- digitalWrite(leftWheelBackward, LOW);
- digitalWrite(rightWheelForward, LOW);
- digitalWrite(rightWheelBackward, HIGH);
- }
- void rotateRight() {
- digitalWrite(leftWheelForward, HIGH);
- digitalWrite(leftWheelBackward, LOW);
- digitalWrite(rightWheelForward, HIGH);
- digitalWrite(rightWheelBackward, LOW);
- }
- void forward() {
- digitalWrite(leftWheelForward, HIGH);
- digitalWrite(leftWheelBackward, LOW);
- digitalWrite(rightWheelForward, HIGH);
- digitalWrite(rightWheelBackward, LOW);
- }
- void backward() {
- digitalWrite(leftWheelForward, LOW);
- digitalWrite(leftWheelBackward, HIGH);
- digitalWrite(rightWheelForward, LOW);
- digitalWrite(rightWheelBackward, HIGH);
- }
- //Reads distance to the closest object in front of a bot (in centimeters)
- long readDistance() {
- digitalWrite(pingPin, LOW);
- delayMicroseconds(2);
- digitalWrite(pingPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(pingPin, LOW);
- long duration = pulseIn(echoPin, HIGH);
- long cm = duration / 29 / 2;
- return cm;
- }
- bool enemyOnSide(bool left, bool front) {
- if(front) {
- if(left) {
- return digitalRead(leftFrontSensor) == LOW;
- }
- else {
- return analogRead(rightFrontSensor) < 300;
- }
- }
- else {
- if(left) {
- return digitalRead(leftBackSensor) == LOW;
- }
- else {
- return digitalRead(rightBackSensor) == LOW;
- }
- }
- }
- bool onBorder() {
- int value0 = analogRead(whiteSensor0);
- int value1 = analogRead(whiteSensor1);
- int value2 = analogRead(whiteSensor2);
- if(value0 < whiteLineThreshold0) {
- return true;
- }
- if(value1 < whiteLineThreshold1) {
- return true;
- }
- if(value2 < whiteLineThreshold2) {
- return true;
- }
- return false;
- }
- bool enemyInFront() {
- return readDistance() < enemySightDistance;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement