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

I am using the wonderful ChibiOS port developed by FatLib16 to attempt to make and arduino project I have multitask.

The original single task code is this:

#include <Servo.h>

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 30; //turn angle
int sAngle = 30; //swing angle
const int pingPin = 12; // define sensor pin

void setup() {
  // initialize serial communication:
  Serial.begin(19200);
  
  Lleg.attach(7);  // attaches the servo on pin x to the servo object 
  Rleg.attach(10);  // attaches the servo on pin x to the servo object 
  Lfoot.attach(8);  // attaches the servo on pin x to the servo object 
  Rfoot.attach(9);  // attaches the servo on pin x to the servo object
  Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  CenterServos(); //center the servos
  delay(500);
  digitalWrite(EnableServo,LOW); //turn power off after centering
  
  pinMode(Red, OUTPUT);
  digitalWrite(Red, LOW);
  pinMode(Blue, OUTPUT);
  digitalWrite(Blue, LOW);
  pinMode(Green, OUTPUT);
  digitalWrite(Green, LOW);
  
    pinMode(BuzzerPin, OUTPUT);
  digitalWrite(BuzzerPin, LOW);
  //Buzzer.PlayMelody();
  
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH); //pull up activated
  
  Serial.print("Ready");

  //while (digitalRead(ButtonPin) != LOW){ 
    //do nothing until the button pressed
  //}
    BuzzerBeep();
  for(int i=0;i<5;++i){
    delay(1000);
  }
    BuzzerBeep();
}

void loop()
{
  //check for obstacles, if none, go Forward, if found, turn the other way
  CheckObstacle();
}

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);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(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
  
  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 CheckObstacle(){
  int Obstacle=0;
  Neck.write(Neckcenter);
  delay(100);
  CheckDistance();
  if (obstacleDistance > 20){ //no obstacle nearby
    Obstacle=0;
    Serial.print(obstacleCenter);
    Serial.print("cm center over 20");
    Serial.println();
  }
  if (obstacleDistance <= 20){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
  digitalWrite(Red, HIGH);
  delay(100);
  CheckDistance();
  obstacleCenter = obstacleDistance;
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Neck.write(Neckcenter+30); //turn head left
  delay(200);
  digitalWrite(Green, HIGH);
  CheckDistance();
  obstacleLeft = obstacleDistance;
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-30); //turn head right
  digitalWrite(Blue, HIGH);
  delay(200); 
  CheckDistance();
  obstacleRight = obstacleDistance;
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  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;
      }
  }
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  switch (Obstacle){
    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;
  }
}

The rest:

void CenterServos() // center the servos on powerup
{ 
    Lleg.write(LLcenter);              // tell servo to go to position in variable 'center' 
    Rleg.write(RLcenter);              // tell servo to go to position in variable 'center' 
    Lfoot.write(LFcenter);              // tell servo to go to position in variable 'center' 
    Rfoot.write(RFcenter);              // tell servo to go to position in variable 'center' 
    Neck.write(Neckcenter);              // tell servo to go to position in variable 'center' 
    delay(100);                     // waits 100ms for the servos to reach the position 
}

void BuzzerBeep(){ //beep the buzzer on event
    digitalWrite(BuzzerPin, HIGH);
    delay(100);
    digitalWrite(BuzzerPin, LOW);
}

//Servo Primatives

void TiltRightUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    delay(sp);
  }
}
void TiltRightDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    delay(sp);
  }
}

void TiltLeftUp(byte ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    delay(sp);
  }
}
void TiltLeftDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    delay(sp);
  }
}

void LeftFootUp(char ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    delay(sp);
  }
}
void LeftFootDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    delay(sp);
  }
}

void RightFootUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Rfoot.write(RFcenter+i);
    delay(sp);
  }
}
void RightFootDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Rfoot.write(RFcenter+i);
    delay(sp);
  }
}

void SwingRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    delay(sp);
  }
}
void SwingRcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    delay(sp);
  }
}

void SwingLeft(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    delay(sp);
  }
}
void SwingLcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    delay(sp);
  }
}

void RightLegIn(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter-i);
    delay(sp);
  }
}
void RightLegIcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter-i);
    delay(sp);
  }
}

void RightLegOut(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter+i);
    delay(sp);
  }
}
void RightLegOcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter+i);
    delay(sp);
  }
}

void LeftLegIn(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    delay(sp);
  }
}
void LeftLegIcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    delay(sp);
  }
}

void LeftLegOut(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter-i);
    delay(sp);
  }
}
void LeftLegOcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter-i);
    delay(sp);
  }
}

