mangavalk
f690f307c1
From 'hexapodV3 28-april-2012-Backup.rar'. Oud/ is the 2011 code it was based on.
56 lines
1.0 KiB
C++
56 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;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|