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

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

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

:cold_sweat:
:slight_smile:

Thanks for the help

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

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.

bytedisorder:
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

You can hardly use that battery for running that stuff.. The above battery is intended for powering systems with 20mA current consumption. A single servo can take 500mA or more when acting.
Better you use for example a 7.4V li-poly rechargeable battery pack.. :slight_smile:

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

500mAh - that is the capacity (C). The Q is how much current it can deliver (in a much shorter time of course).
Li-po batteries can do 1C up to 60C current (C=capacity) afaik, based on type etc. If the li-po pack is 500mAh and for example its C=10, it can deliver ~5A for ~6minutes..

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 :slight_smile:

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.