Code with new ping and schematics updated

bugrobot/bugrobot.ino

@@ -5,16 +5,20 @@
 VarSpeedServo MF;
 VarSpeedServo MB;
 
-const int ultrasonPin = 6;
+#define ULTRA_TRIG_PIN 8 
+#define ULTRA_ECHO_PIN 7
 
-NewPing sonar(ultrasonPin, ultrasonPin );
+#define SERVO_AVANT_CTRL_PIN 5 //Blue, next to the arduino board
+#define SERVO_ARRIERE_CTRL_PIN 4 //Green, outsite the arduino board
 
-const int hm[2] = {81, 95}; // Home position, you need to set them
+NewPing sonar(ULTRA_TRIG_PIN, ULTRA_ECHO_PIN);
+
+const int hm[2] = {35, 40}; // Home position, you need to set them
 const int speed_ = 30; // Slow speed
 
 const int nbrPos = 2;
 const int amplitudeBackDeg = 25;
-const int amplitudeFrontDeg = 8;
+const int amplitudeFrontDeg = 30;
 const int stoper [2] = {0,0};
 const int avancer [nbrPos][2] = {
     {-amplitudeFrontDeg, amplitudeBackDeg},
@@ -36,12 +40,13 @@ const int gauche [3][2] = {
 
 void setup() {
     // put your setup code here, to run once:
-    MF.attach(2);  //Front motor
-    MB.attach(4);  //Back motor
+    MF.attach(SERVO_AVANT_CTRL_PIN);  //Front motor
+    MB.attach(SERVO_ARRIERE_CTRL_PIN);  //Back motor
 
     MF.write(hm[0], speed_, true);
     MB.write(hm[1], speed_, true);
     Serial.begin(115200);
+    delay(3000);
 }
 
 void loop() {
@@ -71,7 +76,7 @@ unsigned int distance()
 
     // Distance proportionnelle à la durée de sortie
     distance = duration/US_ROUNDTRIP_CM;
-    Serial.print(duration);
+    //Serial.print(duration);
     Serial.print(" ");
     Serial.println(distance);
     if(distance > 40 || distance < 3)