From fe472bfce9612d970e55d688ddf7a6f9eafc3abf Mon Sep 17 00:00:00 2001 From: MangaValk Date: Sun, 9 Oct 2011 15:56:03 +0200 Subject: [PATCH] FireFly 2011: Arduino Mega + SSC-32, Wii Nunchuk, preset walking sequences From Electronics\Hexapod\Arduino sketches 2011-2012\hexapod_oud (banner 'Version 3.0'). --- .../2011-nunchuk/hexapod_oud/Data_Manager.cpp | 28 +++ .../2011-nunchuk/hexapod_oud/Data_Manager.h | 20 ++ .../2011-nunchuk/hexapod_oud/Header.h | 50 +++++ .../hexapod_oud/InverseKinemetics.cpp | 153 +++++++++++++ .../hexapod_oud/InverseKinemetics.h | 61 +++++ .../2011-nunchuk/hexapod_oud/Orbit.cpp | 91 ++++++++ .../firmware/2011-nunchuk/hexapod_oud/Orbit.h | 30 +++ .../2011-nunchuk/hexapod_oud/Rotation.cpp | 23 ++ .../2011-nunchuk/hexapod_oud/Rotation.h | 24 ++ .../2011-nunchuk/hexapod_oud/SSC32.cpp | 79 +++++++ .../firmware/2011-nunchuk/hexapod_oud/SSC32.h | 34 +++ .../2011-nunchuk/hexapod_oud/WiiChuck.h | 210 ++++++++++++++++++ .../2011-nunchuk/hexapod_oud/atan2.cpp | 15 ++ .../firmware/2011-nunchuk/hexapod_oud/atan2.h | 26 +++ .../2011-nunchuk/hexapod_oud/hexapod_oud.pde | 196 ++++++++++++++++ .../2011-nunchuk/hexapod_oud/sequnces.h | 39 ++++ 16 files changed, 1079 insertions(+) create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/WiiChuck.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.cpp create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.h create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde create mode 100644 2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.cpp new file mode 100644 index 0000000..13cd069 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.cpp @@ -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; diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.h new file mode 100644 index 0000000..b8ca939 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.h @@ -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 + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h new file mode 100644 index 0000000..1b735e0 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h @@ -0,0 +1,50 @@ +// Include. +#include +#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); +} + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp new file mode 100644 index 0000000..372da5f --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp @@ -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; +} + + + + + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.h new file mode 100644 index 0000000..92216e6 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.h @@ -0,0 +1,61 @@ +#ifndef InverseKinemetics_H +#define InverseKinemetics_H + +#include "WProgram.h" +#include +#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 + + + + + + + + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.cpp new file mode 100644 index 0000000..9c8f4aa --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.cpp @@ -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]; + }*/ +} + + + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.h new file mode 100644 index 0000000..55796c1 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.h @@ -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 + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.cpp new file mode 100644 index 0000000..b6ff0d4 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.cpp @@ -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)); +} + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.h new file mode 100644 index 0000000..af8fffd --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.h @@ -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 + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.cpp new file mode 100644 index 0000000..3a9a1cf --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.cpp @@ -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); +} + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.h new file mode 100644 index 0000000..2351e3e --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.h @@ -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 + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/WiiChuck.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/WiiChuck.h new file mode 100644 index 0000000..86b5cc5 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/WiiChuck.h @@ -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 +#include + + +// 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 + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.cpp new file mode 100644 index 0000000..2e3573c --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.cpp @@ -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))); +} + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.h new file mode 100644 index 0000000..c69e52a --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.h @@ -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 + + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde new file mode 100644 index 0000000..f584639 --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde @@ -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; + } +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h new file mode 100644 index 0000000..84d253f --- /dev/null +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h @@ -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 +}; + + + + + + + +