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