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
}