 
 void setup()
 {
+    Serial.begin(9600);
+
     motor_init(&motor_conveyor, 0.36, MOTOR_CONVEYOR_PIN, 
         MOTOR_CONVEYOR_INT_PIN, motor_conveyor_interrupt);
     motor_init(&motor_feeder, 1.0, MOTOR_FEEDER_PIN, MOTOR_FEEDER_INT_PIN, 
         motor_feeder_interrupt);
     distance_sensor_init(&distance_sensor, RANGE_TRIG, RANGE_ECHO);
     
+    distance_to_wall = calibrate_ultra_sound_sensor();
+    delay(1000);
+    Serial.print("Distance to wall: ");
+    Serial.println(distance_to_wall);
+    delay(2500);
     motor_turn(&motor_conveyor);
-    distance_to_wall = calibrate_ultra_sound_sensor();
+    feed_ball();
 }
 
 void loop()
 {
-    int32_t test_dist = distance_sensor_measure_distance(&distance_sensor);
-    if (test_dist >= distance_to_wall)
+    int32_t test_dist = distance_sensor_double_take(); //distance_sensor_measure_distance(&distance_sensor);
+    //Serial.println(test_dist);
+
+    if(test_dist + 10 < distance_to_wall)
     {
-        delay(100);
-        return;
+        Serial.print("Log: ");
+        Serial.println(millis());
     }
+
+    static uint32_t time = millis();
+    if (time + 5000 < millis())
+    {
+        feed_ball();
+        time = millis();
+    }
+
+    return;
     uint32_t pos = motor_get_degrees(&motor_conveyor);
     // Change TARGET1 for delay before requesting image
-    while(pos + TARGET1 > motor_get_degrees(&motor_conveyor)) {}
-    Serial.print("b");
+    while(pos + 225 > motor_get_degrees(&motor_conveyor)) {}
+    Serial.print("Command:");
+    Serial.print(2);
+    Serial.println("end");
     // Change TARGET2 for delay after requesting image
-    while(pos + TARGET2 > motor_get_degrees(&motor_conveyor)) {}
+    while(pos + 720 > motor_get_degrees(&motor_conveyor)) {}
     feed_ball();
 }
 

  }

  return min;
}

void feed_ball() 
{
  static int16_t deg = 90;
  
    motor_turn_to_degree(&motor_feeder, deg);

    deg += 90;
    if (deg == 360) 
        deg = 0;
}

int32_t calibrate_ultra_sound_sensor()
