55 lines
1.0 KiB
C++
55 lines
1.0 KiB
C++
|
//Ultrasone.cpp
|
||
|
|
|
||
|
|
#include "Ultrasone.h"
|
||
|
|
|
||
|
|
void Ultrasone::GetRadar(Data *DataLine)
|
||
|
|
{
|
||
|
|
DataLine->HeadTime = 0;
|
||
|
|
|
||
|
|
#ifdef DEBUGUS
|
||
|
|
Serial.print("### Ultrasone Direction/Position: ");
|
||
|
|
Serial.print(CurrentDirection);
|
||
|
|
Serial.print(", ");
|
||
|
|
Serial.println(CurrentPosition);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
if (CurrentDirection == 0) // Left
|
||
|
|
{
|
||
|
|
if (CurrentPosition > MaxLeft)
|
||
|
|
{
|
||
|
|
CurrentPosition--;
|
||
|
|
DataLine->HeadAngle = CurrentPosition;
|
||
|
|
|
||
|
|
Con.ConvertHead(DataLine);
|
||
|
|
SSC32.MoveHead(DataLine);
|
||
|
|
|
||
|
|
Serial.print("### Ultrasone Distance: ");
|
||
|
|
Serial.println(DataLine->FireFly.GetUltraSoneDistance());
|
||
|
|
}
|
||
|
|
else
|
||
|
|
CurrentDirection = 1;
|
||
|
|
}
|
||
|
|
else if(CurrentDirection == 1) // Right
|
||
|
|
{
|
||
|
|
if (CurrentPosition < MaxRight)
|
||
|
|
{
|
||
|
|
CurrentPosition++;
|
||
|
|
DataLine->HeadAngle = CurrentPosition;
|
||
|
|
|
||
|
|
Con.ConvertHead(DataLine);
|
||
|
|
SSC32.MoveHead(DataLine);
|
||
|
|
|
||
|
|
Serial.print("### Ultrasone Distance: ");
|
||
|
|
Serial.println(DataLine->FireFly.GetUltraSoneDistance());
|
||
|
|
}
|
||
|
|
else
|
||
|
|
CurrentDirection = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|