void NeckLeft(byte ang, byte sp){
  //swing left
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter+i);
    delay(sp);
  }
}
void NeckRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter-i);
    delay(sp);
  }
}
void NeckIcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter+i);
    delay(sp);
  }
}
void NeckOcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter-i);
    delay(sp);
  }
}

//Walk Functions

void Forward(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltRightDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void Reverse(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltLeftDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnLeft(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(uAngle, Speed);
  delay(20);
  for (byte j=0; j<Steps; ++j){
    LeftLegIn(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    delay(20);
    LeftLegIcenter(sAngle, Speed);
    RightLegOut(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    delay(20);
    RightLegOcenter(sAngle, Speed);
  }
  TiltLeftDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnRight(byte Stps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(uAngle, Speed);
  delay(20);
  for (byte f=0; f<=Stps; ++f){
    RightLegIn(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    delay(20);
    RightLegIcenter(sAngle, Speed);
    LeftLegOut(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    delay(20);
    LeftLegOcenter(sAngle, Speed);
  }
  TiltRightDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

The multithreaded code looks like this:

#include <Servo.h>
#include <ChibiOS_AVR.h>

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
//Setup Variable to Store Switch Value
int Obstacle=0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 30; //turn angle
int sAngle = 30; //swing angle
const int pingPin = 12; // define sensor pin

// remember thread pointers
Thread* tp1;
Thread* tp2;

//------------------------------------------------------------------------------
// thread 1 - high priority for walking motion
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread1, 200);

static msg_t Thread1(void *arg) {
  while (TRUE) {
  WalkDirection();
  }
}

//------------------------------------------------------------------------------
// thread 2 - scan for obstacles as walking
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 200);

static msg_t Thread2(void *arg) {
  while (TRUE) {
    ScanObstacle();
  }
  // end task
}
//------------------------------------------------------------------------------

void setup() {
  // initialize serial communication:
  Serial.begin(19200);
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}
  
  Lleg.attach(7);  // attaches the servo on pin x to the servo object 
  Rleg.attach(10);  // attaches the servo on pin x to the servo object 
  Lfoot.attach(8);  // attaches the servo on pin x to the servo object 
  Rfoot.attach(9);  // attaches the servo on pin x to the servo object
  Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  CenterServos(); //center the servos
  delay(500);
  digitalWrite(EnableServo,LOW); //turn power off after centering
  
  pinMode(Red, OUTPUT);
  digitalWrite(Red, LOW);
  pinMode(Blue, OUTPUT);
  digitalWrite(Blue, LOW);
  pinMode(Green, OUTPUT);
  digitalWrite(Green, LOW);
  
    pinMode(BuzzerPin, OUTPUT);
  digitalWrite(BuzzerPin, LOW);
  //Buzzer.PlayMelody();
  
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH); //pull up activated
  
  Serial.print("Ready... ");

  chBegin(mainThread);
  // chBegin never returns, main thread continues with mainThread()
  // shouldn't return
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {
  
  // start walk thread
  tp1 = chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start object scan thread
  tp2 = chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 2, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}

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);
  delay(2);
  digitalWrite(pingPin, HIGH);
  delay(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
  delay(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 ScanObstacle(){
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  CheckDistance();
  if (obstacleDistance > 20){ //no obstacle nearby
    Obstacle=0;
    Serial.print(obstacleCenter);
    Serial.print("cm center over 20");
    Serial.println();
  }
  if (obstacleDistance <= 20){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
    chThdSleepMilliseconds(100);
  digitalWrite(Red, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleCenter = obstacleDistance;
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Neck.write(Neckcenter+30); //turn head left
  chThdSleepMilliseconds(100);
  digitalWrite(Green, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleLeft = obstacleDistance;
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-30); //turn head right
  chThdSleepMilliseconds(100);
  digitalWrite(Blue, HIGH); 
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleRight = obstacleDistance;
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  digitalWrite(Blue, LOW);
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  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;
  }
    // print memory use
  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)));

  // print stats for heap/main thread area
  Serial.print("Heap/Main,");
  Serial.print(chHeapMainSize());
  Serial.print(",");
  Serial.println(chUnusedHeapMain());
  
  // end task
  }
}
void WalkDirection(){
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  switch (Obstacle){
    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;
  }
}

The rest:

void CenterServos() // center the servos on powerup
{ 
    Lleg.write(LLcenter);              // tell servo to go to position in variable 'center' 
    Rleg.write(RLcenter);              // tell servo to go to position in variable 'center' 
    Lfoot.write(LFcenter);              // tell servo to go to position in variable 'center' 
    Rfoot.write(RFcenter);              // tell servo to go to position in variable 'center' 
    Neck.write(Neckcenter);              // tell servo to go to position in variable 'center' 
    delay(100);                     // waits 100ms for the servos to reach the position 
}

void BuzzerBeep(){ //beep the buzzer on event
    digitalWrite(BuzzerPin, HIGH);
    chThdSleepMilliseconds(100);
    digitalWrite(BuzzerPin, LOW);
}

//Servo Primatives

void TiltRightUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void TiltRightDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void TiltLeftUp(byte ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void TiltLeftDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftFootUp(char ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftFootDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void RightFootUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void RightFootDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void SwingRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void SwingRcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void SwingLeft(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void SwingLcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void RightLegIn(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void RightLegIcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void RightLegOut(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void RightLegOcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftLegIn(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftLegIcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftLegOut(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftLegOcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void NeckLeft(byte ang, byte sp){
  //swing left
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckIcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckOcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

//Walk Functions

void Forward(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltRightDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void Reverse(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltLeftDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnLeft(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(uAngle, Speed);
  chThdSleepMilliseconds(20);
  for (byte j=0; j<Steps; ++j){
    LeftLegIn(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    LeftLegIcenter(sAngle, Speed);
    RightLegOut(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    RightLegOcenter(sAngle, Speed);
  }
  TiltLeftDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnRight(byte Stps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(uAngle, Speed);
  chThdSleepMilliseconds(20);
  for (byte f=0; f<=Stps; ++f){
    RightLegIn(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    RightLegIcenter(sAngle, Speed);
    LeftLegOut(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    LeftLegOcenter(sAngle, Speed);
  }
  TiltRightDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

So the single threaded original code works as expected, the bot walks until it sees an obstacle, stops turns it head and checks how to go around the obstacle or if it's too close to back up.

The idea of the mutithread is that it could check for obstacle while walking and adjust without stopping. The problem being that my sensor values are returning as 0 iun the multi thread version. I've been at this for some hours now, and any input on how to fix it would be MUCH appreciated.

Variables that are shared between threads should be typed volatile.

Also you need to insure access to variables is atomic. A context switch can happen during a multi-byte fetch or store.

See count in the the chBlinkPrint example.

okay I have updated the code to reflect this:

#include <Servo.h>
#include <ChibiOS_AVR.h>

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
//Setup Variable to Store Switch Value
volatile uint32_t Obstacle=0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 30; //turn angle
int sAngle = 30; //swing angle
const int pingPin = 12; // define sensor pin
// pin to trigger interrupt

// remember thread pointers
Thread* tp1;
Thread* tp2;

//------------------------------------------------------------------------------
// thread 1 - high priority for walking motion
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread1, 200);

static msg_t Thread1(void *arg) {
  while (TRUE) {
  WalkDirection();
  }
}

//------------------------------------------------------------------------------
// thread 2 - scan for obstacles as walking
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 200);

static msg_t Thread2(void *arg) {
  while (TRUE) {
    ScanObstacle();
  }
  // end task
}
//------------------------------------------------------------------------------

void setup() {
  // initialize serial communication:
  Serial.begin(19200);
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}
  
  Lleg.attach(7);  // attaches the servo on pin x to the servo object 
  Rleg.attach(10);  // attaches the servo on pin x to the servo object 
  Lfoot.attach(8);  // attaches the servo on pin x to the servo object 
  Rfoot.attach(9);  // attaches the servo on pin x to the servo object
  Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  CenterServos(); //center the servos
  delay(500);
  digitalWrite(EnableServo,LOW); //turn power off after centering
  
  pinMode(Red, OUTPUT);
  digitalWrite(Red, LOW);
  pinMode(Blue, OUTPUT);
  digitalWrite(Blue, LOW);
  pinMode(Green, OUTPUT);
  digitalWrite(Green, LOW);
  
    pinMode(BuzzerPin, OUTPUT);
  digitalWrite(BuzzerPin, LOW);
  //Buzzer.PlayMelody();
  
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH); //pull up activated
  
  Serial.print("Ready... ");

  chBegin(mainThread);
  // chBegin never returns, main thread continues with mainThread()
  // shouldn't return
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {
  
  // start walk thread
  tp1 = chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start object scan thread
  tp2 = chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 2, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}

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);
  delay(2);
  digitalWrite(pingPin, HIGH);
  delay(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
  delay(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 ScanObstacle(){
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  CheckDistance();
  if (obstacleDistance > 20){ //no obstacle nearby
    Obstacle=0;
    Serial.print(obstacleCenter);
    Serial.print("cm center over 20");
    Serial.println();
  }
  if (obstacleDistance <= 20){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
    chThdSleepMilliseconds(100);
  digitalWrite(Red, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleCenter = obstacleDistance;
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Neck.write(Neckcenter+30); //turn head left
  chThdSleepMilliseconds(100);
  digitalWrite(Green, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleLeft = obstacleDistance;
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-30); //turn head right
  chThdSleepMilliseconds(100);
  digitalWrite(Blue, HIGH); 
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleRight = obstacleDistance;
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  digitalWrite(Blue, LOW);
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  noInterrupts();
  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;
  }
  interrupts();
  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)));

  // print stats for heap/main thread area
  Serial.print("Heap/Main,");
  Serial.print(chHeapMainSize());
  Serial.print(",");
  Serial.println(chUnusedHeapMain());
  
  // end task
  }
}
void WalkDirection(){
  noInterrupts();
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  switch (Obstacle){
    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;
      interrupts();
  }
}

void CenterServos() // center the servos on powerup
{ 
    Lleg.write(LLcenter);              // tell servo to go to position in variable 'center' 
    Rleg.write(RLcenter);              // tell servo to go to position in variable 'center' 
    Lfoot.write(LFcenter);              // tell servo to go to position in variable 'center' 
    Rfoot.write(RFcenter);              // tell servo to go to position in variable 'center' 
    Neck.write(Neckcenter);              // tell servo to go to position in variable 'center' 
    delay(100);                     // waits 100ms for the servos to reach the position 
}

void BuzzerBeep(){ //beep the buzzer on event
    digitalWrite(BuzzerPin, HIGH);
    chThdSleepMilliseconds(100);
    digitalWrite(BuzzerPin, LOW);
}

The rest:

//Servo Primatives

void TiltRightUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void TiltRightDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter+i);
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void TiltLeftUp(byte ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void TiltLeftDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    Rfoot.write(RFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftFootUp(char ang, byte sp){
  //tilt left up
  for (int i=0; i<=ang; i+=5){
    Lfoot.write(LFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftFootDown(byte ang, byte sp){
  //tilt left down
  for (int i=ang; i>0; i-=5){
    Lfoot.write(LFcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void RightFootUp(byte ang, byte sp){  
  //tilt right up
  for (int i=0; i<=ang; i+=5){
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void RightFootDown(byte ang, byte sp){
  //tilt right down
  for (int i=ang; i>0; i-=5){
    Rfoot.write(RFcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void SwingRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void SwingRcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Lleg.write(LLcenter-i);
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void SwingLeft(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void SwingLcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void RightLegIn(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void RightLegIcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void RightLegOut(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void RightLegOcenter(byte ang, byte sp){
  //swing r->center
  for (int i=ang; i>0; i-=5){
    Rleg.write(RLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftLegIn(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftLegIcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter+i);
    chThdSleepMilliseconds(sp);
  }
}

void LeftLegOut(byte ang, byte sp){
  //swing left
  for (byte i=0; i<=ang; i=i+5){
    Lleg.write(LLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void LeftLegOcenter(byte ang, byte sp){
  //swing l->center
  for (byte i=ang; i>0; i=i-5){
    Lleg.write(LLcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

void NeckLeft(byte ang, byte sp){
  //swing left
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckRight(byte ang, byte sp){
  //swing right
  for (int i=0; i<=ang; i+=5){
    Neck.write(Neckcenter-i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckIcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter+i);
    chThdSleepMilliseconds(sp);
  }
}
void NeckOcenter(byte ang, byte sp){
  //swing neck->center
  for (byte i=ang; i>0; i=i-5){
    Neck.write(Neckcenter-i);
    chThdSleepMilliseconds(sp);
  }
}

//Walk Functions

void Forward(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltRightDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void Reverse(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(tAngle, Speed);
  for (byte j=0; j<Steps; ++j){
    SwingRight(sAngle, Speed);
    TiltLeftDown(tAngle, Speed);
    TiltRightUp(tAngle, Speed);
    SwingRcenter(sAngle, Speed);
    SwingLeft(sAngle, Speed);
    TiltRightDown(tAngle, Speed);
    TiltLeftUp(tAngle, Speed);
    SwingLcenter(sAngle, Speed);
  }
  TiltLeftDown(tAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnLeft(byte Steps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltLeftUp(uAngle, Speed);
  chThdSleepMilliseconds(20);
  for (byte j=0; j<Steps; ++j){
    LeftLegIn(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    LeftLegIcenter(sAngle, Speed);
    RightLegOut(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    RightLegOcenter(sAngle, Speed);
  }
  TiltLeftDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

void TurnRight(byte Stps, byte Speed){
  digitalWrite(EnableServo,HIGH);
  TiltRightUp(uAngle, Speed);
  chThdSleepMilliseconds(20);
  for (byte f=0; f<=Stps; ++f){
    RightLegIn(sAngle, Speed);
    TiltRightDown(uAngle, Speed);
    TiltLeftUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    RightLegIcenter(sAngle, Speed);
    LeftLegOut(sAngle, Speed);
    TiltLeftDown(uAngle, Speed);
    TiltRightUp(uAngle, Speed);
    chThdSleepMilliseconds(20);
    LeftLegOcenter(sAngle, Speed);
  }
  TiltRightDown(uAngle, Speed);
  digitalWrite(EnableServo,LOW);
}

However the sensor reading are still volatile, thanks for the help and forgive my newb-ish-ness

The latest version not including servo primatives (which remain unchanged):

#include <Servo.h>
#include <ChibiOS_AVR.h>

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
//Setup Variable to Store Switch Value
volatile uint32_t Obstacle=0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 30; //turn angle
int sAngle = 30; //swing angle
const int pingPin = 12; // define sensor pin
// pin to trigger interrupt

// remember thread pointers
Thread* tp1;
Thread* tp2;

//------------------------------------------------------------------------------
// thread 1 - high priority for walking motion
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread1, 200);

static msg_t Thread1(void *arg) {
  while (TRUE) {
  WalkDirection();
  }
}

//------------------------------------------------------------------------------
// thread 2 - scan for obstacles as walking
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 200);

static msg_t Thread2(void *arg) {
  while (TRUE) {
    ScanObstacle();
  }
  // end task
}
//------------------------------------------------------------------------------

void setup() {
  // initialize serial communication:
  Serial.begin(19200);
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}
  
  Lleg.attach(7);  // attaches the servo on pin x to the servo object 
  Rleg.attach(10);  // attaches the servo on pin x to the servo object 
  Lfoot.attach(8);  // attaches the servo on pin x to the servo object 
  Rfoot.attach(9);  // attaches the servo on pin x to the servo object
  Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  CenterServos(); //center the servos
  delay(500);
  digitalWrite(EnableServo,LOW); //turn power off after centering
  
  pinMode(Red, OUTPUT);
  digitalWrite(Red, LOW);
  pinMode(Blue, OUTPUT);
  digitalWrite(Blue, LOW);
  pinMode(Green, OUTPUT);
  digitalWrite(Green, LOW);
  
    pinMode(BuzzerPin, OUTPUT);
  digitalWrite(BuzzerPin, LOW);
  //Buzzer.PlayMelody();
  
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH); //pull up activated
  
  Serial.print("Ready... ");

  chBegin(mainThread);
  // chBegin never returns, main thread continues with mainThread()
  // shouldn't return
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {
  
  // start walk thread
  tp1 = chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start object scan thread
  tp2 = chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 2, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}

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);
  delay(2);
  digitalWrite(pingPin, HIGH);
  delay(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
  delay(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 ScanObstacle(){
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  CheckDistance();
  if (obstacleDistance > 20){ //no obstacle nearby
    Obstacle=0;
    Serial.print(obstacleCenter);
    Serial.print("cm center over 20");
    Serial.println();
  }
  if (obstacleDistance <= 20){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
    chThdSleepMilliseconds(100);
  digitalWrite(Red, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleCenter = obstacleDistance;
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Neck.write(Neckcenter+30); //turn head left
  chThdSleepMilliseconds(100);
  digitalWrite(Green, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleLeft = obstacleDistance;
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-30); //turn head right
  chThdSleepMilliseconds(200);
  digitalWrite(Blue, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleRight = obstacleDistance;
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  digitalWrite(Blue, LOW);
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  noInterrupts();
  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;
  }
  interrupts();
  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)));

  // print stats for heap/main thread area
  Serial.print("Heap/Main,");
  Serial.print(chHeapMainSize());
  Serial.print(",");
  Serial.println(chUnusedHeapMain());
  
  // end task
  }
}
void WalkDirection(){
  Serial.print(Obstacle);
  Serial.print(" Case");
  Serial.println();
  noInterrupts();
  switch (Obstacle){
    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;
      interrupts();
  }
}

Now only the "left" reading isn't working, it always returns a 0 value.

Are all the shared variables volatile like this?

volatile int sharedVar;

Otherwise the compiler sometimes fails to store or fetch values correctly.

I believe the only shared variable is "Obstacle" should I make them all volatile for good measure?

The serial outputs I added for diagnostics give me this:

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,161
Heap/Main,1393,1338
0cm center over 20
0cm center over 20
0cm center over 20
0cm center over 20
0cm center over 20
0cm center over 20
0cm center over 20
0 Case
177cm center
70cm left
0 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,161
Heap/Main,1393,1338
177cm center over 20
177cm center over 20
0 Case
0 Case
0cm center
0 Case
0cm left
0 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,145
Heap/Main,1393,1338
20cm center
4 Case
0cm left
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,145
Heap/Main,1393,1338
4 Case
21cm center
0cm left
0cm right
4 Case

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,145
Heap/Main,1393,1338
21cm center over 20
25cm center
0cm left
0 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,145
Heap/Main,1393,1338
25cm center over 20
0 Case
22cm center
0 Case
0cm left
0 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,145
Heap/Main,1393,1338
4 Case
0cm center
0cm left
4 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,144
Heap/Main,1393,1338
21cm center
4 Case
0cm left
18cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,144
Heap/Main,1393,1338
14cm center
0cm left
4 Case
0cm right

Memory use
Area,Size,Unused
Thread 1,269,161
Thread 2,269,144
Heap/Main,1393,1338

It looks like its missing the sensor reading sometime. I could add a 5th case where if any of the readings is 0 it continues forward, but it would not be very accurate for obstacle avoidance. There has to be something I'm missing here.

Thanks for all of your help.

Is it possible that the arduino mini is just not powerful enough to do this all in real time?

Or it could also be that while my 9v 170mAh rechargeable is providing enough current to drive 1 servo and the ping, 5 servos or 6 servos (and 2 RGB LEDs... It is not providing enough current to run 6 servos, the ping and 2RGB LEDs concurrently

After setting up a test task with 4 threads running concurrently I can say without a doubt that the power supply is the issue. The ping does not have sufficient current to send out the ultrasonic blast.

You need to check that the sensor data is being updated properly. Adding a time of reading would be helpful.

Here is a demo sketch that shows data sharing with an accelerometer. The shared variable dataT is the time in milis() when the data was acquired.

Note the mutex, volatile shared vatiables, and the use of temp variables to limit the time shared data is locked.

// Simple demo of daata sharing.
// Adafruit ADXL335 - 5V ready triple-axis accelerometer.
#include <ChibiOS_AVR.h>

const uint8_t X_PIN = 0;
const uint8_t Y_PIN = 1;
const uint8_t Z_PIN = 2;

// approximate zero g value.
const int dataOffset = 338;
//------------------------------------------------------------------------------
// Shared data, use volatile to insure correct access.

// Mutex for atomic access to data.
MUTEX_DECL(dataMutex);

// Time data was read.
volatile uint32_t dataT;

// Data X value.
volatile int dataX;

// Data Y value.
volatile int dataY;

// Data Z value.
volatile int dataZ;
//------------------------------------------------------------------------------
// Thread 1, high priority to read accelerometer.
// 64 byte stack beyond task switch and interrupt needs.
static WORKING_AREA(waThread1, 64);

static msg_t Thread1(void *arg) {

  // Read data every 10 ms.
  systime_t wakeTime = chTimeNow();
  
  while (1) {
    // Add ticks for 10 ms.
    wakeTime += MS2ST(10);
    chThdSleepUntil(wakeTime);
    
    // Use temp variables to acquire data.
    uint32_t tmpT = millis();
    int tmpX = analogRead(X_PIN);
    int tmpY = analogRead(Y_PIN);
    int tmpZ = analogRead(Z_PIN);

    // Lock access to data.
    chMtxLock(&dataMutex);

    // Copy tmp variables to shared variables.
    dataT = tmpT;
    dataX = tmpX;
    dataY = tmpY;
    dataZ = tmpX;
    
    // Unlock data access.
    chMtxUnlock();
  }
  return 0;
}
//------------------------------------------------------------------------------
// thread 2 - print data every second.
// 100 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 100);

static msg_t Thread2(void *arg) {

  // print count every second
  systime_t wakeTime = chTimeNow();
  while (1) {
    // Sleep for one second.
    wakeTime += MS2ST(1000);
    chThdSleepUntil(wakeTime);
    
    // Lock access to data.
    chMtxLock(&dataMutex);

    // Copy shared data to tmp variables.
    uint32_t tmpT = dataT;
    int tmpX = dataX;
    int tmpY = dataY;
    int tmpZ = dataZ;

    // Unlock data access.
    chMtxUnlock();
    
    Serial.print(F("dataAge: "));
    Serial.print(millis() - tmpT);
    Serial.print(F(" ms, dataX: "));
    Serial.print(tmpX -dataOffset);
    Serial.print(F(", dataY: "));
    Serial.print(tmpY - dataOffset);
    Serial.print(F(", dataZ: "));
    Serial.println(tmpZ - dataOffset);
  }
}
//------------------------------------------------------------------------------
void setup() {
  Serial.begin(9600);
  // wait for USB Serial
  while (!Serial) {}
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}

  chBegin(mainThread);
  // chBegin never returns, main thread continues with mainThread()
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {

  // start blink thread
  chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start print thread
  chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 1, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}

Here is output from the sketch. Note the data is never older than 10 milliseconds.

dataAge: 4 ms, dataX: 0, dataY: 1, dataZ: 100
dataAge: 8 ms, dataX: -1, dataY: 1, dataZ: 99
dataAge: 3 ms, dataX: 4, dataY: -13, dataZ: 104
dataAge: 7 ms, dataX: 83, dataY: -5, dataZ: 183
dataAge: 2 ms, dataX: 90, dataY: -54, dataZ: 190
dataAge: 6 ms, dataX: 19, dataY: 65, dataZ: 119
dataAge: 1 ms, dataX: 112, dataY: 93, dataZ: 212

I did make that shared variable volatile and during use by a thread. I as able to execute the script correctly buy running it on an arduino with no servos attached, unfortunately I don't think it was a code issue but a power supply issue as the "ping" sensor itself was not lighting fully with the servos on but with them disabled I had no problems.

This was my final code:

#include <Servo.h>
#include <ChibiOS_AVR.h>

MUTEX_DECL(lockMutex);

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6


Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 35; //turn angle
int sAngle = 30; //swing angle
int neckAngle = 30; //angle for meck turn
const int pingPin = 12; // define sensor pin

// remember thread pointers
Thread* tp1;
Thread* tp2;

//------------------------------------------------------------------------------
// thread 1 - high priority for walking motion
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread1, 200);

static msg_t Thread1(void *arg) {
  while (TRUE) {
  WalkDirection();
  }
}

//------------------------------------------------------------------------------
// thread 2 - scan for obstacles as walking
// 200 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 200);

static msg_t Thread2(void *arg) {
  while (TRUE) {
    ScanObstacle();
  }
  // end task
}
//------------------------------------------------------------------------------

void setup() {
  // initialize serial communication:
  Serial.begin(19200);
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}
  
  Lleg.attach(7);  // attaches the servo on pin x to the servo object 
  Rleg.attach(10);  // attaches the servo on pin x to the servo object 
  Lfoot.attach(8);  // attaches the servo on pin x to the servo object 
  Rfoot.attach(9);  // attaches the servo on pin x to the servo object
  Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  CenterServos(); //center the servos
  delay(500);
  digitalWrite(EnableServo,LOW); //turn power off after centering
  
  pinMode(Red, OUTPUT);
  digitalWrite(Red, LOW);
  pinMode(Blue, OUTPUT);
  digitalWrite(Blue, LOW);
  pinMode(Green, OUTPUT);
  digitalWrite(Green, LOW);
  
    pinMode(BuzzerPin, OUTPUT);
  digitalWrite(BuzzerPin, LOW);
  //Buzzer.PlayMelody();
  
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH); //pull up activated
  
  Serial.print("Ready... ");

  chBegin(mainThread);
  // chBegin never returns, main thread continues with mainThread()
  // shouldn't return
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {
  
  // start walk thread
  tp1 = chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start object scan thread
  tp2 = chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 2, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}

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 ScanObstacle(){
  Neck.write(Neckcenter);
  chThdSleepMilliseconds(100);
  CheckDistance();
  if (obstacleDistance > obstacleAhead){ //no obstacle nearby
    chMtxLock(&lockMutex);
    Obstacle=0;
    chMtxUnlock();
    Serial.print(obstacleDistance);
    Serial.print("cm center over 20");
    Serial.println();
  }
  if (obstacleDistance <= obstacleAhead){ //check sensor
    BuzzerBeep();
    Neck.write(Neckcenter);
    chThdSleepMilliseconds(100);
  digitalWrite(Red, HIGH);
  CheckDistance();
  chThdSleepMilliseconds(10);
  obstacleCenter = obstacleDistance;
  Serial.print(obstacleCenter);
  Serial.print("cm center");
  Serial.println();
  Neck.write(Neckcenter+neckAngle); //turn head left
  chThdSleepMilliseconds(200);
  digitalWrite(Green, HIGH);
  CheckDistance();
  obstacleLeft = obstacleDistance;
  Serial.print(obstacleLeft);
  Serial.print("cm left");
  Serial.println();
  digitalWrite(Green, LOW);
  Neck.write(Neckcenter-neckAngle); //turn head right
  chThdSleepMilliseconds(200);
  digitalWrite(Blue, HIGH);
  CheckDistance();
  obstacleRight = obstacleDistance;
  Serial.print(obstacleRight);
  Serial.print("cm right");
  Serial.println();
  digitalWrite(Blue, LOW);
  Neck.write(Neckcenter);
  chMtxLock(&lockMutex);
  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;
  }
  chMtxUnlock();
  }
}
void WalkDirection(){
  chMtxLock(&lockMutex);
  int walkToggle = Obstacle;
  chMtxUnlock();
  Serial.print(walkToggle);
  Serial.print(" Case");
  Serial.println();
  
  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;
  }
}

However the serial output and actions of the robot always reflect case 4 (0cm, 0cm, 0cm)

If I power it with 9V and USB some of those readings come back with a value.

Something like this running three threads including the scan thread works fine, plus I see that the ping is powered.

#include <Servo.h>
#include <ChibiOS_AVR.h>

MUTEX_DECL(serialMutex);

#define EnableServo 13
#define BuzzerPin 4
#define ButtonPin 2
#define Red 3
#define Green 5
#define Blue 6

Servo Lleg;  // create servo object to control a servo 
Servo Rleg;
Servo Lfoot;
Servo Rfoot;
Servo Neck;

int RFcenter = 80;    // variables to store the center servo positions
int LLcenter = 80;
int RLcenter = 80;
int LFcenter = 80;
int Neckcenter = 90;
// Setup variables to store sensor readings
int obstacleDistance = 0;
int obstacleLeft = 0;
int obstacleCenter = 0;
int obstacleRight = 0;
int presentDistance = 0;
//Setup Variable to Store Switch Value
volatile int Obstacle=0;
//Non volatile int to limit time with no interrupt
int walkToggle = 0;
// declare reaction distances on object preception
int obstacleAhead = 20;
int obstacleWarning = 10;
int obstacleAlert = 8;
// declare angle values for walking
int tAngle = 25; //tilt angle
int uAngle = 30; //turn angle
int sAngle = 30; //swing angle
const int pingPin = 12; // define sensor pin
// pin to trigger interrupt

const uint8_t LED_PIN = 6;


volatile uint32_t count = 0;

// remember thread pointers
Thread* tp1;
Thread* tp2;
Thread* tp3;
//------------------------------------------------------------------------------
// thread 1 - high priority for blinking LED
// 64 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread1, 64);

static msg_t Thread1(void *arg) {
  pinMode(LED_PIN, OUTPUT);
  while (!chThdShouldTerminate()) {
    digitalWrite(LED_PIN, HIGH);
    chThdSleepMilliseconds(50);
    digitalWrite(LED_PIN, LOW);
    chThdSleepMilliseconds(150);
  }
  return 0;
}
//------------------------------------------------------------------------------
static WORKING_AREA(waThread2, 200);

static msg_t Thread2(void *arg) {

  Serial.println("Type any character for stack use");

  // print count every second
  while (!Serial.available()) {
    Serial.println(count);
    count = 0;
    chThdSleepMilliseconds(1000);
  }
  // Terminate the LED thread
  chThdTerminate(tp1);

  // print memory use
  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)));

  // print stats for heap/main thread area
  Serial.print("Heap/Main,");
  Serial.print(chHeapMainSize());
  Serial.print(",");
  Serial.println(chUnusedHeapMain());
  
  // end task
  return 0;
}
//------------------------------------------------------------------------------
// thread 3 - high priority for blinking LED
// 64 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread3, 200);

static msg_t Thread3(void *arg) {

while (TRUE){
  ScanObstacle();
}  
}
//------------------------------------------------------------------------------
void setup() {
  Serial.begin(9600);
  // wait for USB Serial
  while (!Serial) {}
  
  // read any input
  delay(200);
  while (Serial.read() >= 0) {}
  
 Neck.attach(11);  // attaches the servo on pin x to the servo object

  pinMode(EnableServo,OUTPUT); 
  digitalWrite(EnableServo,HIGH); //this turns on the power to the servos
  delay(100);
  digitalWrite(EnableServo,LOW); //turn power off after centering

  chBegin(mainThread);
  while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {

  // start blink thread
  tp1 = chThdCreateStatic(waThread1, sizeof(waThread1),
                          NORMALPRIO + 2, Thread1, NULL);

  // start print thread
  tp2 = chThdCreateStatic(waThread2, sizeof(waThread2),
                          NORMALPRIO + 1, Thread2, NULL);
    // start print thread
  tp3 = chThdCreateStatic(waThread3, sizeof(waThread3),
                          NORMALPRIO + 1, Thread3, NULL);

  // increment counter
  while (1) {
    noInterrupts();
    count++;
    interrupts();
  }
}
//------------------------------------------------------------------------------
void loop() {
 // not used
}