 void loop() {
   // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
   // delay(50); 
-  cm = sonar.ping_cm();
-  doStaff(); // All logic controller is implemented here
+  int cm = sonar.ping_cm();
+  if (cm <= 15) {
+    stopMotor();
+  }
+  doStaff(cm); // All logic controller is implemented here
 }
 
-void doStaff() {
+void doStaff(int cm) {
   uint8_t serIn = 0;
   if (Serial.available()) {
     serIn = Serial.read();

  
  switch (serIn) {
    case 1:
      // move robot forward
      goForward();
      break;
    case 2:
      // move robot backward

#define TRIGGER_PIN  2  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.

// Declare L298N Dual H-Bridge Motor Controller directly since there is not a library to load.
const int speed = 200;
