64 lines
1.4 KiB
C++
64 lines
1.4 KiB
C++
// Converter.cpp
|
|
// V3.0
|
|
|
|
#include "Converter.h"
|
|
|
|
void Converter::setup()
|
|
{
|
|
//Serial.begin(9600);
|
|
}
|
|
|
|
int ConverttoMS(int Position, int low, int high)
|
|
{
|
|
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
|
return low + tmp;
|
|
}
|
|
|
|
void ConvertServos(struct Data *DataLine)
|
|
{
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
DataLine->ServoData[i].t1 = ConverttoMS(DataLine->ServoData[i].o1, smin0, smax0);
|
|
DataLine->ServoData[i].t2 = ConverttoMS(DataLine->ServoData[i].o2, smin1, smax1);
|
|
DataLine->ServoData[i].t3 = ConverttoMS(DataLine->ServoData[i].o3, smin2, smax2);
|
|
}
|
|
|
|
#ifdef DEBUGS
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
if (i != 0)
|
|
Serial.print(", ");
|
|
else
|
|
Serial.print("### Servos->mSend ServosAngles Are: ");
|
|
|
|
Serial.print(DataLine->ServoData[i].o1);
|
|
Serial.print(", ");
|
|
Serial.print(DataLine->ServoData[i].o2);
|
|
Serial.print(", ");
|
|
Serial.print(DataLine->ServoData[i].o3);
|
|
}
|
|
Serial.println(";");
|
|
#endif
|
|
}
|
|
|
|
void Converter::ConvertHead(struct Data *DataLine)
|
|
{
|
|
DataLine->HeadPosition = ConverttoMS((90 + DataLine->HeadAngle), smin3, smax3);
|
|
|
|
#ifdef DEBUGC
|
|
Serial.print("### Converter->HeadPosition is: ");
|
|
Serial.println(DataLine->HeadPosition);
|
|
#endif
|
|
}
|
|
|
|
void Converter::Convert(Data *DataLine)
|
|
{
|
|
#ifdef DEBUGC
|
|
Serial.println("### Converter->Converter ###");
|
|
#endif
|
|
|
|
ConvertServos(DataLine);
|
|
}
|
|
|
|
|