FireFly 2012: start of a behaviour-based rewrite (unfinished, does not compile)
From 'FireFly 19.rar'.
This commit is contained in:
mangavalk committed
1 parent
0911543c03
commit
f0b272a1d3
10 files changed
+685
No files matched your search
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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 );
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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.
|
||||
*/
|
||||
};
|
||||
|
||||
};
|
||||
Reference in new issue
Block a user