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/Data.h

@@ -0,0 +1,71 @@
+//Data.h Data Structure
+
+#ifndef DATA
+#define DATA
+
+#include "Arduino.h"
+#include "Def.h"
+#include "SerD.h"
+
+//    LAYOUT
+//      _U_
+//  0==/   \==3
+//     >***<
+// 1===|---|===4
+//     >***<
+//  2==\___/==5
+
+struct Data
+{
+  //MAIN
+  // Pointers to classes
+  //hexapod FireFly;
+
+  //Watchdog
+  boolean Send;
+  double LastSendTime;
+  int WalkTime;
+  int HeadTime;  //100
+
+  //Converter
+  SerD ServoData[6];
+  float HeadAngle;
+  int ReadyToSend;
+
+  //IK
+  int HeadPosition;
+  boolean ReadyToConvert;
+  float l3;
+  float s;
+
+  //Init
+  Data()
+  {
+    LastSendTime = 0;
+    HeadAngle = 0;
+    HeadTime = 100;
+
+    ServoData[0](5, 3, 8);
+    ServoData[1](8, 3, 0);
+    ServoData[2](5, 3, -8);
+    ServoData[3](5, 3, 8);
+    ServoData[4](8, 3, 0);
+    ServoData[5](5, 3, -8);
+  }
+
+  Data& operator() (DataObject *R)
+	{
+		ServoData[0] = R.s0;
+		ServoData[1] = R.s1;
+		ServoData[2] = R.s2;
+		ServoData[3] = R.s3;
+		ServoData[4] = R.s4;
+		ServoData[5] = R.s5;
+	}
+
+};
+
+#endif
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h

@@ -0,0 +1,25 @@
+// Def.h
+// Define some Hard Coded Variables
+//  V3.0
+
+//DEBUG CODES
+//#define DEBUGC  // Converter
+//#define DEBUGIK // InverseKinemetics
+//#define DEBUGS  // Servos
+//#define DEBUGHS // ServosHead
+//#define DEBUGW  // Watchdog
+//#define Freeze  // Protect all the servos
+
+//InverseKinemetics VARIABLES
+#define l2 15
+#define l1 10
+
+//Converter VARIABLES
+#define smin0 600
+#define smin1 960
+#define smin2 600
+#define smin3 600
+#define smax0 2400
+#define smax1 2100
+#define smax2 2400
+#define smax3 2400

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

@@ -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);
+}
+
+
+

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

@@ -0,0 +1,88 @@
+//Gedrags classen,
+
+#ifndef GCC
+#define GCC
+
+#include "SerD.h"
+
+//    LAYOUT
+//      _U_
+//  0==/   \==3
+//     >***<
+// 1===|---|===4
+//     >***<
+//  2==\___/==5
+
+class DataObject
+{
+public:
+
+	SerD S0;
+	SerD S1;
+	SerD S2;
+	SerD S3;
+	SerD S4;
+	SerD S5;
+
+	DataObject& operator() (SerD s0, SerD s1, SerD s2, SerD s3, SerD s4, SerD s5)
+	{
+		S0 = s0;
+		S1 = s1;
+		S2 = s2;
+		S3 = s3;
+		S4 = s4;
+		S5 = s5;
+	}
+
+	DataObject& operator() (	int a1, int a2, int a3,
+		int b1, int b2, int b3,
+		int c1, int c2, int c3,
+		int d1, int d2, int d3,
+		int e1, int e2, int e3,
+		int f1, int f2, int f3)
+	{
+		S0(a1, a2, a3);
+		S1(b1, b2, b3);
+		S2(c1, c2, c3);
+		S3(d1, d2, d3);
+		S4(e1, e2, e3);
+		S5(f1, f2, f3);
+	}
+};
+
+class WalkForward	//Direction
+{
+
+private:
+
+	DataObject LoopPatroon[8];
+	DataObject HuidigePositie;
+
+	//Startup VARIABLES
+	void init()
+	{
+		/*LoopPatroon.S0[0][0](, , );
+		LoopPatroon.S1[0][1](, , );
+		LoopPatroon.S3[0][2](, , );
+		LoopPatroon.S4[0][3](, , );
+		LoopPatroon.S5[0][4](, , );
+		LoopPatroon.S6[0][5](, , );*/
+
+		LoopPatroon[0](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+
+	}
+
+
+public:
+
+	void Run()
+	{
+		//Start runnning code until exit or expection raises.
+
+	}
+
+
+};
+
+#endif
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h

@@ -0,0 +1,87 @@
+// SerD.h
+
+#ifndef SERD
+#define SERD
+
+class SerD
+{
+public:
+
+  //Coordinate system
+  int x;
+  int y;
+  int z;
+
+  //Angles
+  int o1;
+  int o2;
+  int o3;
+
+  //Timings
+  int t1;
+  int t2;
+  int t3;
+
+  //Input
+  SerD& operator() (int X, int Y, int Z)
+  {
+    x = X;
+    y = Y;
+    z = Z;
+
+    return *this;
+  }
+
+  SerD& operator= (SerD& R)
+  {
+    x = R.x;
+    y = R.y;
+    z = R.z;
+
+    return *this;
+  }
+
+};
+
+class DataObject
+{
+public:
+
+	SerD S0;
+	SerD S1;
+	SerD S2;
+	SerD S3;
+	SerD S4;
+	SerD S5;
+
+	DataObject& operator() (SerD s0, SerD s1, SerD s2, SerD s3, SerD s4, SerD s5)
+	{
+		S0 = s0;
+		S1 = s1;
+		S2 = s2;
+		S3 = s3;
+		S4 = s4;
+		S5 = s5;
+	}
+
+	DataObject& operator() (	int a1, int a2, int a3,
+		int b1, int b2, int b3,
+		int c1, int c2, int c3,
+		int d1, int d2, int d3,
+		int e1, int e2, int e3,
+		int f1, int f2, int f3)
+	{
+		S0(a1, a2, a3);
+		S1(b1, b2, b3);
+		S2(c1, c2, c3);
+		S3(d1, d2, d3);
+		S4(e1, e2, e3);
+		S5(f1, f2, f3);
+	}
+};
+
+
+#endif
+
+
+

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 );    
+*/
+
+
+
+

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

