mangavalk
f690f307c1
From 'hexapodV3 28-april-2012-Backup.rar'. Oud/ is the 2011 code it was based on.
113 lines
2.1 KiB
C++
113 lines
2.1 KiB
C++
// Servos.cpp
|
|
|
|
#include "Servos.h"
|
|
|
|
void Servos::MoveHead (Data *DataLine)
|
|
{
|
|
#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)
|
|
{
|
|
if (DataLine->Send)
|
|
{
|
|
#ifdef DEBUGS
|
|
Serial.println("### Servos->Servos ###");
|
|
Serial.println("### Servos Send ###");
|
|
digitalWrite(13, HIGH);
|
|
delay(10);
|
|
digitalWrite(13, LOW);
|
|
#endif
|
|
|
|
#ifndef Freeze
|
|
mSend(DataLine);
|
|
#endif
|
|
|
|
DataLine->Send = false;
|
|
DataLine->ReadyToSend = false;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|