103 lines
2.0 KiB
C++
103 lines
2.0 KiB
C++
// Servos.cpp
|
|
// V3.0
|
|
|
|
#include "Servos.h"
|
|
|
|
void Servos::MoveHead (Data *DataLine)
|
|
{
|
|
Serial.begin(38400);
|
|
Serial1.begin(38400);
|
|
|
|
#ifdef DEBUGHS
|
|
Serial.print("### Servos->Head Time/Position: ");
|
|
Serial.print(DataLine->HeadTime);
|
|
Serial.print(", ");
|
|
Serial.println(DataLine->HeadPosition);
|
|
#endif
|
|
|
|
Serial1.print(" #");
|
|
Serial1.print(9);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->HeadPosition);
|
|
Serial1.print(" T");
|
|
Serial1.println(DataLine->HeadTime);
|
|
}
|
|
|
|
void mSend(Data *DataLine)
|
|
{
|
|
int s=3;
|
|
|
|
for (int i=0; i<9; i += 3){
|
|
Serial1.print(" #");
|
|
Serial1.print(i);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t1);
|
|
Serial1.print(" #");
|
|
Serial1.print(i+1);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t2);
|
|
Serial1.print(" #");
|
|
Serial1.print(i+2);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t3);
|
|
s++;
|
|
}
|
|
|
|
s = 0;
|
|
|
|
for (int i=16; i<25; i += 3){
|
|
Serial1.print(" #");
|
|
Serial1.print(i);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t1);
|
|
Serial1.print(" #");
|
|
Serial1.print(i+1);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t2);
|
|
Serial1.print(" #");
|
|
Serial1.print(i+2);
|
|
Serial1.print(" P");
|
|
Serial1.print(DataLine->ServoData[s].t3);
|
|
s++;
|
|
}
|
|
|
|
Serial1.print(" T");
|
|
Serial1.println(DataLine->WalkTime);
|
|
|
|
#ifdef DEBUGC
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
if (i != 0)
|
|
Serial.print(", ");
|
|
else
|
|
{
|
|
Serial.print("### Converter->ServosAnglesTime Are: ");
|
|
}
|
|
|
|
Serial.print(DataLine->ServoData[i].t1);
|
|
Serial.print(", ");
|
|
Serial.print(DataLine->ServoData[i].t2);
|
|
Serial.print(", ");
|
|
Serial.print(DataLine->ServoData[i].t3);
|
|
}
|
|
Serial.println(";");
|
|
#endif
|
|
}
|
|
|
|
void Servos::Send(Data *DataLine)
|
|
{
|
|
#ifdef DEBUGS
|
|
Serial.println("### Servos->Servos ###");
|
|
Serial.println("### Servos Send ###");
|
|
digitalWrite(13, HIGH);
|
|
delay(10);
|
|
digitalWrite(13, LOW);
|
|
#endif
|
|
|
|
#ifndef Freeze
|
|
mSend(DataLine);
|
|
#endif
|
|
}
|
|
|
|
|