FireFly 2012: start of a behaviour-based rewrite (unfinished, does not compile)

From 'FireFly 19.rar'.

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp

@@ -0,0 +1,60 @@
+//Ultrasone.cpp
+//  V3.0
+
+#include "Ultrasone.h"
+
+void Ultrasone::GetRadar(Data *DataLine) 
+{
+  DataLine->HeadTime = 0;
+
+#ifdef DEBUGUS
+  Serial.print("### Ultrasone Direction/Position: ");
+  Serial.print(CurrentDirection);
+  Serial.print(", ");
+  Serial.println(CurrentPosition);
+#endif
+
+  if (CurrentDirection == 0) // Left
+  {
+    if (CurrentPosition > MaxLeft)
+    {
+      CurrentPosition--;
+      DataLine->HeadAngle = CurrentPosition;
+
+      Con.ConvertHead(DataLine);
+      SSC32.MoveHead(DataLine);
+
+      Serial.print("### Ultrasone Distance: ");
+      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
+    }
+    else
+      CurrentDirection = 1;
+  }
+  else if(CurrentDirection == 1) // Right
+  {
+    if (CurrentPosition < MaxRight)
+    {
+      CurrentPosition++;
+      DataLine->HeadAngle = CurrentPosition;
+
+      Con.ConvertHead(DataLine);
+      SSC32.MoveHead(DataLine);
+
+      Serial.print("### Ultrasone Distance: ");
+      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
+    }
+    else
+      CurrentDirection = 0;
+  }
+}
+
+
+/*
+    [Vector]
+    X = Distance / Cos( CurrentPosition * pi2rad );
+    Y = Distance / sin( CurrentPosition / pi2rad );    
+*/
+
+
+
+