FireFly 2012: the robot code as an Arduino library 'hexapod' (v3.1): IK, SSC-32, IR, ultrasonic, GPS, 7-segment

From Electronics\Hexapod\Arduino library 2012.

2011-firefly-mega-ssc32/firmware/2012-library/hexapod/Servos.cpp

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