197 lines
3.8 KiB
Arduino
197 lines
3.8 KiB
Arduino
// 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);
|
|
}
|
|
|
|
|
|
|