2019: add the May 2019 printed robot parts (STL)

Fusion 360 STL exports of the body, legs, hinges, foot, servo horn and
test legs into robot-parts/, and an early remote part into remote-case/.
Found in an old Hexapod folder; the remote files that came with them were
already in the repo.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

.gitattributes

@@ -1,3 +1,5 @@
+# Store every file exactly as it is: no line-ending conversion, so old code stays byte-for-byte original.
+* -text
 *.wmv filter=lfs diff=lfs merge=lfs -text
 *.WMV filter=lfs diff=lfs merge=lfs -text
 *.mp4 filter=lfs diff=lfs merge=lfs -text

2009-fpga/README.md

@@ -0,0 +1,10 @@
+# 2009–2011: the FPGA version
+
+The first hexapod, driven by an FPGA. Its frame, `original-frame/`, was the mechanical base of every
+version up to 2023. The FPGA code has not survived; what is left are the photos and videos in `media/`,
+among them `hexapod film 18-11-09.wmv`.
+
+- `original-frame/` – the leg, body and bracket designs (DXF/DWG, 2009–2011).
+- `media/` – photos and videos of the FPGA version, sorted by the owner; those of the FireFly version are in
+  `../2011-firefly-mega-ssc32/media/`. (Most file dates are copy dates. `DSCF*` photos carry a camera date
+  of 2003.)

2009-fpga/original-frame/README.md

@@ -0,0 +1,12 @@
+# Original frame (2009–2023)
+
+The mechanical design every version up to 2023 was built on. The 2026 robot has a new design, see
+`../../2026-current/hardware/fusion360/`.
+
+| File | Date | What |
+|---|---|---|
+| `poot.dxf` | 2009-10 | leg ("poot") |
+| `hexapod been.dxf` | 2009-12 | leg ("been") |
+| `poot2.dwg` | 2011-04 | leg, second version |
+| `body.dxf`, `Body.jpg` | 2011-07 | body plate, for the Arduino Mega + SSC-32 rebuild |
+| `bracket.dxf` | 2011-07 | servo bracket |

2011-firefly-mega-ssc32/README.md

@@ -0,0 +1,28 @@
+# 2011–2012: FireFly (Arduino Mega + SSC-32)
+
+In June 2011 the robot was rebuilt around an Arduino Mega 2560 and a Lynxmotion SSC-32 servo controller,
+with a new body and bracket (`../2009-fpga/original-frame/`). The Mega does the inverse kinematics and sends
+`#n Pxxxx Txxx` commands to the SSC-32 over `Serial1` at 38400 baud. The serial banner calls it "FireFly".
+
+## firmware/
+
+Each folder is an Arduino sketch (or library); successive versions are successive commits, so
+`git log -p -- <folder>` shows what changed.
+
+| Folder | Dates | What |
+|---|---|---|
+| `2011-nunchuk/hexapod_oud/` | 2011-10, 2011-12 | Wii Nunchuk input, fixed walking sequences, IK, SSC-32 output. Second commit = `hexapod V2 29-12-11.rar` |
+| `2012-glcd-remote/hexapod_remote_glcd/` | 2012-03 | handheld remote: graphic LCD, IR receiver and Wii Nunchuk, using the hexapod library |
+| `2012-ir-radar-gps/hexapodV3/` | 2012-04, 2012-05 | v3 restructure (Data, Converter, IK, Servos classes), RC6 IR remote, ultrasonic sensor on a swivelling head servo, watchdog. `Oud/` is the 2011 code it started from |
+| `2012-library/hexapod/` | 2012-07, ≤2014-02 | the robot code as an Arduino library "hexapod" v3.1: IK, SSC-32, IR, ultrasonic, GPS (TinyGPS on `Serial2`), 7-segment display. Second commit = the variant kept in the 2014 GitLab archive |
+| `2012-behaviours-wip/FireFly/` | 2012-07 | start of a behaviour-based rewrite on top of the library; unfinished, does not compile |
+
+## Other folders
+
+- `hardware/` – the Arduino Mega 2560 (SparkFun DEV-09949) and SSC-32 photos of June 2011, `Layout Hexapod.jpg`.
+- `docs/math/` – the IK and body-rotation derivations (`[1]`–`[5]`, `Hexapod draaien Y as.png`, IK.pdf as `Movement Space Leg.pdf`). Older versions (`wisFORlive*.docx`) are in the history.
+- `docs/planning/` – workflow and to-do documents (2012).
+- `docs/research/` – papers on hexapod workspace and Q-learning, GPS module and TinyGPS manuals.
+- `docs/concepts/` – inspiration images and early sketches (2008–2012).
+- `tools/` – two small Windows programs from November 2011 (`hexapod 1.0.exe`, `hexapod Source 0.2.exe`); what they do is not known.
+- `media/` – photos and videos of the FireFly version, sorted by the owner, incl. `Hexapod 29-11-11.wmv`.

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;

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
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h

