Ultra-Sonic robot

Hi, everyone,

i am pretty fresh with programming and Arduino itself, so any positive critic is more than welcome. I am trying to assemble my first robot, which is using Pololu Zumo platform. I have two DC motors connected via DRV8833 . Also, i have an US-100 attached to a servo which is continuously scanning in front for obstacles. The problem is that there is some delay changing the path of the robot. Currently i have set 10cm limit distance, but there like 1-2 seconds before actually turning around after us-100 detects something.
Another issue that i have is that one of the motors is running at slower speed, currently i solved this one with another variable setting the speed for the 2nd motor a little bit lower, but i doubt it this is the right solution.

Again, this is my first "next-level" code, still trying to learn the basics. Any feedback will be appreciated.

#include <SoftwareSerial.h>;
#include <Servo.h>.
#include <DRV8833.h>

DRV8833 driver = DRV8833();
Servo myServo;


const int US100_TX = 2;  //TX INPUT for Ultra-Sonic Sensor
const int US100_RX = 3;  //RX INPUT for Ultra-Sonic Sensor
const int inputA1 = 6, inputA2 = 9, inputB1 = 11, inputB2 = 10; // Define DC motors OUTPUTS; PWN INPUTS
const int motorSpeedA = 130; //Define MotorA speed;
const int motorSpeedB = 100; //Define MotorB speed;



SoftwareSerial puertoUS100(US100_RX, US100_TX);

unsigned int MSByteDist = 0;
unsigned int LSByteDist = 0;
unsigned int mmDist = 0;
int temp = 0;

void setup() {

  Serial.begin(9600);
  puertoUS100.begin(9600); //Define Ultra-Sonic Sensor
  driver.attachMotorA(inputA1, inputA2); //Define MotorA Right
  driver.attachMotorB(inputB1, inputB2); //Define MotorB Left
  myServo.attach(4); //Define Servo motor
}

void loop() {
  puertoUS100.flush();
  puertoUS100.write(0x55);
  if (puertoUS100.available() >= 2)
  {
    MSByteDist = puertoUS100.read();
    LSByteDist  = puertoUS100.read();
    mmDist  = MSByteDist * 256 + LSByteDist; //Distance calculation
    if ((mmDist > 1) && (mmDist < 10000))
    {
      Serial.print("Distance: ");
      Serial.print(mmDist, DEC);
      Serial.println(" mm");
    }
  }
  for (int i = -5; i > 120; i++) {
    myServo.write(i);
    delay(4);    
  }
  for (int i = 120; i > -5; i--) {
    myServo.write(i);
    delay(4);
  }
  if (mmDist > 100){
  driver.motorAForward(motorSpeedA);
  driver.motorBForward(motorSpeedB);
  }
  if (mmDist < 100) {
  driver.motorAForward(motorSpeedA);
  driver.motorBStop();
  }
}

What does the Servo library do with negative angles?

Why does the servo sweep?

125 * 4 * 2 = 1000ms

That loop is going to stop before it starts because -5 is not >120. I suspect you wanted:

  for (int i = -5; i < 120; i++) {
    myServo.write(i);
    delay(4);    
  }

Yes, that's right, that actually fixed the movement of the servo a little bit. Thank you, John!

Anyway, i've tried everything that came to my mind and couldn't fix the issue with the sensor. I even tried few sketches i have found online, but still the result same the same. There is a still some delay before the DC motors take action.

So, i went back to my initial design. I added three IR sensors in front of chassis, for every direction. Right now, with very basic code is working properly.

Anyway, thanks for the comments guys. I will try to upload the project once i finish it.

Again - why does the servo sweep?