FireFly 2012: behaviour rewrite now uses the hexapod library
From Electronics\Hexapod\Arduino sketches 2011-2012\FireFly. Data.h, Def.h and SerD.h moved into the library.
This commit is contained in:
mangavalk committed
1 parent
304367a769
commit
6e50819e01
9 files changed
+84
-465
No files matched your search
@@ -1,71 +0,0 @@
|
||||
//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
|
||||
|
||||
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
// 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
|
||||
@@ -1,196 +1,31 @@
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
// Hexapod FireFly
|
||||
|
||||
#include "hexapod.h"
|
||||
|
||||
#include "Gedrags_Classen.cpp"
|
||||
|
||||
//Watchdog WD;
|
||||
//Ultrasone US;
|
||||
hexapod *FireFly = new hexapod;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial1.begin(38400);
|
||||
pinMode(13, OUTPUT);
|
||||
|
||||
FireFly->FireFly();
|
||||
}
|
||||
|
||||
// Main loop.
|
||||
void loop()
|
||||
{
|
||||
DataObject *cache = new DataObject;
|
||||
|
||||
cache->input(5, 3, 8, 8, 3, 0, 5, 3, -8, 5, 3, 8, 8, 3, 0, 5, 3, -8, 500);
|
||||
|
||||
//WD.CheckTime(DataLine);
|
||||
|
||||
FireFly->CoreSend(cache);
|
||||
}
|
||||
|
||||
@@ -1,88 +1,55 @@
|
||||
//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
|
||||
//Gedrags classen,
|
||||
|
||||
#ifndef GCC
|
||||
#define GCC
|
||||
|
||||
#include "hexapod.h"
|
||||
|
||||
// LAYOUT
|
||||
// _U_
|
||||
// 0==/ \==3
|
||||
// >***<
|
||||
// 1===|---|===4
|
||||
// >***<
|
||||
// 2==\___/==5
|
||||
|
||||
class WalkForward //Direction
|
||||
{
|
||||
|
||||
private:
|
||||
DataObject LoopPatroon[8];
|
||||
DataObject HuidigePositie;
|
||||
hexapod *FireFly;
|
||||
|
||||
//Startup VARIABLES
|
||||
void init(hexapod *pntr)
|
||||
{
|
||||
LoopPatroon[0](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[1](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[2](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[3](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[4](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[5](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[6](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
LoopPatroon[7](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
|
||||
|
||||
FireFly = pntr;
|
||||
}
|
||||
|
||||
void send(DataObject pntr)
|
||||
{
|
||||
FireFly->CoreSend(&pntr);
|
||||
}
|
||||
|
||||
|
||||
public:
|
||||
void Run()
|
||||
{
|
||||
//Start runnning code until exit or expection raises.
|
||||
send(LoopPatroon[0]);//Test
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,87 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
File renamed without changes.
File renamed without changes.
File renamed without changes.
Reference in new issue
Block a user