@@ -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); 
+}
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp

@@ -0,0 +1,157 @@
+//#include "WProgram.h"
+#include "InverseKinemetics.h"
+
+void InverseKinemetics::RL3 (int i)
+{
+  l3 = sqrt(pow(abs(COR_2_IK[0+(i*3)]), 2) + pow(abs(COR_2_IK[2+(i*3)]), 2));
+}
+
+float InverseKinemetics::ZX (int i)
+{
+  RL3(i);
+  if (COR_2_IK[2+(i*3)] != 0)
+    float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
+  else
+    float temp = 90; // Math Error Protection.
+  return temp;
+}
+
+float InverseKinemetics::TT (int i)
+{
+  int x = pow(abs(COR_2_IK[0+(i*3)]), 2);
+  int y = pow(abs(COR_2_IK[1+(i*3)]), 2);
+  int z = pow(abs(COR_2_IK[2+(i*3)]), 2);  
+  s = sqrt(x + y + z);
+
+  if (s > (l1 + l2) )
+    R[i] = 3;
+
+  float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2))
+    / (2 * s * COR_2_IK[1+(i*3)])));
+
+  float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2))
+    / (2 * s * l1)));
+
+  float temp = (int)t1 + t2;
+  //Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp);
+
+  return temp;
+}
+
+int InverseKinemetics::LL ()
+{
+  int temp = degrees(acos((float)(pow(l1, 2) + pow(l2, 2) - pow(s, 2)) / (2 * l1 * l2)));
+  return temp;
+}
+
+void InverseKinemetics::setleg (int i)
+{
+  Serial.print("Set Leg "); 
+  Serial.println(i);
+  if (i >= 3)
+  {
+    //Error_Coordinate(i);
+    DEG_2_CON[0+(i*3)] = 180 - ZX(i);
+    DEG_2_CON[1+(i*3)] = 180 - TT(i);
+    DEG_2_CON[2+(i*3)] = LL();
+    Error_Angle(i);  
+  }
+  else
+  {
+    //Error_Coordinate(i);
+    DEG_2_CON[0+(i*3)] = ZX(i);
+    DEG_2_CON[1+(i*3)] = TT(i);
+    DEG_2_CON[2+(i*3)] = 180 - LL();
+    Error_Angle(i);
+  }
+}
+
+void InverseKinemetics::setlegs ()
+{
+  for (int i=0; i<6; i++)
+  {
+    setleg(i);
+  }
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("Cor: ");
+    Serial.print(COR_2_IK[0+(i*3)]);
+    Serial.print(", ");
+    Serial.print(COR_2_IK[1+(i*3)]);
+    Serial.print(", ");
+    Serial.print(COR_2_IK[2+(i*3)]);
+  }
+  Serial.println(";");
+}
+
+void InverseKinemetics::Error_Angle (int i)
+{  
+  for (int i2=0; i2<3; i2++)
+    R[i2+(i*3)] = 0;
+  if (DEG_2_CON[0+(i*3)] < 0) 
+    R[0+(i*3)] = 1;
+  if (DEG_2_CON[0+(i*3)] > 180) 
+    R[0+(i*3)] = 2;
+  if (DEG_2_CON[1+(i*3)] < 0) 
+    R[1+(i*3)] = 1; 
+  if (DEG_2_CON[1+(i*3)] > 180)
+    R[1+(i*3)] = 2;
+  if (DEG_2_CON[2+(i*3)] < 0)
+    R[2+(i*3)] = 1;
+  if (DEG_2_CON[2+(i*3)] > 180)
+    R[2+(i*3)] = 2;
+}
+
+void InverseKinemetics::Error_Coordinate (int i)
+{  
+  Ra[i] = 0;
+  float temp = sqrt(pow(COR_2_IK[0+(i*3)], 2) + pow(COR_2_IK[1+(i*3)], 2) + pow(COR_2_IK[2+(i*3)], 2));
+  if (temp <= 5) {
+    Ra[i] = 1;
+  }    
+  if (temp > l1 + l2) {
+    Ra[i] = 2;
+  }
+}
+
+int InverseKinemetics::CheckForErrorCor()
+{
+  for (int i=1;  i<19; i++)
+    if (R[i-1]!=0)
+      return i;
+  return 0;
+}
+
+String InverseKinemetics::CheckForErrorCor_print()
+{
+  String temp = "Error Coordinate: ";
+  for (int i=0; i<18; i++)
+  {
+    temp = temp + (int)DEG_2_CON[i] + ", ";
+  }
+  temp = temp + "\nCheckForErrorCor returnd: " + CheckForErrorCor();
+  return temp;
+}
+
+int InverseKinemetics::CheckForErrorAng()
+{
+  for (int i=0;  i<6; i++)
+    if (Ra[i]!=0)
+      return i;
+  return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+

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 <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
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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];
+   }*/
+}
+
+
+
+
+
+
+
+
+

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
+
+
+
+
+
+
+

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));
+}
+
+
+

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
+
+
+
+
+
+
+

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);
+}
+
+
+

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
+
+
+
+
+
+

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 <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
+

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)));
+}
+

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
+
+
+
+
+
+
+
+

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)] = 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;
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h

