Compare commits
4
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 mangavalk
|
0447cb8c7a | ||
|
mangavalk
|
00bf3187fe | ||
|
mangavalk
|
c977b3acab | ||
|
mangavalk
|
78c5c11d41 |
No files matched your search
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large.
Load diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,5 +0,0 @@
|
||||
[{000214A0-0000-0000-C000-000000000046}]
|
||||
Prop3=19,11
|
||||
[InternetShortcut]
|
||||
URL=https://www.siggraph.org/education/materials/HyperGraph/modeling/mod_tran/3drota.htm
|
||||
IDList=
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
LFS
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,28 +0,0 @@
|
||||
#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;
|
||||
@@ -1,20 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
@@ -1,50 +0,0 @@
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,157 +0,0 @@
|
||||
//#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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
#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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
// 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];
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,23 +0,0 @@
|
||||
// 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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,24 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,79 +0,0 @@
|
||||
#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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,34 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,210 +0,0 @@
|
||||
/*
|
||||
* 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
|
||||
|
||||
@@ -1,15 +0,0 @@
|
||||
// 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)));
|
||||
}
|
||||
|
||||
@@ -1,26 +0,0 @@
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,196 +0,0 @@
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
// 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}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
// 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,55 +0,0 @@
|
||||
//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,60 +0,0 @@
|
||||
//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 );
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,39 +0,0 @@
|
||||
//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
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
// Watchdog.h
|
||||
// V3.0
|
||||
|
||||
#ifndef WATCHDOG_H
|
||||
#define WATCHDOG_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Data.h"
|
||||
|
||||
class Watchdog
|
||||
{
|
||||
public:
|
||||
void CheckTime(Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -1,62 +0,0 @@
|
||||
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.
|
||||
*/
|
||||
};
|
||||
|
||||
};
|
||||
-259
@@ -1,259 +0,0 @@
|
||||
/*
|
||||
GLCD Library - Hello World
|
||||
|
||||
This sketch prints "Hello World!" to the LCD
|
||||
and shows the time in seconds since reset.
|
||||
|
||||
The circuit:
|
||||
See the inlcuded documentation in glcd/doc directory for how to wire
|
||||
up the glcd module. glcd/doc/GLCDref.htm can be viewed in your browser
|
||||
by clicking on the file.
|
||||
|
||||
*/
|
||||
|
||||
// include the library header
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.h"
|
||||
#include "glcd.h"
|
||||
//#include "math.h"
|
||||
//#include "Wire.h"
|
||||
//#include "WiiChuck.h"
|
||||
#include "bitmaps/allBitmaps.h"
|
||||
//#include "IRremote.h"
|
||||
|
||||
#define MAXANGLE 90
|
||||
#define MINANGLE -90
|
||||
|
||||
// include the Fonts
|
||||
#include "fonts/allFonts.h"
|
||||
|
||||
WiiChuck chuck = WiiChuck();
|
||||
int angleStart, currentAngle;
|
||||
int tillerStart = 0;
|
||||
double angle;
|
||||
int RECV_PIN = 11;
|
||||
int m, mt; //mode
|
||||
long int oud, oud2, oudt, t; //ouden ontvangen code
|
||||
String protocol;
|
||||
|
||||
IRrecv irrecv(RECV_PIN);
|
||||
|
||||
decode_results results;
|
||||
|
||||
void setup() {
|
||||
// Initialize the GLCD
|
||||
GLCD.Init();
|
||||
|
||||
pinMode(13, OUTPUT);
|
||||
digitalWrite(13, LOW);
|
||||
|
||||
// Select the font for the default text area
|
||||
GLCD.SelectFont(SystemFont5x7);
|
||||
|
||||
Serial.begin(9600);
|
||||
irrecv.enableIRIn(); // Start the IR receiver
|
||||
|
||||
chuck.begin();
|
||||
while (!chuck.plugd)
|
||||
chuck.CheckifPlugd();
|
||||
|
||||
//GLCD.DrawBitmap(Earth, 0, 0, WHITE);
|
||||
|
||||
GLCD.CursorTo(12, 1);
|
||||
GLCD.print("Nederland");
|
||||
delay(3500);
|
||||
}
|
||||
|
||||
long int getIR(decode_results *results) {
|
||||
return results->value;
|
||||
}
|
||||
|
||||
void dump(decode_results *results) {
|
||||
int count = results->rawlen;
|
||||
GLCD.CursorTo(0, 1);
|
||||
if (results->decode_type == UNKNOWN) {
|
||||
protocol = "Unknown encoding: ";
|
||||
}
|
||||
else if (results->decode_type == NEC) {
|
||||
protocol = "Decoded NEC: ";
|
||||
}
|
||||
else if (results->decode_type == SONY) {
|
||||
protocol = "Decoded SONY: ";
|
||||
}
|
||||
else if (results->decode_type == RC5) {
|
||||
protocol = "Decoded RC5: ";
|
||||
}
|
||||
else if (results->decode_type == RC6) {
|
||||
protocol = "Decoded RC6: ";
|
||||
}
|
||||
GLCD.print(protocol);
|
||||
GLCD.CursorTo(0, 2);
|
||||
GLCD.print(oud2, HEX);
|
||||
GLCD.CursorTo(0, 3);
|
||||
GLCD.print(oud, HEX);
|
||||
GLCD.CursorTo(0, 4);
|
||||
long int t = results->value;
|
||||
oudt = t;
|
||||
}
|
||||
|
||||
#define s1 50
|
||||
#define s2 50
|
||||
int stop = 0;
|
||||
int stopstate = 0;
|
||||
|
||||
void loop()
|
||||
{
|
||||
if (irrecv.decode(&results)){
|
||||
long int t2 = getIR(&results);
|
||||
t = t2;
|
||||
}
|
||||
|
||||
if (stop)
|
||||
{
|
||||
if ((t == 0x1045B) & (stopstate != 2)) {
|
||||
mt = 0;
|
||||
if (m == 0)
|
||||
m = 1;
|
||||
else
|
||||
m = 0;
|
||||
stopstate = 2;
|
||||
}
|
||||
else if (( t == 0x45B) & (stopstate != 3)) {
|
||||
mt = 0;
|
||||
if (m == 0)
|
||||
m = 1;
|
||||
else
|
||||
m = 0;
|
||||
stopstate = 3;
|
||||
}
|
||||
else if ((t == 0x1042C ) || ( t == 0x42C)) {
|
||||
stop = false;
|
||||
}
|
||||
}
|
||||
else if ((t == 0x431 ) || ( t == 0x10431)) {
|
||||
digitalWrite(13, HIGH);
|
||||
stop = true;
|
||||
}
|
||||
else if ((t == 0x40F ) || ( t == 0x1040F)) {
|
||||
digitalWrite(13, HIGH);
|
||||
irrecv.resume(); // Receive the next value
|
||||
m = 60;
|
||||
}
|
||||
else if ((t == 0x1040C ) || ( t == 0x40C)) {
|
||||
//power
|
||||
GLCD.ClearScreen();
|
||||
//GLCD.DrawBitmap(Power, 64, 0, BLACK);
|
||||
|
||||
while (1){
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
mt++;
|
||||
digitalWrite(13, LOW);
|
||||
}
|
||||
|
||||
switch (mt){
|
||||
case 1:
|
||||
m = 0;
|
||||
break;
|
||||
case s1:
|
||||
m = 1;
|
||||
break;
|
||||
case s1+s2:
|
||||
mt = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
if (m == 0)
|
||||
{
|
||||
GLCD.ClearScreen();
|
||||
GLCD.CursorTo(0, 0);
|
||||
GLCD.print("IR Receiver");
|
||||
//GLCD.DrawBitmap(Ir, 116, 0, BLACK);
|
||||
if (irrecv.decode(&results)) {
|
||||
dump(&results);
|
||||
delay(100);
|
||||
irrecv.resume(); // Receive the next value
|
||||
oud2 = oud;
|
||||
}
|
||||
else
|
||||
{
|
||||
oud = oudt;
|
||||
GLCD.CursorTo(0, 1);
|
||||
GLCD.print(protocol);
|
||||
GLCD.CursorTo(0, 2);
|
||||
GLCD.print(oud2, HEX);
|
||||
GLCD.CursorTo(0, 3);
|
||||
GLCD.print(oud, HEX);
|
||||
GLCD.CursorTo(0, 4);
|
||||
GLCD.print("No DATA Input");
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
GLCD.ClearScreen();
|
||||
|
||||
chuck.update();
|
||||
if (!chuck.plugd)
|
||||
chuck.CheckifPlugd();
|
||||
else {
|
||||
|
||||
GLCD.CursorTo(0, 0);
|
||||
|
||||
GLCD.print("MangaValk");
|
||||
//GLCD.DrawBitmap(Nun, 106, 0, BLACK);
|
||||
|
||||
GLCD.CursorTo(0, 2);
|
||||
|
||||
GLCD.print("C: ");
|
||||
GLCD.print(chuck.buttonC);
|
||||
GLCD.print(" Z: ");
|
||||
GLCD.print(chuck.buttonZ);
|
||||
|
||||
GLCD.CursorTo(0, 3);
|
||||
GLCD.print("Roll: ");
|
||||
GLCD.print(chuck.readRoll());
|
||||
GLCD.CursorTo(0, 4);
|
||||
GLCD.print("Pitch: ");
|
||||
GLCD.print(chuck.readPitch()-75);
|
||||
GLCD.CursorTo(0, 5);
|
||||
GLCD.print("Jy: ");
|
||||
GLCD.print(chuck.readJoyY());
|
||||
GLCD.CursorTo(0, 6);
|
||||
GLCD.print("Jx: ");
|
||||
GLCD.print(chuck.readJoyX());
|
||||
GLCD.CursorTo(0, 7);
|
||||
GLCD.print("Atan2: ");
|
||||
GLCD.print(atan2(chuck.readJoyX(), chuck.readJoyY()));
|
||||
}
|
||||
|
||||
irrecv.resume(); // Receive the next value
|
||||
}
|
||||
delay(100);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,70 +0,0 @@
|
||||
// Converter.cpp
|
||||
// V3.0
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,27 +0,0 @@
|
||||
// Converter.h
|
||||
// V3.0
|
||||
|
||||
#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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,63 +0,0 @@
|
||||
//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
|
||||
|
||||
|
||||
|
||||
Loaded 100 of 539 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user