FireFly 2011: Arduino Mega + SSC-32, Wii Nunchuk, preset walking sequences
From Electronics\Hexapod\Arduino sketches 2011-2012\hexapod_oud (banner 'Version 3.0').
This commit is contained in:
mangavalk committed
1 parent
160b6ec36a
commit
fe472bfce9
16 files changed
+1079
No files matched your search
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,153 @@
|
||||
//#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);
|
||||
float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
|
||||
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)] = 15;// z side body
|
||||
COR_2_IK[0+(4*3)] = 15;
|
||||
}
|
||||
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,39 @@
|
||||
// sequnces
|
||||
|
||||
// Hexapod seqencer.
|
||||
int TriPoD_Size = 8;
|
||||
// zij, laag, achter
|
||||
// 15, 15, -15, 15, 15, 0, 15, 15, 15, 15, 15, -15, 15, 15, 0, 15, 15, 15;
|
||||
int TriPoD[8][18] = {
|
||||
{// -15 is voor
|
||||
15, 2, 0, 15, 15, 0, 15, 2, 0, 15, 15, 0,15, 2, 0, 15, 15, 0 }//miden*hoog, miden*laag
|
||||
,
|
||||
{
|
||||
15, 2, -2, 15, 15, 2, 15, 2, -2, 15, 15, 2,15, 2, -2, 15, 15, 2 }// voor*hoog, achter*laag
|
||||
,
|
||||
{
|
||||
15, 15, -2, 15, 15, 2, 15, 15, -2, 15, 15, 2,15, 15, -2, 15, 15, 2 }// laag, niks
|
||||
,
|
||||
{
|
||||
15, 15, -2, 15, 2, 2, 15, 15, -2, 15, 2, 2,15, 15, -2, 15, 2, 2 }// niks, hoog
|
||||
,
|
||||
{
|
||||
15, 15, 0, 15, 2, 0, 15, 15, 0, 15, 2, 0,15, 15, 0, 15, 2, 0 }//midden*laag, miden*hoog
|
||||
,
|
||||
{
|
||||
15, 15, 2, 15, 2, -2, 15, 15, 2, 15, 2, -2,15, 15, 2, 15, 2, -2 }//voor-laag, achter/hoog
|
||||
,
|
||||
{
|
||||
15, 15, 2, 15, 15, -2, 15, 15, 2, 15, 15, -2,15, 15, 2, 15, 15, -2 }// niks, laag
|
||||
,
|
||||
{
|
||||
15, 2, 2, 15, 15, -2, 15, 2, 2, 15, 15, -2,15, 2, 2, 15, 15, -2 }// hoog, niks
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in new issue
Block a user