mangavalk
0911543c03
From Electronics\Hexapod\Arduino sketches 2011-2012\hexapodV3 (backup 2012-07-09).
61 lines
1.2 KiB
C++
61 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 );
|
|
*/
|
|
|
|
|
|
|
|
|