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

From 'FireFly 19.rar'.
2012-07-19 20:51:25 +02:00
1 parent 0911543c03
commit f0b272a1d3
@@ -0,0 +1,196 @@
+// Hexapod FireFly
+//  V3.0
+
+#include "hexapod.h"
+#include "Data.h"
+#include "hexapod.h"
+//#include "Converter.h"
+//#include "InverseKinemetics.h"
+//#include "Servos.h"
+#include "Watchdog.h"
+#include "Ultrasone.h"
+
+//Data *DataLine = new Data;
+/*Converter Con;
+InverseKinemetics IK;
+Servos SSC32;*/
+Watchdog WD;
+Ultrasone US;
+hexapod FireFly;
+
+void setup()
+{
+  Serial.begin(38400);
+  Serial1.begin(38400);
+  pinMode(13, OUTPUT);  
+  //DataLine->WalkTime = 100;
+  FireFly.FireFly();
+}
+
+int a=0;
+int b=6;
+int tt;
+byte Sit;
+byte SitC;
+
+// Main loop.
+void loop()
+{
+  int d = FireFly.GetNewCmd();
+
+  US.GetRadar(DataLine);
+
+  /*if(Sit == 1)
+   if (SitC <= millis())
+   {
+   Sit = 0;
+   DataLine->WalkTime = tt;
+   } */
+
+  if (d != 0)
+  {
+    switch(d)
+    {
+    case power:
+      DataLine->ServoData[0](5, 3, 8);
+      DataLine->ServoData[1](8, 3, 0);
+      DataLine->ServoData[2](5, 3, -8);
+      DataLine->ServoData[3](5, 3, 8);
+      DataLine->ServoData[4](8, 3, 0);
+      DataLine->ServoData[5](5, 3, -8); 
+      break;
+    case up:  
+      Serial.println("UP");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].y -= 1;
+      break;
+    case down:  
+      Serial.println("DOWN");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].y += 1;
+      break;
+    case left:  
+      Serial.println("LEFT");
+      for (int i=a; i<=b; i++)
+      {
+        if (i<3)
+          DataLine->ServoData[i].x += 1;
+        else
+          DataLine->ServoData[i].x -= 1;
+      }
+      break;
+    case right:  
+      Serial.println("RIGHT");
+      for (int i=a; i<=b; i++)
+      {
+        if (i<3)
+          DataLine->ServoData[i].x -= 1;
+        else
+          DataLine->ServoData[i].x += 1;
+      }
+      break;
+    case forward:  
+      Serial.println("forward");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].z -= 1;
+      break;
+    case back:  
+      Serial.println("back");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].z += 1;
+      break;
+    case ok:  
+      Serial.println("OK");
+
+      tt = DataLine->WalkTime;
+      DataLine->WalkTime = 3000;
+
+      DataLine->ServoData[0](5, 3, 8);
+      DataLine->ServoData[1](8, 3, 0);
+      DataLine->ServoData[2](5, 3, -8);
+      DataLine->ServoData[3](5, 3, 8);
+      DataLine->ServoData[4](8, 3, 0);
+      DataLine->ServoData[5](5, 3, -8);
+
+      /*IK.IK(DataLine);
+      Con.Convert(DataLine);
+      DataLine->Send = 1;
+      SSC32.Send(DataLine);*/
+
+      Sit = 1;
+      break;
+    case IR_0:  
+      Serial.println("0");
+      a = 0;
+      b = 5;
+      break;
+    case IR_1:  
+      Serial.println("1");
+      a = 0;
+      b = 0;
+      break;
+      /*case IR_2:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_3:  
+      Serial.println("3");
+      a = 3;
+      b = 3;
+      break;
+    case IR_4:  
+      Serial.println("4");
+      a = 1;
+      b = 1;
+      break;
+      /*case IR_5:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_6:  
+      Serial.println("6");
+      a = 4;
+      b = 4;
+      break;
+    case IR_7:  
+      Serial.println("7");
+      a = 2;
+      b = 2;
+      break;
+      /*case IR_8:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_9:  
+      Serial.println("9");
+      a = 5;
+      b = 5;
+      break;
+    default:  
+      Serial.println(d);
+      break;
+    }
+  }
+  /*IK.IK(DataLine);
+  Con.Convert(DataLine);*/
+  WD.CheckTime(DataLine);
+
+  if (Sit){
+    if ((DataLine->Send) & (SitC))
+    {
+      DataLine->WalkTime = tt;
+      SitC = 0;
+      Sit = 0;
+    }
+    else if (DataLine->Send)
+      SitC = 1;
+  }
+
+  //SSC32.Send(DataLine);
+}
+
+
+