Semaphores, Mutex and RTOS on Arduino (ChibiOS Help?)

void ScanObstacle(){
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  CheckDistance();
  if (obstacleDistance > 20){ //no obstacle nearby
    Obstacle=0;
    chMtxLock(&serialMutex);
    Serial.print(obstacleDistance);
    Serial.print("cm center over 20");
    Serial.println();
    chMtxUnlock();
  }
  if (obstacleDistance <= 20){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
    chThdSleepMilliseconds(100);
  digitalWrite(Red, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleCenter = obstacleDistance;
  Neck.write(Neckcenter+30); //turn head left
  chThdSleepMilliseconds(200);
  digitalWrite(Green, HIGH);
  CheckDistance();
  obstacleLeft = obstacleDistance;
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-30); //turn head right
  chThdSleepMilliseconds(200);
  digitalWrite(Blue, HIGH);
  CheckDistance();
  obstacleRight = obstacleDistance;
  digitalWrite(Blue, LOW);
  Neck.write(Neckcenter);
  if ((obstacleLeft <= obstacleAhead) && (obstacleRight >= obstacleLeft)){
    Obstacle=1;
    }
    if ((obstacleRight <= obstacleAhead) && (obstacleLeft >= obstacleRight)){
      Obstacle=2;
    }
      if (((obstacleLeft <= obstacleAhead && obstacleRight <= obstacleAhead && obstacleCenter <= obstacleAhead) && (obstacleCenter == obstacleLeft && obstacleCenter == obstacleRight)) || (obstacleLeft <= obstacleWarning && obstacleRight <= obstacleWarning && obstacleCenter <= obstacleWarning)){
      Obstacle=3;
  }
      if ((obstacleLeft <= obstacleAlert) || (obstacleRight <= obstacleAlert) || (obstacleCenter <= obstacleAlert)) {
      Obstacle=4;
  }
  }
  chMtxLock(&serialMutex);
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  Serial.println("Memory use");
  Serial.println("Area,Size,Unused");
  Serial.print("Thread 1,");
  
  // size of stack for thread 1
  Serial.print(sizeof(waThread1) - sizeof(Thread));
  Serial.write(',');
  
  // unused stack for thread 1
  Serial.println(chUnusedStack(waThread1, sizeof(waThread1)));
  
  Serial.print("Thread 2,");
  
  // size of stack for thread 2
  Serial.print(sizeof(waThread2) - sizeof(Thread));
  Serial.write(',');

  // unused stack for thread 2
  Serial.println(chUnusedStack(waThread2, sizeof(waThread2)));
  
    Serial.print("Thread 3,");
  
  // size of stack for thread 3
  Serial.print(sizeof(waThread3) - sizeof(Thread));
  Serial.write(',');
  
  // unused stack for thread 3
  Serial.println(chUnusedStack(waThread3, sizeof(waThread3)));

  // print stats for heap/main thread area
  Serial.print("Heap/Main,");
  Serial.print(chHeapMainSize());
  Serial.print(",");
  Serial.println(chUnusedHeapMain());
  
  // end task
  chMtxUnlock();
}

void CheckDistance(){
    // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
  long duration, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  chThdSleepMilliseconds(2);
  digitalWrite(pingPin, HIGH);
  chThdSleepMilliseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  chThdSleepMilliseconds(10);
  cm = microsecondsToCentimeters(duration);
  obstacleDistance = cm;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

void WalkDirection(){
  noInterrupts();
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  walkToggle = Obstacle;
  interrupts();
  
  switch (walkToggle){
    case 0: //no object
      digitalWrite(Green, HIGH);
      digitalWrite(Red, HIGH);
      Forward(1,30); //one step Forward
      digitalWrite(Green, LOW);
      digitalWrite(Red, LOW);
      break;
    case 1: //object on Left
      digitalWrite(Green, HIGH);
      TurnRight(2,30);
      digitalWrite(Green, LOW);
      break;
    case 2: //object on Right
      digitalWrite(Blue, HIGH);
      TurnLeft(2,30);
      digitalWrite(Blue, LOW);
      break;
    case 3: //obect in Front (both Left and Right detect the object)
      digitalWrite(Red, HIGH);
      TurnLeft(4,30); //turn around
      digitalWrite(Red, LOW);
      break;
    case 4: //obect in Front (both Left and Right detect the object)
      digitalWrite(Red, HIGH);
      Reverse(2,30); //turn around
      digitalWrite(Red, LOW);
      break;
  }
}