FireFly 2012 v3: restructured into Data/Converter/IK/Servos classes, IR remote, ultrasonic radar head

From 'hexapodV3 28-april-2012-Backup.rar'. Oud/ is the 2011 code it was based on.

2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.cpp

@@ -0,0 +1,112 @@
+// 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;
+  }
+}
+
+
+
+
+
+
+
+
+
+