FireFly 2012 v3: restructured into Data/Converter/IK/Servos classes, IR remote, ultrasonic radar head
From 'hexapodV3 28-april-2012-Backup.rar'. Oud/ is the 2011 code it was based on.
This commit is contained in:
mangavalk committed
1 parent
2e063fc0d9
commit
f690f307c1
30 files changed
+1990
No files matched your search
@@ -0,0 +1,73 @@
|
||||
// Converter.cpp
|
||||
|
||||
#include "Converter.h"
|
||||
|
||||
void Converter::setup()
|
||||
{
|
||||
//Serial.begin(9600);
|
||||
}
|
||||
|
||||
int ConverttoMS(int Position, int low, int high)
|
||||
{
|
||||
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
||||
return low + tmp;
|
||||
}
|
||||
|
||||
void ConvertServos(struct Data *DataLine)
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
DataLine->ServoData[i].t1 = ConverttoMS(DataLine->ServoData[i].o1, smin0, smax0);
|
||||
DataLine->ServoData[i].t2 = ConverttoMS(DataLine->ServoData[i].o2, smin1, smax1);
|
||||
DataLine->ServoData[i].t3 = ConverttoMS(DataLine->ServoData[i].o3, smin2, smax2);
|
||||
}
|
||||
|
||||
#ifdef DEBUGS
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("### Servos->mSend ServosAngles Are: ");
|
||||
|
||||
Serial.print(DataLine->ServoData[i].o1);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].o2);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].o3);
|
||||
}
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
void Converter::ConvertHead(struct Data *DataLine)
|
||||
{
|
||||
DataLine->HeadPosition = ConverttoMS((90 + DataLine->HeadAngle), smin3, smax3);
|
||||
|
||||
#ifdef DEBUGC
|
||||
Serial.print("### Converter->HeadPosition is: ");
|
||||
Serial.println(DataLine->HeadPosition);
|
||||
#endif
|
||||
}
|
||||
|
||||
void Converter::Convert(Data *DataLine)
|
||||
{
|
||||
#ifdef DEBUGC
|
||||
Serial.println("### Converter->Converter ###");
|
||||
#endif
|
||||
|
||||
ConvertServos(DataLine);
|
||||
//ConvertHead(DataLine);
|
||||
|
||||
#ifdef DEBUGC
|
||||
Serial.println("### Converter->ReadyToSend = true");
|
||||
#endif
|
||||
|
||||
DataLine->ReadyToSend = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
// Converter.h
|
||||
|
||||
#ifndef CONVERTER_H
|
||||
#define CONVERTER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Data.h"
|
||||
|
||||
class Converter
|
||||
{
|
||||
public:
|
||||
void setup();
|
||||
|
||||
void Convert(Data *DataLine);
|
||||
void ConvertHead(Data *DataLine);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
//Data.h Data Structure
|
||||
|
||||
#ifndef DATA
|
||||
#define DATA
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.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;
|
||||
|
||||
//Startup VARIABLES
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
//Def.h
|
||||
//Define some Hard Coded Variables
|
||||
|
||||
//DEBUG CODES
|
||||
//#define DEBUGC
|
||||
//#define DEBUGIK
|
||||
//#define DEBUGS
|
||||
//#define DEBUGHS // ServosHead
|
||||
//#define DEBUGW
|
||||
//#define Freeze
|
||||
|
||||
//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,158 @@
|
||||
#include "InverseKinemetics.h"
|
||||
|
||||
//Variables
|
||||
boolean error;
|
||||
|
||||
void RL3 (int i, Data *DataLine)
|
||||
{
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
|
||||
if ((x == 0) && (z == 0))
|
||||
DataLine->l3 = 0;
|
||||
else DataLine->l3 = hypot(z, x);
|
||||
|
||||
if (x < 0)
|
||||
DataLine->l3 = DataLine->l3 * -1;
|
||||
}
|
||||
|
||||
float O1 (int i, Data *DataLine)
|
||||
{
|
||||
RL3(i, DataLine);
|
||||
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
float r;
|
||||
|
||||
if ((x != 0) & (z != 0)){
|
||||
double a = (z / x);
|
||||
return atan(a);
|
||||
}
|
||||
else
|
||||
return atan(0.0); // Math Error Protection.
|
||||
}
|
||||
|
||||
float T1 (int i, Data *DataLine)
|
||||
{
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float y = DataLine->ServoData[i].y;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
float x2 = x * x;
|
||||
float y2 = y * y;
|
||||
float z2 = z * z;
|
||||
float l32 = DataLine->l3 * DataLine->l3;
|
||||
|
||||
DataLine->s = sqrt(x2 + y2 + z2);
|
||||
|
||||
float t1 = 0;
|
||||
|
||||
float s2 = DataLine->s * DataLine->s;
|
||||
|
||||
if ((y == 0) && (DataLine->l3 == 0))
|
||||
t1 = 0; // Math Error Protection.
|
||||
else
|
||||
t1 = (atan2(float(DataLine->l3), float(y)));
|
||||
|
||||
float a = (l1 * l1) - (l2*l2) + s2;
|
||||
float b = 2 * l1 * DataLine->s;
|
||||
float t2 = acos(a / b);
|
||||
|
||||
float ret = 0;
|
||||
|
||||
ret = t1 + abs(t2);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
float T3 (Data *DataLine)
|
||||
{
|
||||
return (acos(((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
|
||||
}
|
||||
|
||||
void setleg (int i, Data *DataLine)
|
||||
{
|
||||
Segment Seg;
|
||||
|
||||
if (i < 3)
|
||||
{
|
||||
float a = ((float)90 - degrees(O1(i, DataLine)));
|
||||
float b = ((float)180 - degrees(T1(i, DataLine)));
|
||||
float c = degrees(T3(DataLine));
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
DataLine->ServoData[i].o1 = a;
|
||||
DataLine->ServoData[i].o2 = b;
|
||||
DataLine->ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
Seg.sevenSegWrite('e');
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float a = ((float)90 + degrees(O1(i, DataLine)));
|
||||
float b = degrees(T1(i, DataLine));
|
||||
float c = ((float)180 - degrees(T3(DataLine)));
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
DataLine->ServoData[i].o1 = a;
|
||||
DataLine->ServoData[i].o2 = b;
|
||||
DataLine->ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
Seg.sevenSegWrite('e');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void InverseKinemetics::IK(Data *DataLine)
|
||||
{
|
||||
Segment Seg;
|
||||
Seg.sevenSegWrite('o');
|
||||
|
||||
error = false;
|
||||
|
||||
for (int i=0; i<6; i++)
|
||||
setleg(i, DataLine);
|
||||
|
||||
#ifdef DEBUGIK
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("### InverseKinemetics->ServosPosition's are: ");
|
||||
|
||||
Serial.print(DataLine->ServoData[i].x);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].y);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].z);
|
||||
}
|
||||
if (error == true)
|
||||
Serial.println("; ERROR");
|
||||
else
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
// InverseKinemetics.h
|
||||
|
||||
#ifndef inverseKinemetics_H
|
||||
#define inverseKinemetics_H
|
||||
|
||||
#include "Data.h"
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.h"
|
||||
|
||||
class InverseKinemetics
|
||||
{
|
||||
public:
|
||||
void IK(Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
#include "Data_Manager.h"
|
||||
|
||||
// Raw variable that must be send to the ssc32.
|
||||
int RAW_2_SERVO[18];
|
||||
|
||||
// Angel to converter.
|
||||
int DEG_2_CON[18];
|
||||
|
||||
// Coordinate to iverse kinematics.
|
||||
int COR_2_IK[18];
|
||||
|
||||
// Orbit angles.
|
||||
int ORBIT_X, ORBIT_Y, ORBIT_Z;
|
||||
|
||||
// Hight.
|
||||
int HIGHT;
|
||||
|
||||
// Walk Direction relative from body.
|
||||
int DIR_R_BODY;
|
||||
|
||||
// IK Legnth.
|
||||
int l1, l2;
|
||||
|
||||
// Min and Max for SSC32.
|
||||
int smin[3], smax[3];
|
||||
|
||||
// WalkTime
|
||||
int walktime;
|
||||
@@ -0,0 +1,20 @@
|
||||
// Data_Manager.h
|
||||
|
||||
#ifndef DATA_MANAGER
|
||||
#define DATA_MANAGER
|
||||
|
||||
extern int RAW_2_SERVO[18];
|
||||
extern int DEG_2_CON[18];// int
|
||||
extern int COR_2_IK[18];
|
||||
extern int ORBIT_X, ORBIT_Y, ORBIT_Z;
|
||||
extern int HIGHT;
|
||||
extern int DIRECTION;
|
||||
extern int l1, l2;
|
||||
extern int smin[3], smax[3];
|
||||
extern int walktime;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
// Include.
|
||||
#include <math.h>
|
||||
#include "InverseKinemetics.h"
|
||||
#include "SSC32.h"
|
||||
#include "sequnces.h"
|
||||
#include "Orbit.h"
|
||||
#include "Data_Manager.h"
|
||||
|
||||
// Classes.
|
||||
InverseKinemetics ik;
|
||||
SSC32 body;
|
||||
Orbit orbit;
|
||||
|
||||
// Nunchuck.
|
||||
#include "WiiChuck.h"
|
||||
|
||||
#define MAXANGLE 90
|
||||
#define MINANGLE -90
|
||||
|
||||
WiiChuck chuck = WiiChuck();
|
||||
int angleStart, currentAngle;
|
||||
int tillerStart = 100;
|
||||
double angle;
|
||||
|
||||
// Functions.
|
||||
void FireFly()
|
||||
{
|
||||
Serial.println("");
|
||||
Serial.println(" ____ ____ ___ ");
|
||||
Serial.println("/\\ _`\\ __ /\\ _`\\ /\\_ \\ ");
|
||||
Serial.println("\\ \\ \\L\\_\\/\\_\\ _ __ __\\ \\ \\L\\_\\//\\ \\ __ __ ");
|
||||
Serial.println(" \\ \\ _\\/\\/\\ \\/\\`'__\\/'__`\\ \\ _\\/ \\ \\ \\ /\\ \\/\\ \\ ");
|
||||
Serial.println(" \\ \\ \\/ \\ \\ \\ \\ \\//\\ __/\\ \\ \\/ \\_\\ \\_\\ \\ \\_\\ \\ ");
|
||||
Serial.println(" \\ \\_\\ \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\ /\\____\\\\/`____ \\ ");
|
||||
Serial.println(" \\/_/ \\/_/\\/_/ \\/____/ \\/_/ \\/____/ `/___/> \\");
|
||||
Serial.println(" /\\___/");
|
||||
Serial.println(" \\/__/ ");
|
||||
Serial.println("");
|
||||
Serial.println("Version 3.0");
|
||||
delay(800);
|
||||
}
|
||||
|
||||
void warning ()
|
||||
{
|
||||
digitalWrite(13, HIGH);
|
||||
delay(100);
|
||||
digitalWrite(13, LOW);
|
||||
}
|
||||
|
||||
|
||||
+157
@@ -0,0 +1,157 @@
|
||||
//#include "WProgram.h"
|
||||
#include "InverseKinemetics.h"
|
||||
|
||||
void InverseKinemetics::RL3 (int i)
|
||||
{
|
||||
l3 = sqrt(pow(abs(COR_2_IK[0+(i*3)]), 2) + pow(abs(COR_2_IK[2+(i*3)]), 2));
|
||||
}
|
||||
|
||||
float InverseKinemetics::ZX (int i)
|
||||
{
|
||||
RL3(i);
|
||||
if (COR_2_IK[2+(i*3)] != 0)
|
||||
float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
|
||||
else
|
||||
float temp = 90; // Math Error Protection.
|
||||
return temp;
|
||||
}
|
||||
|
||||
float InverseKinemetics::TT (int i)
|
||||
{
|
||||
int x = pow(abs(COR_2_IK[0+(i*3)]), 2);
|
||||
int y = pow(abs(COR_2_IK[1+(i*3)]), 2);
|
||||
int z = pow(abs(COR_2_IK[2+(i*3)]), 2);
|
||||
s = sqrt(x + y + z);
|
||||
|
||||
if (s > (l1 + l2) )
|
||||
R[i] = 3;
|
||||
|
||||
float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2))
|
||||
/ (2 * s * COR_2_IK[1+(i*3)])));
|
||||
|
||||
float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2))
|
||||
/ (2 * s * l1)));
|
||||
|
||||
float temp = (int)t1 + t2;
|
||||
//Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp);
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
int InverseKinemetics::LL ()
|
||||
{
|
||||
int temp = degrees(acos((float)(pow(l1, 2) + pow(l2, 2) - pow(s, 2)) / (2 * l1 * l2)));
|
||||
return temp;
|
||||
}
|
||||
|
||||
void InverseKinemetics::setleg (int i)
|
||||
{
|
||||
Serial.print("Set Leg ");
|
||||
Serial.println(i);
|
||||
if (i >= 3)
|
||||
{
|
||||
//Error_Coordinate(i);
|
||||
DEG_2_CON[0+(i*3)] = 180 - ZX(i);
|
||||
DEG_2_CON[1+(i*3)] = 180 - TT(i);
|
||||
DEG_2_CON[2+(i*3)] = LL();
|
||||
Error_Angle(i);
|
||||
}
|
||||
else
|
||||
{
|
||||
//Error_Coordinate(i);
|
||||
DEG_2_CON[0+(i*3)] = ZX(i);
|
||||
DEG_2_CON[1+(i*3)] = TT(i);
|
||||
DEG_2_CON[2+(i*3)] = 180 - LL();
|
||||
Error_Angle(i);
|
||||
}
|
||||
}
|
||||
|
||||
void InverseKinemetics::setlegs ()
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
setleg(i);
|
||||
}
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("Cor: ");
|
||||
Serial.print(COR_2_IK[0+(i*3)]);
|
||||
Serial.print(", ");
|
||||
Serial.print(COR_2_IK[1+(i*3)]);
|
||||
Serial.print(", ");
|
||||
Serial.print(COR_2_IK[2+(i*3)]);
|
||||
}
|
||||
Serial.println(";");
|
||||
}
|
||||
|
||||
void InverseKinemetics::Error_Angle (int i)
|
||||
{
|
||||
for (int i2=0; i2<3; i2++)
|
||||
R[i2+(i*3)] = 0;
|
||||
if (DEG_2_CON[0+(i*3)] < 0)
|
||||
R[0+(i*3)] = 1;
|
||||
if (DEG_2_CON[0+(i*3)] > 180)
|
||||
R[0+(i*3)] = 2;
|
||||
if (DEG_2_CON[1+(i*3)] < 0)
|
||||
R[1+(i*3)] = 1;
|
||||
if (DEG_2_CON[1+(i*3)] > 180)
|
||||
R[1+(i*3)] = 2;
|
||||
if (DEG_2_CON[2+(i*3)] < 0)
|
||||
R[2+(i*3)] = 1;
|
||||
if (DEG_2_CON[2+(i*3)] > 180)
|
||||
R[2+(i*3)] = 2;
|
||||
}
|
||||
|
||||
void InverseKinemetics::Error_Coordinate (int i)
|
||||
{
|
||||
Ra[i] = 0;
|
||||
float temp = sqrt(pow(COR_2_IK[0+(i*3)], 2) + pow(COR_2_IK[1+(i*3)], 2) + pow(COR_2_IK[2+(i*3)], 2));
|
||||
if (temp <= 5) {
|
||||
Ra[i] = 1;
|
||||
}
|
||||
if (temp > l1 + l2) {
|
||||
Ra[i] = 2;
|
||||
}
|
||||
}
|
||||
|
||||
int InverseKinemetics::CheckForErrorCor()
|
||||
{
|
||||
for (int i=1; i<19; i++)
|
||||
if (R[i-1]!=0)
|
||||
return i;
|
||||
return 0;
|
||||
}
|
||||
|
||||
String InverseKinemetics::CheckForErrorCor_print()
|
||||
{
|
||||
String temp = "Error Coordinate: ";
|
||||
for (int i=0; i<18; i++)
|
||||
{
|
||||
temp = temp + (int)DEG_2_CON[i] + ", ";
|
||||
}
|
||||
temp = temp + "\nCheckForErrorCor returnd: " + CheckForErrorCor();
|
||||
return temp;
|
||||
}
|
||||
|
||||
int InverseKinemetics::CheckForErrorAng()
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
if (Ra[i]!=0)
|
||||
return i;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
#ifndef InverseKinemetics_H
|
||||
#define InverseKinemetics_H
|
||||
|
||||
#include "WProgram.h"
|
||||
#include <math.h>
|
||||
#include "Data_Manager.h"
|
||||
|
||||
class InverseKinemetics
|
||||
{
|
||||
public:
|
||||
//
|
||||
void RL3(int i);
|
||||
float ZX (int i);
|
||||
float TT (int i);
|
||||
int LL ();
|
||||
void setlegs ();
|
||||
void setleg (int i);
|
||||
|
||||
/* // Input
|
||||
void setLeftFrontLeg (int ix, int iy, int iz); // clockwise, t3=clockwise, t3=counterclockwise
|
||||
void setLeftMiddleLeg (int ix, int iy, int iz); // clockwise, t3=clockwise, t3=counterclockwise
|
||||
void setLeftBackLeg (int ix, int iy, int iz); // clockwise, t3=clockwise, t3=counterclockwise
|
||||
void setRightFrontLeg (int ix, int iy, int iz); // counterclockwise, t3=counterclockwise, t3=clockwise
|
||||
void setRightMiddleLeg(int ix, int iy, int iz); // counterclockwise, t3=counterclockwise, t3=clockwise
|
||||
void setRightBackLeg (int ix, int iy, int iz); // counterclockwise, t3=counterclockwise, t3=clockwise*/
|
||||
|
||||
// 0 | 3
|
||||
// 1 | 4
|
||||
// 2 | 5
|
||||
|
||||
//
|
||||
void Error_Angle (int i);
|
||||
void Error_Coordinate (int i);
|
||||
|
||||
int CheckForErrorCor();
|
||||
String CheckForErrorCor_print();
|
||||
int CheckForErrorAng();
|
||||
|
||||
// Variable's
|
||||
int s;
|
||||
float l3;
|
||||
byte R[18];
|
||||
byte Ra[6];
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
// Orbit.cpp
|
||||
|
||||
#include "Orbit.h"
|
||||
|
||||
void Orbit::getData(int iy[6], int ix[6], int iz[6])
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
x[i] = ix[i];
|
||||
y[i] = iy[i];
|
||||
z[i] = iz[i];
|
||||
}
|
||||
lx[0] = 5;
|
||||
lx[1] = 7;
|
||||
lx[2] = 5;
|
||||
lx[3] = 5;
|
||||
lx[4] = 7;
|
||||
lx[5] = 5;
|
||||
ly[0] = 10;
|
||||
ly[1] = 0;
|
||||
ly[2] = 10;
|
||||
ly[3] = 10;
|
||||
ly[4] = 0;
|
||||
ly[5] = 10;
|
||||
}
|
||||
|
||||
void Orbit::getAngle (int ix, int iy)
|
||||
{
|
||||
oP = ix;
|
||||
o2P = iy;
|
||||
oN = 180 - ix;
|
||||
o2N = 180 - iy;
|
||||
}
|
||||
|
||||
void Orbit::calculate()
|
||||
{
|
||||
// X Size.
|
||||
// Right side.
|
||||
for (int i=3; i<6; i++)
|
||||
{
|
||||
float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
|
||||
float o1 = degrees(atan(y[i] / x[i]));
|
||||
float o2 = oP - o1;
|
||||
Ny[i] = sin(radians(o2)) * l1;
|
||||
Nx[i] = (cos(radians(o2)) * l1) - lx[i];
|
||||
Nz[i] = z[i];
|
||||
}
|
||||
|
||||
// Left size;
|
||||
for (int i=0; i<4; i++)
|
||||
{
|
||||
float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
|
||||
float o1 = degrees(atan(y[i] / x[i]));
|
||||
float o2 = oN - o1;
|
||||
Ny[i] = sin(radians(o2)) * l1;
|
||||
Nx[i] = (cos(radians(o2)) * l1) - lx[i];
|
||||
Nz[i] = z[i];
|
||||
}
|
||||
|
||||
// Y Size.
|
||||
// Right side.
|
||||
/*for (int i=3; i<6; i++) WRong
|
||||
{
|
||||
float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
|
||||
float o1 = degrees(atan(y[i] / x[i]));
|
||||
float o2 = o2P - o1;
|
||||
Ny[i] = sin(radians(o2)) * l1;
|
||||
Nx[i] = (cos(radians(o2)) * l1) - lx[i];
|
||||
Nz[i] = z[i];
|
||||
}
|
||||
|
||||
// Left size;
|
||||
for (int i=0; i<4; i++) WRong
|
||||
{
|
||||
float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
|
||||
float o1 = degrees(atan(y[i] / x[i]));
|
||||
float o2 = o2N - o1;
|
||||
Ny[i] = sin(radians(o2)) * l1;
|
||||
Nx[i] = (cos(radians(o2)) * l1) - lx[i];
|
||||
Nz[i] = z[i];
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
// Orbit.h
|
||||
|
||||
#ifndef Orbit_H
|
||||
#define Orbit_H
|
||||
|
||||
#include "WProgram.h"
|
||||
|
||||
class Orbit
|
||||
{
|
||||
public:
|
||||
void getData(int ix[6], int iy[6], int iz[6]);
|
||||
void getAngle (int ix, int iy);
|
||||
|
||||
void calculate();
|
||||
|
||||
int x[6], y[6], z[6];
|
||||
int Nx[6], Ny[6], Nz[6];
|
||||
int oP, o2P, oN, o2N;
|
||||
int lx[6], ly[6];
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
// Rotation.cpp
|
||||
|
||||
#include "Rotation.h"
|
||||
|
||||
void Rotation::getData(int ix[6], int iy[6], int iz[6])
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
x[i] = ix[i];
|
||||
y[i] = iy[i];
|
||||
z[i] = iz[i];
|
||||
}
|
||||
}
|
||||
|
||||
int NewX, NewY;
|
||||
void rotate (int x, int y, int angel)
|
||||
{
|
||||
NewY = x * sin(radians(angel)) + y * cos(radians(angel));
|
||||
NewX = x * cos(radians(angel)) - y * sin(radians(angel));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
// Rotation.h
|
||||
|
||||
#ifndef Rotation_H
|
||||
#define Rotation_H
|
||||
|
||||
#include "WProgram.h"
|
||||
|
||||
class Rotation
|
||||
{
|
||||
public:
|
||||
void getData(int ix[6], int iy[6], int iz[6]);
|
||||
|
||||
int x[6], y[6], z[6];
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
#include "SSC32.h"
|
||||
|
||||
void SSC32::setup()
|
||||
{
|
||||
Serial1.begin(38400);
|
||||
Serial.begin(38400);
|
||||
time = 0;
|
||||
walktime = 0;
|
||||
}
|
||||
|
||||
void SSC32::MoveHead (int angel_from_mid)
|
||||
{
|
||||
Serial1.print(" #");
|
||||
Serial1.print(9);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS((90 + angel_from_mid), smin[0], smax[0]));
|
||||
Serial1.print(" T");
|
||||
Serial1.println(100);
|
||||
}
|
||||
|
||||
void SSC32::Render()
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
RAW_2_SERVO[0+(i*3)] = ConverttoMS(DEG_2_CON[0+(i*3)], smin[0], smax[0]);
|
||||
RAW_2_SERVO[1+(i*3)] = ConverttoMS(DEG_2_CON[1+(i*3)], smin[1], smax[1]);
|
||||
RAW_2_SERVO[2+(i*3)] = ConverttoMS(DEG_2_CON[2+(i*3)], smin[2], smax[2]);
|
||||
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("Ang: ");
|
||||
|
||||
Serial.print(DEG_2_CON[0+(i*3)]);
|
||||
Serial.print(", ");
|
||||
Serial.print(DEG_2_CON[1+(i*3)]);
|
||||
Serial.print(", ");
|
||||
Serial.print(DEG_2_CON[2+(i*3)]);
|
||||
}
|
||||
Serial.println(";");
|
||||
}
|
||||
|
||||
int SSC32::ConverttoMS(int i, int low, int high)
|
||||
{
|
||||
int tmp = ( i * ( ( high - low ) / 180 ) );
|
||||
return low + tmp;
|
||||
}
|
||||
|
||||
void SSC32::toServos ()
|
||||
{
|
||||
// puss to servos
|
||||
|
||||
// var. SSC32.
|
||||
// 0 | 3 // 16 | 0
|
||||
// 1 | 4 // | | |
|
||||
// 2 | 5 // 24 | 8
|
||||
|
||||
for (int i=0; i<9; i++)
|
||||
{
|
||||
Serial1.print(" #");
|
||||
Serial1.print(16+i);// 18 22
|
||||
Serial1.print(" P");
|
||||
Serial1.print(RAW_2_SERVO[9+i]);
|
||||
}
|
||||
|
||||
for (int i=0; i<9; i++)
|
||||
{
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(RAW_2_SERVO[i]);
|
||||
}
|
||||
|
||||
Serial1.print(" T");
|
||||
Serial1.println(walktime);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
// SSC32.h
|
||||
|
||||
#ifndef SSC32_H
|
||||
#define SSC32_H
|
||||
|
||||
#include "WProgram.h"
|
||||
#include "Data_Manager.h"
|
||||
|
||||
class SSC32
|
||||
{
|
||||
public:
|
||||
void setup();
|
||||
|
||||
void Render();
|
||||
void toServos ();
|
||||
void MoveHead (int angel_from_mid);
|
||||
|
||||
int ConverttoMS(int i, int low, int high);
|
||||
|
||||
// 0 | 3
|
||||
// 1 | 4
|
||||
// 2 | 5
|
||||
|
||||
int time;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,210 @@
|
||||
/*
|
||||
* Nunchuck -- Use a Wii Nunchuck
|
||||
* Tim Hirzel http://www.growdown.com
|
||||
*
|
||||
notes on Wii Nunchuck Behavior.
|
||||
This library provides an improved derivation of rotation angles from the nunchuck accelerometer data.
|
||||
The biggest different over existing libraries (that I know of ) is the full 360 degrees of Roll data
|
||||
from teh combination of the x and z axis accelerometer data using the math library atan2.
|
||||
|
||||
It is accurate with 360 degrees of roll (rotation around axis coming out of the c button, the front of the wii),
|
||||
and about 180 degrees of pitch (rotation about the axis coming out of the side of the wii). (read more below)
|
||||
|
||||
In terms of mapping the wii position to angles, its important to note that while the Nunchuck
|
||||
sense Pitch, and Roll, it does not sense Yaw, or the compass direction. This creates an important
|
||||
disparity where the nunchuck only works within one hemisphere. At a result, when the pitch values are
|
||||
less than about 10, and greater than about 170, the Roll data gets very unstable. essentially, the roll
|
||||
data flips over 180 degrees very quickly. To understand this property better, rotate the wii around the
|
||||
axis of the joystick. You see the sensor data stays constant (with noise). Because of this, it cant know
|
||||
the difference between arriving upside via 180 degree Roll, or 180 degree pitch. It just assumes its always
|
||||
180 roll.
|
||||
|
||||
|
||||
*
|
||||
* This file is an adaptation of the code by these authors:
|
||||
* Tod E. Kurt, http://todbot.com/blog/
|
||||
*
|
||||
* The Wii Nunchuck reading code is taken from Windmeadow Labs
|
||||
* http://www.windmeadow.com/node/42
|
||||
*/
|
||||
|
||||
#ifndef WiiChuck_h
|
||||
#define WiiChuck_h
|
||||
|
||||
#include "WProgram.h"
|
||||
#include <Wire.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
// these may need to be adjusted for each nunchuck for calibration
|
||||
#define ZEROX 510
|
||||
#define ZEROY 490
|
||||
#define ZEROZ 460
|
||||
#define RADIUS 180 // probably pretty universal
|
||||
|
||||
#define DEFAULT_ZERO_JOY_X 124
|
||||
#define DEFAULT_ZERO_JOY_Y 132
|
||||
|
||||
|
||||
|
||||
class WiiChuck {
|
||||
private:
|
||||
byte cnt;
|
||||
uint8_t status[6]; // array to store wiichuck output
|
||||
byte averageCounter;
|
||||
//int accelArray[3][AVERAGE_N]; // X,Y,Z
|
||||
int i;
|
||||
int total;
|
||||
uint8_t zeroJoyX; // these are about where mine are
|
||||
uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
|
||||
int lastJoyX;
|
||||
int lastJoyY;
|
||||
int angles[3];
|
||||
|
||||
boolean lastZ, lastC;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
byte joyX;
|
||||
byte joyY;
|
||||
boolean buttonZ;
|
||||
boolean buttonC;
|
||||
void begin()
|
||||
{
|
||||
Wire.begin();
|
||||
cnt = 0;
|
||||
averageCounter = 0;
|
||||
Wire.beginTransmission (0x52); // transmit to device 0x52
|
||||
Wire.send (0x40); // sends memory address
|
||||
Wire.send (0x00); // sends memory address
|
||||
Wire.endTransmission (); // stop transmitting
|
||||
update();
|
||||
for (i = 0; i<3;i++) {
|
||||
angles[i] = 0;
|
||||
}
|
||||
zeroJoyX = DEFAULT_ZERO_JOY_X;
|
||||
zeroJoyY = DEFAULT_ZERO_JOY_Y;
|
||||
}
|
||||
|
||||
|
||||
void calibrateJoy() {
|
||||
zeroJoyX = joyX;
|
||||
zeroJoyY = joyY;
|
||||
}
|
||||
|
||||
void update() {
|
||||
|
||||
Wire.requestFrom (0x52, 6); // request data from nunchuck
|
||||
while (Wire.available ()) {
|
||||
// receive byte as an integer
|
||||
status[cnt] = _nunchuk_decode_byte (Wire.receive()); //
|
||||
cnt++;
|
||||
}
|
||||
if (cnt > 5) {
|
||||
lastZ = buttonZ;
|
||||
lastC = buttonC;
|
||||
lastJoyX = readJoyX();
|
||||
lastJoyY = readJoyY();
|
||||
//averageCounter ++;
|
||||
//if (averageCounter >= AVERAGE_N)
|
||||
// averageCounter = 0;
|
||||
|
||||
cnt = 0;
|
||||
joyX = (status[0]);
|
||||
joyY = (status[1]);
|
||||
for (i = 0; i < 3; i++)
|
||||
//accelArray[i][averageCounter] = ((int)status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
|
||||
angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
|
||||
|
||||
//accelYArray[averageCounter] = ((int)status[3] << 2) + ((status[5] & B00110000) >> 4);
|
||||
//accelZArray[averageCounter] = ((int)status[4] << 2) + ((status[5] & B11000000) >> 6);
|
||||
|
||||
buttonZ = !( status[5] & B00000001);
|
||||
buttonC = !((status[5] & B00000010) >> 1);
|
||||
_send_zero(); // send the request for next bytes
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// UNCOMMENT FOR DEBUGGING
|
||||
//byte * getStatus() {
|
||||
// return status;
|
||||
//}
|
||||
|
||||
float readAccelX() {
|
||||
// total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
|
||||
return (float)angles[0] - ZEROX;
|
||||
}
|
||||
float readAccelY() {
|
||||
// total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
|
||||
return (float)angles[1] - ZEROY;
|
||||
}
|
||||
float readAccelZ() {
|
||||
// total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
|
||||
return (float)angles[2] - ZEROZ;
|
||||
}
|
||||
|
||||
boolean zPressed() {
|
||||
return (buttonZ && ! lastZ);
|
||||
}
|
||||
boolean cPressed() {
|
||||
return (buttonC && ! lastC);
|
||||
}
|
||||
|
||||
// for using the joystick like a directional button
|
||||
boolean rightJoy(int thresh=60) {
|
||||
return (readJoyX() > thresh and lastJoyX <= thresh);
|
||||
}
|
||||
|
||||
// for using the joystick like a directional button
|
||||
boolean leftJoy(int thresh=60) {
|
||||
return (readJoyX() < -thresh and lastJoyX >= -thresh);
|
||||
}
|
||||
|
||||
|
||||
int readJoyX() {
|
||||
return (int) joyX - zeroJoyX;
|
||||
}
|
||||
|
||||
int readJoyY() {
|
||||
return (int)joyY - zeroJoyY;
|
||||
}
|
||||
|
||||
|
||||
// R, the radius, generally hovers around 210 (at least it does with mine)
|
||||
// int R() {
|
||||
// return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());
|
||||
// }
|
||||
|
||||
|
||||
// returns roll degrees
|
||||
int readRoll() {
|
||||
return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
|
||||
}
|
||||
|
||||
// returns pitch in degrees
|
||||
int readPitch() {
|
||||
return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0); // optionally swap 'RADIUS' for 'R()'
|
||||
}
|
||||
|
||||
private:
|
||||
byte _nunchuk_decode_byte (byte x)
|
||||
{
|
||||
x = (x ^ 0x17) + 0x17;
|
||||
return x;
|
||||
}
|
||||
|
||||
void _send_zero()
|
||||
{
|
||||
Wire.beginTransmission (0x52); // transmit to device 0x52
|
||||
Wire.send (0x00); // sends one byte
|
||||
Wire.endTransmission (); // stop transmitting
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
// atan2.cpp
|
||||
|
||||
#include "atan2.h"
|
||||
|
||||
void joy::JoyToDegAndDis (int x, int y)
|
||||
{
|
||||
deg = !(90 + degrees(atan2(y, x)));
|
||||
distance = sqrt(y^2 + x^2);
|
||||
}
|
||||
|
||||
int joy::JoyToDeg (int x, int y)
|
||||
{ // watchout 90 + does not work good
|
||||
return !(degrees(atan2(y, x)));
|
||||
}
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
// atan2.h
|
||||
|
||||
#ifndef joy_H
|
||||
#define joy_H
|
||||
|
||||
#include "WProgram.h"
|
||||
|
||||
class joy
|
||||
{
|
||||
public:
|
||||
void JoyToDegAndDis (int x, int y);
|
||||
int JoyToDeg (int x, int y);
|
||||
|
||||
int deg, distance;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,196 @@
|
||||
// Hexapod FireFly
|
||||
|
||||
#include "Header.h"
|
||||
#include "Wire.h" // Just dont work's, if you put it in the Header file.
|
||||
|
||||
// Setup.
|
||||
void setup()
|
||||
{
|
||||
chuck.begin();
|
||||
chuck.update();
|
||||
smin[0] = 600;
|
||||
smin[1] = 960;
|
||||
smin[2] = 600;
|
||||
smax[0] = 2400;
|
||||
smax[1] = 2100;
|
||||
smax[2] = 2400;
|
||||
l1 = 10;
|
||||
l2 = 15;
|
||||
pinMode(13, OUTPUT); // Led L
|
||||
Serial.begin(38400);
|
||||
body.setup();
|
||||
FireFly();
|
||||
}
|
||||
|
||||
// Position.
|
||||
int Position=1;
|
||||
int PosMax=15;
|
||||
int BeginPos=0;
|
||||
int Directory=0;
|
||||
|
||||
void UpdatePosition(int i=0)
|
||||
{
|
||||
if (i == 0)
|
||||
{
|
||||
if ((Position < PosMax) && (Directory == 0))
|
||||
Position++;
|
||||
else if (Directory == 0)
|
||||
Directory=1;
|
||||
else if ((Position > 1) && (Directory == 1))
|
||||
Position--;
|
||||
else
|
||||
Directory=0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Position < PosMax)
|
||||
Position++;
|
||||
else
|
||||
Position = BeginPos;
|
||||
|
||||
if (Directory == 1)
|
||||
{
|
||||
Directory = 2;
|
||||
Position = BeginPos;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*==============================================/
|
||||
|
||||
Go Use Class::operator=, To give Data From IK Class To Rotation or SSC32 Class.
|
||||
|
||||
/==============================================*/
|
||||
|
||||
// Variables.
|
||||
double past = millis();
|
||||
int Watchdog=0;
|
||||
|
||||
// Main loop.
|
||||
void loop()
|
||||
{
|
||||
if (millis() < past) // to handle te exception of 50 day overflow of millis()
|
||||
past = millis();
|
||||
|
||||
while ( ((millis() - past) < walktime) || (Watchdog == 0))
|
||||
{
|
||||
/* chuck.update();
|
||||
ojx = chuck.readJoyX();
|
||||
ojy = chuck.readJoyY();
|
||||
oax = chuck.readAccelX();
|
||||
oay = chuck.readAccelY();
|
||||
|
||||
// Mode Head Servo.
|
||||
body.MoveHead(chuck.readRoll());*/
|
||||
|
||||
if (Watchdog == 0)
|
||||
{
|
||||
walktime = 300;
|
||||
|
||||
if (Directory==0)
|
||||
{
|
||||
UpdatePosition();
|
||||
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
COR_2_IK[0+(i*3)] = 10; //x
|
||||
COR_2_IK[1+(i*3)] = Position; //y
|
||||
COR_2_IK[2+(i*3)] = 0; //z
|
||||
}
|
||||
COR_2_IK[2+(0*3)] = -10;// x side body
|
||||
COR_2_IK[2+(2*3)] = 10;///
|
||||
COR_2_IK[2+(3*3)] = -10;//
|
||||
COR_2_IK[2+(5*3)] = 10;///
|
||||
COR_2_IK[0+(1*3)] = 10;// z side body
|
||||
COR_2_IK[0+(4*3)] = 10;
|
||||
}
|
||||
else
|
||||
{
|
||||
PosMax = 7;
|
||||
UpdatePosition(1);
|
||||
for (int i2=0; i2<18; i2++)
|
||||
{
|
||||
COR_2_IK[i2] = TriPoD[Position][i2];
|
||||
COR_2_IK[i2] = TriPoD[Position][i2];
|
||||
}
|
||||
for(int i=0; i<6; i++)
|
||||
{
|
||||
// zij, laag, achter
|
||||
// 10, 15, -10, 15, 15, 0, 10, 15, 10, 10, 15, -10, 15, 15, 0, 10, 15, 10;
|
||||
int cor[6] = { -5, 0, 5, -5, 0, 5 };
|
||||
COR_2_IK[2+(i*3)] += cor[i];
|
||||
}
|
||||
}
|
||||
|
||||
/* orbit.getAngle(90 + (chuck.readRoll()/3), 90);// 90 + (chuck.readPitch()/3));
|
||||
orbit.getData(x, y, z);
|
||||
orbit.calculate();*/
|
||||
|
||||
// x, y, z
|
||||
// x, z is above the leg, z is depth
|
||||
// y, l3 is side leg, y is height
|
||||
ik.setlegs();
|
||||
body.Render();
|
||||
|
||||
Serial.println("Done Rendering");
|
||||
/*if (ik.CheckForErrorCor() != 0)
|
||||
{
|
||||
warning();
|
||||
Serial.println("Coordinate Error. ");
|
||||
Serial.println(ik.CheckForErrorCor_print());
|
||||
}
|
||||
else*/
|
||||
Watchdog = 1;
|
||||
}
|
||||
}
|
||||
|
||||
past = millis(); // gifs an overflow after 50 day run.
|
||||
Serial.println("Out of loop");
|
||||
|
||||
if (Watchdog == 1)
|
||||
{
|
||||
if (ik.CheckForErrorAng() == 0) // check if there is no error.
|
||||
{
|
||||
Serial.println("Sending");
|
||||
body.toServos();
|
||||
}
|
||||
else
|
||||
{
|
||||
warning();
|
||||
Serial.println("Axis Error.");
|
||||
}
|
||||
Watchdog = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
// sequnces
|
||||
|
||||
// Hexapod seqencer.
|
||||
int TriPoD_Size = 8;
|
||||
|
||||
int TriPoD[8][18] = {
|
||||
{10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0},
|
||||
{10,10,-2,10,15, 2,10,10,-2,10,15, 3,10,10,-2,10,15, 2},
|
||||
{10,15,-2,10,15, 2,10,15,-2,10,15, 3,10,15,-2,10,15, 2},
|
||||
{10,15,-2,10,10, 2,10,15,-2,10,10, 3,10,15,-2,10,10, 2},
|
||||
{10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0},
|
||||
{10,15, 2,10,10,-2,10,15, 2,10,10,-2,10,15, 2,10,10,-2},
|
||||
{10,15, 2,10,15,-2,10,15, 2,10,15,-2,10,15, 2,10,15,-2},
|
||||
{10,10, 2,10,15,-2,10,10, 2,10,15,-2,10,10, 2,10,15,-2}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
//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;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,112 @@
|
||||
// Servos.cpp
|
||||
|
||||
#include "Servos.h"
|
||||
|
||||
void Servos::MoveHead (Data *DataLine)
|
||||
{
|
||||
#ifdef DEBUGHS
|
||||
Serial.print("### Servos->Head Time/Position: ");
|
||||
Serial.print(DataLine->HeadTime);
|
||||
Serial.print(", ");
|
||||
Serial.println(DataLine->HeadPosition);
|
||||
#endif
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(9);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->HeadPosition);
|
||||
Serial1.print(" T");
|
||||
Serial1.println(DataLine->HeadTime);
|
||||
}
|
||||
|
||||
void mSend(Data *DataLine)
|
||||
{
|
||||
int s=3;
|
||||
|
||||
for (int i=0; i<9; i += 3){
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t1);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t2);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t3);
|
||||
s++;
|
||||
}
|
||||
|
||||
s = 0;
|
||||
|
||||
for (int i=16; i<25; i += 3){
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t1);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t2);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t3);
|
||||
s++;
|
||||
}
|
||||
|
||||
Serial1.print(" T");
|
||||
Serial1.println(DataLine->WalkTime);
|
||||
|
||||
#ifdef DEBUGC
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
{
|
||||
Serial.print("### Converter->ServosAnglesTime Are: ");
|
||||
}
|
||||
|
||||
Serial.print(DataLine->ServoData[i].t1);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].t2);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].t3);
|
||||
}
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
void Servos::Send(Data *DataLine)
|
||||
{
|
||||
if (DataLine->Send)
|
||||
{
|
||||
#ifdef DEBUGS
|
||||
Serial.println("### Servos->Servos ###");
|
||||
Serial.println("### Servos Send ###");
|
||||
digitalWrite(13, HIGH);
|
||||
delay(10);
|
||||
digitalWrite(13, LOW);
|
||||
#endif
|
||||
|
||||
#ifndef Freeze
|
||||
mSend(DataLine);
|
||||
#endif
|
||||
|
||||
DataLine->Send = false;
|
||||
DataLine->ReadyToSend = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
// Servos.h
|
||||
|
||||
#ifndef SERVOS_H
|
||||
#define SERVOS_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Data.h"
|
||||
|
||||
class Servos
|
||||
{
|
||||
public:
|
||||
void Send(Data *DataLine);
|
||||
void MoveHead (Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,55 @@
|
||||
//Ultrasone.cpp
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,42 @@
|
||||
//Ultrasone.h
|
||||
|
||||
#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,39 @@
|
||||
// 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,24 @@
|
||||
// Watchdog.h
|
||||
|
||||
#ifndef WATCHDOG_H
|
||||
#define WATCHDOG_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Data.h"
|
||||
|
||||
class Watchdog
|
||||
{
|
||||
public:
|
||||
void CheckTime(Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,212 @@
|
||||
// Hexapod FireFly
|
||||
|
||||
#include "hexapod.h"
|
||||
#include "Data.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;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial1.begin(38400);
|
||||
pinMode(13, OUTPUT);
|
||||
DataLine->WalkTime = 100;
|
||||
DataLine->FireFly.FireFly();
|
||||
}
|
||||
|
||||
int a=0;
|
||||
int b=6;
|
||||
int tt;
|
||||
byte Sit;
|
||||
byte SitC;
|
||||
|
||||
// Main loop.
|
||||
void loop()
|
||||
{
|
||||
int d = DataLine->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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in new issue
Block a user