60 lines
1.2 KiB
C++
60 lines
1.2 KiB
C++
|
//Ultrasone.cpp
|
||
|
|
// V3.0
|
||
|
|
|
||
|
|
#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;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
[Vector]
|
||
|
|
X = Distance / Cos( CurrentPosition * pi2rad );
|
||
|
|
Y = Distance / sin( CurrentPosition / pi2rad );
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|