@@ -0,0 +1,39 @@
+//Ultrasone.h
+//  V3.0
+
+#ifndef ULTRASONE_H
+#define ULTRASONE_H
+
+#include "Arduino.h"
+#include "Data.h"
+#include "Servos.h"
+#include "Converter.h"
+
+#define MaxLeft -45
+#define MaxRight 45
+
+class  Ultrasone
+{
+private:
+  Servos SSC32;
+  Converter Con;
+
+public:
+  void GetRadar(Data *DataLine);
+
+  int CurrentPosition;
+  byte CurrentDirection;
+
+  Ultrasone()
+  {
+    CurrentPosition = 0; 
+    CurrentDirection = 0; // Left
+  }
+};
+
+
+#endif
+
+
+
+

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

@@ -0,0 +1,37 @@
+// Watchdog.cpp
+
+#include "Watchdog.h"
+
+void Watchdog::CheckTime(Data *DataLine)
+{
+  if ((millis() >= (DataLine->LastSendTime + DataLine->WalkTime)) & (DataLine->ReadyToSend = true ))
+  {
+    DataLine->Send = 1;
+
+    DataLine->LastSendTime = millis();
+#ifdef DEBUGW
+    Serial.println("### WatchDog->Watchdog ###");
+    Serial.print("### ");
+    Serial.print( millis());
+    Serial.println(" WatchDog->DataLine.Send = True");
+    Serial.print("### WatchDog->Watchdog Walktime: ");
+    Serial.println(DataLine->WalkTime);
+#endif
+  }
+  else if ((DataLine->LastSendTime == 0 ) & (DataLine->ReadyToSend = true))
+  {
+    DataLine->Send = 1;
+
+    DataLine->LastSendTime = millis();
+#ifdef DEBUGW
+    Serial.println("### WatchDog->Watchdog Startup ###");
+    Serial.print("### ");
+    Serial.print( millis());
+    Serial.println(" WatchDog->DataLine.Send = True");
+    Serial.print("### WatchDog->Watchdog Walktime: ");
+    Serial.println(DataLine->WalkTime);
+#endif
+  }
+}
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h

@@ -0,0 +1,20 @@
+// Watchdog.h
+//  V3.0
+
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Watchdog 
+{
+public:
+  void CheckTime(Data *DataLine);
+
+};
+
+
+#endif
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/concept_informatie.txt

@@ -0,0 +1,62 @@
+class gedrag{
+
+	private:
+
+	data_line PositieInformatie;
+	body_struct main_positie;
+	int HuidigePositieinProgramma;
+	int matrix[..] MatrixmetdeGedragPatroonen;
+
+	public:
+
+	run()
+	{
+		/*
+			run normaal gedrag patroon.
+			
+			[loop patroon dat niet word beinvloed door userinput]
+			vraag aankomende gedrag op uit matrix,
+			-bereken de nieuwe posities
+			--sla deze op in cache
+			---wacht tot de watchdog een timeout geeft
+			----geef de cache door aan de servo controller.
+			
+			[loop patroon dat WORD beinvloed door userinput]
+			vraag aankomende gedrag op uit matrix met userinput,
+			-bereken de nieuwe posities
+			--sla deze op in cache
+			---loop tot de watchdog een timeout geeft
+			----geef de cache door aan de servo controller.
+		*/
+	};
+
+	quite()
+	{
+		while()
+		{
+			/*
+				maak routine af om terug te gaan naar de main positie.
+				daarna return 0;
+				
+				verplaats de body in een neutrale positie
+				+als niet mogeleijk verplaats de poten die problemen geven
+				-mogelijk ga verder met de poten een voor een in de goode positie te zetten
+			*/
+		}
+	};
+
+	exit()
+	{
+		/*
+			NOOD, stop nu en return positie;
+		*/
+	};
+
+	userinput()
+	{
+		/*
+			Mogelijkheid tot user input.
+		*/
+	};
+	
+};