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.