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