Thanks for all of your amazing work porting these RTOSs to arduino.

I am using 9 gram HTX900 servos which at peak have 1.6kg of torque but at no time are they lifting more then 350 grams so I doubt they are using the 500 mA each, I know that wiring lithium batteries in parallel is supposed to be a no-no

I know we are off topic for software now!

This thing was previously running on a single nickel rechargeable 170mAh and I think that is why with 5 servos and the sensor reading were fine but with servo six things got inconsistent because it could not supply the current even at full charge.

What about a 9v li-po rechargable at 500mAh?

My serial output with the battery and USB hooked up looks like this:

Code:

24cm center
22cm left
0cm right
4 Case
28cm center
29cm left
0cm right
4 Case
43cm center
62cm left
39cm right
6cm center
4 Case
0cm left
39cm right
4 Case
38cm center
16cm left
0cm right
4 Case
40cm center
0cm left
41cm right
80cm center over 20
59cm center over 20
42cm center over 20
40cm center over 20
0 Case
0 Case
0cm center
4cm left
69cm right
4 Case
0cm center
244cm left
70cm right
4 Case
220cm center
245cm left
69cm right
79cm center
246cm left
69cm right
210cm center over 20
0 Case
22cm center
246cm left
63cm right
0 Case
0 Case

It's still missing some sensor readings, although not all the time. Maybe i need to use a semaphore to get the two processes synchronized? I was moving the bot around during that to get different readings and the reported distances are accurate.

So I guess I was scratching my head for 15 hours because I need a second power source for the servos...

6 servos, 2 RGB lights an Arduino Micro and a Ping Sensor cant run on one 9v 170mAh battery, plus I learned the ins and outs of ChibiOS

 
 

Thanks for the help

Code:

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;
  }
}

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

Code:

#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
}

This was my final code:

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.

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.

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.

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

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

The serial outputs I added for diagnostics give me this:

Code:

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.

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

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

Code:

#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.