Transformer's Baby: Arduino Avoiding Obstacle Robot

#define CW  0
#define CCW 1

// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1

// Pin Assignments //
// Don't change these! These pins are statically defined by shield layout
const byte PWMA = 3;  // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13; // Direction control for motor B
int Trig = 6;  //pin 2 Arduino poczony z pinem Trigger czujnika
int Echo = 7;  //pin 3 Arduino poczony z pinem Echo czujnika
int CM;        //odlego w cm
long CZAS;     //dugo powrotnego impulsu w uS
int wPrawo=0;
const int SPEED=200;
const int HALF_SPEED=127;
const int QUARTER_SPEED=80;
long randNumber;
 

void setup()
{
  setupArdumoto(); // Set all pins as outputs
  setupCzujnik();
  randomSeed(analogRead(0));
}

void loop()
{
  distanceMeasurement();              //pomiar odlegoci
  
  if (CM > 40) {
    //both motors forward
    driveArdumoto(MOTOR_A, CW, HALF_SPEED); // Set motor A to CCW at max
    driveArdumoto(MOTOR_B, CCW, HALF_SPEED); // Set motor B to CCW at max
  };
  

  if (CM <= 40 and CM >17) {
   randNumber = random(300);
   if (randNumber>=200) {
    //turn left
    driveArdumoto(MOTOR_A, CW, HALF_SPEED); // Set motor A tQUARTER_SPEEDo CCW at max
    driveArdumoto(MOTOR_B, CW, HALF_SPEED); // Set motor B to CCW at max
    delay(300);
   } else {
    //turn right
    driveArdumoto(MOTOR_A, CCW, HALF_SPEED); // Set motor A to CCW at max
    driveArdumoto(MOTOR_B, CCW, HALF_SPEED); // Set motor B to CCW at max
   }
  };  

  if (CM <= 17) {
    //both motors backwards
    driveArdumoto(MOTOR_A, CCW, HALF_SPEED); // Set motor A to CCW at max
    driveArdumoto(MOTOR_B, CW, HALF_SPEED); // Set motor B to CCW at max
    delay(300);
  };
  
  delay(550);
}

void driveArdumoto(byte motor, byte dir, byte spd)
{
  if (motor == MOTOR_A)
  {
    digitalWrite(DIRA, dir);
    analogWrite(PWMA, spd);
  }
  else if (motor == MOTOR_B)
  {
    digitalWrite(DIRB, dir);
    analogWrite(PWMB, spd);
  }  
}

void stopArdumoto(byte motor)
{
  driveArdumoto(motor, 0, 0);
}


void distanceMeasurement ()
{
  digitalWrite(Trig, HIGH);   
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  CZAS = pulseIn(Echo, HIGH);
  CM = CZAS / 58;             
}

// setupArdumoto initialize all pins
void setupArdumoto()
{
  // All pins should be setup as outputs:
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);
  
  // Initialize all pins as low:
  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(DIRA, LOW);
  digitalWrite(DIRB, LOW);
}

void setupCzujnik()
{
 Serial.begin(9600);                        //inicjalizaja monitora szeregowego
  pinMode(Trig, OUTPUT);                     //ustawienie pinu 2 w Arduino jako wyjcie
  pinMode(Echo, INPUT);                      //ustawienie pinu 3 w Arduino jako wejcie
}

void setupDiody() {
  pinMode(9, OUTPUT); 
}