mangavalk
f690f307c1
From 'hexapodV3 28-april-2012-Backup.rar'. Oud/ is the 2011 code it was based on.
74 lines
1.4 KiB
C++
74 lines
1.4 KiB
C++
// Converter.cpp
|
|
|
|
#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);
|
|
//ConvertHead(DataLine);
|
|
|
|
#ifdef DEBUGC
|
|
Serial.println("### Converter->ReadyToSend = true");
|
|
#endif
|
|
|
|
DataLine->ReadyToSend = true;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|