@@ -0,0 +1,19 @@
+// sequnces
+
+// Hexapod seqencer.
+int TriPoD_Size = 8;
+
+int TriPoD[8][18] = { 
+ {10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0},
+ {10,10,-2,10,15, 2,10,10,-2,10,15, 3,10,10,-2,10,15, 2},
+ {10,15,-2,10,15, 2,10,15,-2,10,15, 3,10,15,-2,10,15, 2},
+ {10,15,-2,10,10, 2,10,15,-2,10,10, 3,10,15,-2,10,10, 2},
+ {10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0},
+ {10,15, 2,10,10,-2,10,15, 2,10,10,-2,10,15, 2,10,10,-2},
+ {10,15, 2,10,15,-2,10,15, 2,10,15,-2,10,15, 2,10,15,-2},
+ {10,10, 2,10,15,-2,10,10, 2,10,15,-2,10,10, 2,10,15,-2}
+ };
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino

@@ -0,0 +1,31 @@
+// 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);
+}
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp

@@ -0,0 +1,55 @@
+//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
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.cpp

@@ -0,0 +1,60 @@
+//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 );    
+*/
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.h

@@ -0,0 +1,39 @@
+//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
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.cpp

@@ -0,0 +1,37 @@
+// 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
+  }
+}
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.h

@@ -0,0 +1,20 @@
+// Watchdog.h
+//  V3.0
+
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Watchdog 
+{
+public:
+  void CheckTime(Data *DataLine);
+
+};
+
+
+#endif
+
+