v2.0: Use sonar sensor to detect distance to walls and obstacles;

v2.1: Check current consumption to know when is stuck and need to get out;

WheelBot-01/Brain.ino

@@ -0,0 +1,62 @@
+void lookAroundAndTurn() {
+  int leftDistance = getDistanceOnTheLeft();
+  int rightDistance = getDistanceOnTheRight();
+
+  // decide where to go
+  if (leftDistance > rightDistance) {
+    turnLeft();
+  } else {
+    turnRight();
+  }
+}
+
+int getDistanceOnTheLeft() {
+  int distanceOnTheLeft;
+
+  // turn backwards to left
+  leftBackward(SPEED_MEDIUM, 0);
+  rightBackward(SPEED_LOW, 0);
+  delay(300);
+  brakeOn(200);
+
+  distanceOnTheLeft = getDistanceInCm();
+
+  // get back to straight position
+  leftForward(SPEED_MEDIUM, 0);
+  rightForward(SPEED_LOW, 0);
+  delay(300);
+  brakeOn(200);
+
+  return distanceOnTheLeft;
+}
+
+int getDistanceOnTheRight() {
+  int distanceOnTheRight;
+
+  // turn backwards to right
+  leftBackward(SPEED_LOW, 0);
+  rightBackward(SPEED_MEDIUM, 0);
+  delay(300);
+  brakeOn(200);
+
+  distanceOnTheRight = getDistanceInCm();
+
+  // get back to straight position
+  leftForward(SPEED_LOW, 0);
+  rightForward(SPEED_MEDIUM, 0);
+  delay(300);
+  brakeOn(200);
+
+  return distanceOnTheRight;
+}
+
+boolean isStuck() {
+  boolean stuck = false;
+  int leftCurrent = getLeftConsumption();
+  int rightCurrent = getRightConsumption();
+  if (leftCurrent > CURRENT_CONSUMPTION_MAX || rightCurrent > CURRENT_CONSUMPTION_MAX) {
+    stuck = true;
+  }
+
+  return stuck;
+}

WheelBot-01/Motors.ino

@@ -51,13 +51,15 @@ void goBackward(unsigned int speed, unsigned int time) {
   }
 }
 void leftBackward(unsigned int speed, unsigned int time) {
+  brakeOff();
   digitalWrite(LEFT_DIRECTION, LOW);
   leftSpeed(speed);
   if (time > 0) {
     delay(time);
   }
 }
 void rightBackward(unsigned int speed, unsigned int time) {
+  brakeOff();
   digitalWrite(RIGHT_DIRECTION, HIGH);
   rightSpeed(speed);
   if (time > 0) {

WheelBot-01/Sonar.ino

@@ -0,0 +1,12 @@
+NewPing sonar(SONAR_PING_PIN, SONAR_PING_PIN, SONAR_MAX_DISTANCE);
+
+void initSonar() {
+  pinMode(SONAR_GROUND_PIN,OUTPUT);
+  pinMode(SONAR_POWER_PIN,OUTPUT);
+  digitalWrite(SONAR_GROUND_PIN, LOW);
+  digitalWrite(SONAR_POWER_PIN, HIGH);
+}
+
+int getDistanceInCm() {
+  return sonar.ping_cm();
+}

WheelBot-01/WheelBot-01.ino

@@ -1,26 +1,37 @@
+#include <NewPing.h>
+
 #define DEBUG_MODE true
-#define DEBUG_RATE 9600
+#define DEBUG_RATE 115200
+
+#define SONAR_PING_PIN 39
+#define SONAR_GROUND_PIN 43
+#define SONAR_POWER_PIN 41
+#define SONAR_MAX_DISTANCE 500
+#define SONAR_MIN_DISTANCE 20
 
-#define LEFT_DECREASE 0
-#define RIGHT_DECREASE 2
+#define LEFT_DECREASE 2
+#define RIGHT_DECREASE 0
 
-#define LEFT_SPEED 3
-#define LEFT_DIRECTION 12
-#define LEFT_BRAKE 9
-#define LEFT_SNS A0
+#define LEFT_SPEED 11
+#define LEFT_DIRECTION 13
+#define LEFT_BRAKE 8
+#define LEFT_SNS A1
 
-#define RIGHT_SPEED 11
-#define RIGHT_DIRECTION 13
-#define RIGHT_BRAKE 8
-#define RIGHT_SNS A1
+#define RIGHT_SPEED 3
+#define RIGHT_DIRECTION 12
+#define RIGHT_BRAKE 9
+#define RIGHT_SNS A0
 
 #define SPEED_MAX 255
 #define SPEED_MEDIUM 150
 #define SPEED_LOW 70
 #define SPEED_ZERO 0
 
+#define CURRENT_CONSUMPTION_MAX 250
+
 void setup() {
   initLogger();
+  initSonar();
 
   pinMode(LEFT_BRAKE, OUTPUT);
   pinMode(LEFT_DIRECTION, OUTPUT);
@@ -30,14 +41,18 @@ void setup() {
 }
 
 void loop() {
-  goForward(SPEED_LOW, 3500);
-  brakeOn(500);
-
-  rotate();
-  brakeOn(1000);
-
-  goForward(SPEED_LOW, 3500);
-  brakeOn(500);
-
-  while(1);
+  int currentDistance = getDistanceInCm();
+
+  if (currentDistance > SONAR_MIN_DISTANCE || currentDistance == 0) {
+    goForward(SPEED_LOW, 150);
+    if (isStuck()) {
+      brakeOn(200);
+      goBackward(SPEED_MEDIUM, 300);
+      lookAroundAndTurn();
+    }
+  } else if (currentDistance > 0) {
+    Serial.println("look around");
+    brakeOn(200);
+    lookAroundAndTurn();
+  }
 }