From f0b272a1d3fdcc7ef9dcd6544c3864185bdf7869 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Thu, 19 Jul 2012 20:51:25 +0200 Subject: [PATCH] FireFly 2012: start of a behaviour-based rewrite (unfinished, does not compile) From 'FireFly 19.rar'. --- .../2012-behaviours-wip/FireFly/Data.h | 71 +++++++ .../2012-behaviours-wip/FireFly/Def.h | 25 +++ .../2012-behaviours-wip/FireFly/FireFly.ino | 196 ++++++++++++++++++ .../FireFly/Gedrags_Classen.cpp | 88 ++++++++ .../2012-behaviours-wip/FireFly/SerD.h | 87 ++++++++ .../2012-behaviours-wip/FireFly/Ultrasone.cpp | 60 ++++++ .../2012-behaviours-wip/FireFly/Ultrasone.h | 39 ++++ .../2012-behaviours-wip/FireFly/Watchdog.cpp | 37 ++++ .../2012-behaviours-wip/FireFly/Watchdog.h | 20 ++ .../FireFly/concept_informatie.txt | 62 ++++++ 10 files changed, 685 insertions(+) create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.h create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h create mode 100644 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/concept_informatie.txt diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h new file mode 100644 index 0000000..9490e45 --- /dev/null +++ b/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 + + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h new file mode 100644 index 0000000..a58ff4f --- /dev/null +++ b/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 diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino new file mode 100644 index 0000000..bd6dd26 --- /dev/null +++ b/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); +} + + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp new file mode 100644 index 0000000..0656b03 --- /dev/null +++ b/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 + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h new file mode 100644 index 0000000..a4a340a --- /dev/null +++ b/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 + + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp new file mode 100644 index 0000000..d9b5d9e --- /dev/null +++ b/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 ); +*/ + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.h new file mode 100644 index 0000000..657b90b --- /dev/null +++ b/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 + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.cpp new file mode 100644 index 0000000..4822c5c --- /dev/null +++ b/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 + } +} + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h new file mode 100644 index 0000000..e8160ac --- /dev/null +++ b/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 + + diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/concept_informatie.txt b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/concept_informatie.txt new file mode 100644 index 0000000..74ddcb5 --- /dev/null +++ b/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. + */ + }; + +};