update

2016-ssc32-processing-port/CustomTimer.h

@@ -0,0 +1,33 @@
+// Version 1
+
+#ifndef CustomTimer_H
+#define CustomTimer_H
+
+class CustomTimer
+{
+  int time;
+  boolean start;
+  public: CustomTimer()
+  {
+    start=false; 
+  }
+  public: boolean Delay(int delay)
+  {
+    if (!start)
+    {
+      time = millis();
+      start = true;
+    }
+    if (millis() - time >= delay)
+    {
+      start = false;
+      return true;
+    } 
+    else
+      return false;
+  }
+};
+
+#endif
+
+

2016-ssc32-processing-port/FootClass.h

@@ -0,0 +1,259 @@
+// Version 1
+
+#ifndef Foot_H
+#define Foot_H
+
+#include "Vector.h";
+#include "Global.h"
+
+const double pi = 3.14159265359;
+
+class Foot
+{
+  public:
+    float TranslateX, TranslateY, TranslateZ;
+    boolean Invert;
+
+    void SetFootCoordinates(Vector P)
+    {
+      Xi = P.x;
+      Yi = P.y;
+      Zi = P.z;
+    }
+
+    // ?used to put in position from body?
+    void Translate(int x, int y, int z)
+    {
+      TranslateX = x;
+      TranslateY = y;
+      TranslateZ = z;
+    }
+
+    void Translate(Vector i)
+    {
+      TranslateX = i.x;
+      TranslateY = i.y;
+      TranslateZ = i.z;
+    }
+
+    //  Inverse Kinemetics Region
+    double l3, s, O1, O2, O3, X2, Y2, Z2;
+    double l1, l2;
+    double Xi, Yi, Zi;
+    double X, Y, Z;
+    boolean error;
+
+    void RotateX()
+    {
+      Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
+      Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
+    }
+
+    void RotateY()
+    {
+      Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
+      X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
+    }
+
+    void RotateZ()
+    {
+      X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
+      Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
+    }
+
+    void RL3 ()
+    {
+      double x = X;
+      double z = Z;
+
+      if ((x == 0) && (z == 0))
+        l3 = 0;
+      else l3 = sqrt((float)(x * x + z * z));
+
+      if (x < 0)
+        l3 = l3 * -1;
+    }
+
+    float FO1 ()
+    {
+      RL3();
+
+      double x = X;
+      double z = Z;
+      double r;
+
+      if ((x != 0) & (z != 0)) {
+        double a = (z / x);
+        return atan((float)(a));
+      }
+      else
+        return atan(0.0); // Math Error Protection.
+    }
+
+    float T1 ()
+    {
+      float x = X;
+      float y = Y;
+      float z = Z;
+      float x2 = x * x;
+      float y2 = y * y;
+      float z2 = z * z;
+      float l32 = l3 * l3;
+
+      s = sqrt((float)(x2 + y2 + z2));
+
+      float t1 = 0;
+
+      float s2 = s * s;
+
+      if ((y == 0) && (l3 == 0))
+        t1 = 0; // Math Error Protection.
+      else
+        t1 = (atan2((float)(l3), (float)(y)));
+
+      float a = (l1 * l1) - (l2 * l2) + s2;
+      float b = 2 * l1 * s;
+      float t2 = acos(a / b);
+
+      float ret = 0;
+
+      ret = t1 + abs(t2);
+
+      return ret;
+    }
+
+    float T3 ()
+    {
+      return (acos((float)(((l1 * l1) + (l2 * l2) - (s * s)) / (2 * l1 * l2))));
+    }
+
+    void setleg()
+    {
+      setleg(Invert);
+    }
+
+    void setleg(boolean Side)
+    {
+      l1 = 10;
+      l2 = 15;
+
+      X = Xi;
+      Y = Yi;
+      Z = Zi;
+
+      //  Rotation from body point
+      if (Invert)
+      { //  Right
+        X = (-1 * X) + TranslateX;
+        Z = (-1 * Z) + TranslateZ;
+      }
+      else
+      {
+        X = Xi + TranslateX;
+        Z = (-1 * Z) + TranslateZ;
+      }
+
+      RotateY();
+      RotateX();
+      RotateZ();
+
+      //  Remove gaits
+      if (Invert)
+      { //  Right
+        X = -1 * (X - TranslateX);
+        Z = -1 * (Z - TranslateZ);
+      }
+      else
+      {
+        X -= TranslateX;
+        Z = -1 * (Z - TranslateZ);
+      }
+
+      if (Invert)
+      { //  right
+        double a = (90 - degrees(FO1()));
+        double b = (180 - degrees(T1()));
+        double c = degrees(T3());
+
+        /*if ((a >= 15) & (a <= 165)
+            & (b >= 15) & (b <= 165)
+            & (c >= 15) & (c <= 165))
+          {*/
+        error = false;
+
+        O1 = a;
+        O2 = b;
+        O3 = c;
+
+        double d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          X2 = d * l1;
+        else
+          X2 = 0;
+
+        d = cos((float)(b * (pi / 180)));
+        if ( d != 0 )
+          Y2 = d * l1;
+        else
+          Y2 = 0;
+
+        d = sin((float)((a - 90) * (pi / 180)));
+        if ( d != 0 )
+          Z2 = d * abs((float)(X2));
+        else
+          Z2 = 0;
+        /*}
+          else
+          {
+          error = true;
+          }*/
+      }
+      else
+      {
+        double a = (90 - degrees((float)(FO1())));
+        double b = degrees((float)(T1()));
+        double c = 180 - degrees((float)(T3()));
+
+        /*if ((a >= 15) & (a <= 165)
+            & (b >= 15) & (b <= 165)
+            & (c >= 15) & (c <= 165))
+          {*/
+        error = false;
+
+        O1 = a;
+        O2 = b;
+        O3 = c;
+
+        a = (90 - degrees((float)(FO1())));
+        b = (180 - degrees((float)(T1())));
+        c = degrees((float)(T3()));
+
+        double d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          X2 = d * l1;
+        else
+          X2 = 0;
+
+        d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          Y2 = d * l1;
+        else
+          Y2 = 0;
+
+        d = sin((float)((a - 90) * (pi / 180)));
+        if ( d != 0 )
+          Z2 = d * abs((float)(X2));
+        else
+          Z2 = 0;
+        /*}
+          else
+          {
+          error = true;
+          Serial.println("ERROR");
+          }*/
+      }
+    }
+};
+
+#endif
+

2016-ssc32-processing-port/Global.h

@@ -0,0 +1,30 @@
+// Version 1
+
+#ifndef Global_H
+#define Global_H
+
+float rotationX = 0;
+float rotationY = 0;
+float rotationZ = 0;
+
+CustomTimer timerAnimation = CustomTimer();
+CustomTimer TimerFPS_SERVO = CustomTimer();
+
+const int FPS = 30;
+int FPS_SERVO = 30;
+
+float counterRotation = 0;
+int counterXRotation = 0;
+int counterYRotation = 0;
+int counterZRotation = 0;
+
+Vector *walk1 = new Vector[8];
+
+float robotHeigth = 12;
+
+int rotationRange = 8;
+
+#endif
+
+
+

2016-ssc32-processing-port/HexapodClass.h

@@ -0,0 +1,376 @@
+// Version 1
+
+#ifndef Hexapod_H
+#define Hexapod_H
+
+#include "FootClass.h";
+#include "Vector.h";
+#include "CustomTimer.h";
+#include "Global.h"
+#include <Streaming.h>
+
+Foot foot1 = Foot();
+Foot foot2 = Foot();
+Foot foot3 = Foot();
+Foot foot4 = Foot();
+Foot foot5 = Foot();
+Foot foot6 = Foot();
+
+//  Spaces from body to feet
+//	9.5
+//	14
+//	9.5
+
+//	10
+//	10
+
+//    LAYOUT HARDWARE
+//      _U_
+//  1==/   \==4
+//     >***<
+// 2===|---|===5
+//     >***<
+//  3==\___/==6
+
+//    LAYOUT SOFTWARE
+//      _U_
+//  1==/   \==6
+//     >***<
+// 2===|---|===5
+//     >***<
+//  3==\___/==4
+
+Vector point1 = Vector(4.75, 0, 10);
+Vector point2 = Vector(7, 0, 0);
+Vector point3 = Vector(4.75, 0, -10);
+Vector point4 = Vector(-4.75, 0, -10);
+Vector point5 = Vector(-7, 0, 0);
+Vector point6 = Vector(-4.75, 0, 10);
+
+Vector fpoint1 = Vector(10, 5, 0);
+Vector fpoint2 = Vector(10, 5, 0); //  y negative is down from body.
+Vector fpoint3 = Vector(10, 5, 0);
+Vector fpoint4 = Vector(10, 5, 0);
+Vector fpoint5 = Vector(10, 5, 0);
+Vector fpoint6 = Vector(10, 5, 0);
+
+Vector* standup = new Vector[2];
+
+//  Animation new
+int animationSpeed = 20;
+
+int ConverttoMS(double Position, int low, int high, int _min = 15, int _max = 165, int correction = 0)
+{
+  //Position += 90;
+
+  if (Position < _min | Position > _max)
+  {
+    Position = 90;
+    Serial.println("ERROR");
+  }
+
+  int tmp = ( Position * ( ( high - low ) / 180 ) );
+
+  //Serial1.println(Position);
+
+  return low + tmp + correction;
+}
+
+void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6)
+{
+  int smin0 = 600;
+  int smin1 = 960;
+  int smin2 = 600;
+  int smin3 = 600;
+  int smax0 = 2400;
+  int smax1 = 2100;
+  int smax2 = 2400;
+  int smax3 = 2400;
+
+  Serial1.print(" #");
+  Serial1.print(0);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f4.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(1);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f4.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(2);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f4.O3, smin2, smax2));
+  Serial1.print(" #");
+  Serial1.print(3);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f5.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(4);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f5.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(5);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f5.O3, smin2, smax2));
+  Serial1.print(" #");
+  Serial1.print(6);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f6.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(7);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f6.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(8);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f6.O3, smin2, smax2));
+
+  Serial1.print(" #");
+  Serial1.print(9);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(90, smin3, smax3));
+
+  Serial1.print(" #");
+  Serial1.print(16);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(17);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(18);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+  Serial1.print(" #");
+  Serial1.print(19);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f2.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(20);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f2.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(21);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f2.O3, smin2, smax2));
+  Serial1.print(" #");
+  Serial1.print(22);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f3.O1, smin0, smax0));
+  Serial1.print(" #");
+  Serial1.print(23);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f3.O2, smin1, smax1));
+  Serial1.print(" #");
+  Serial1.print(24);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f3.O3, smin2, smax2));
+
+  Serial1.print(" T");
+  Serial1.println(0);
+
+
+
+  /*Serial << " #";
+    Serial << 0;
+    Serial << " P";
+    Serial << f4.O1;
+    Serial << " #";
+    Serial << 1;
+    Serial << " P";
+    Serial << f4.O2;
+    Serial << " #";
+    Serial << 2;
+    Serial << " P";
+    Serial << f4.O3;
+    Serial << " #";
+    Serial << 3;
+    Serial << " P";
+    Serial << f5.O1;
+    Serial << " #";
+    Serial << 4;
+    Serial << " P";
+    Serial << f5.O2;
+    Serial << " #";
+    Serial << 5;
+    Serial << " P";
+    Serial << f5.O3;
+    Serial << " #";
+    Serial << 6;
+    Serial << " P";
+    Serial << f6.O1;
+    Serial << " #";
+    Serial << 7;
+    Serial << " P";
+    Serial << f6.O2;
+    Serial << " #";
+    Serial << 8;
+    Serial << " P";
+    Serial << f6.O3;
+
+    Serial << " #";
+    Serial << 16;
+    Serial << " P";
+    Serial << f1.O1;
+    Serial << " #";
+    Serial << 17;
+    Serial << " P";
+    Serial << f1.O2;
+    Serial << " #";
+    Serial << 18;
+    Serial << " P";
+    Serial << f1.O3;
+    Serial << " #";
+    Serial << 19;
+    Serial << " P";
+    Serial << f2.O1;
+    Serial << " #";
+    Serial << 20;
+    Serial << " P";
+    Serial << f2.O2;
+    Serial << " #";
+    Serial << 21;
+    Serial << " P";
+    Serial << f2.O3;
+    Serial << " #";
+    Serial << 22;
+    Serial << " P";
+    Serial << f3.O1;
+    Serial << " #";
+    Serial << 23;
+    Serial << " P";
+    Serial << f3.O2;
+    Serial << " #";
+    Serial << 24;
+    Serial << " P";
+    Serial << f3.O3;
+
+    Serial << " T";
+    Serial << 0 << "\n\r";*/
+}
+//////////////////////////////////////////////////
+
+void DrawFeet()
+{
+  if (TimerFPS_SERVO.Delay(1000 / FPS_SERVO))
+  {
+    foot1.Translate(point1);
+    foot1.SetFootCoordinates(fpoint1);
+    foot1.Invert = true;
+    foot1.setleg();
+
+    foot1.Translate(point2);
+    foot2.SetFootCoordinates(fpoint2);
+    foot2.Invert = true;
+    foot2.setleg();
+
+    foot1.Translate(point3);
+    foot3.SetFootCoordinates(fpoint3);
+    foot3.Invert = true;
+    foot3.setleg();
+
+    foot1.Translate(point4);
+    foot4.SetFootCoordinates(fpoint4);
+    foot4.Invert = false;
+    foot4.setleg();
+
+    foot1.Translate(point5);
+    foot5.SetFootCoordinates(fpoint5);
+    foot5.Invert = false;
+    foot5.setleg();
+
+    foot1.Translate(point6);
+    foot6.SetFootCoordinates(fpoint6);
+    foot6.Invert = false;
+    foot6.setleg();
+
+    mSend(foot1, foot2, foot3, foot4, foot5, foot6);
+  }
+}
+
+/*void RobotWalk()
+  {
+  if (robotStanding)
+  {
+    //animationRepeat = true;
+    animationSpeed = 20;
+
+    if (!animationRepeatComplete)
+      if (timerAnimation.Delay(animationSpeed))
+        animationSubStep++;
+
+    Serial1.println("robot walk animation");
+
+    foot1.SetFootCoordinates(Animation(walk1, 8, 0));  //  BULLSHIT BUG
+    foot1.SetFootCoordinates(Animation(walk1, 8, 0));
+    foot2.SetFootCoordinates(Animation(walk1, 8, 4));
+    foot3.SetFootCoordinates(Animation(walk1, 8, 0));//
+    foot4.SetFootCoordinates(Animation(walk1, 8, 4));//
+    foot5.SetFootCoordinates(Animation(walk1, 8, 0));
+    foot6.SetFootCoordinates(Animation(walk1, 8, 4));
+  }
+  }*/
+
+float _animationProgress = 0;
+bool rotation = false;
+
+void RobotStandup()
+{
+  standup[0] = Vector(10, 5, 0);
+  standup[1] = Vector(10, 15, 0);
+
+  if (timerAnimation.Delay(1000 / FPS))
+  {
+    Serial.println("robot standup animation");
+
+    fpoint1.Lerp(standup[0], standup[1], _animationProgress);
+    fpoint2.Lerp(standup[0], standup[1], _animationProgress);
+    fpoint3.Lerp(standup[0], standup[1], _animationProgress);
+    fpoint4.Lerp(standup[0], standup[1], _animationProgress);
+    fpoint5.Lerp(standup[0], standup[1], _animationProgress);
+    fpoint6.Lerp(standup[0], standup[1], _animationProgress);
+
+    Serial.println(point1.Print());
+
+    int _speed = 2;
+
+    _animationProgress = _animationProgress < 1.0f ? (_animationProgress + (1.0f / (FPS * _speed))) : 1.0f;
+
+    /*if (_animationProgress == 1)
+      {
+      int uhh = (millis() / 100) % 100;
+      if (rotation == true)
+      {
+        Serial.println("rotate up");
+        rotationZ = map((millis() / 100) % 100, 0, 99, -25, 25);
+        if (uhh >= 99)
+        {
+          rotation = true;
+          uhh++;
+        }
+      }
+      else
+      {
+        Serial.println("rotate upside down");
+        rotationZ = map((millis() / 100) % 100, 0, 99, 25, -25);
+        if (uhh >= 99)
+        {
+          rotation = false;
+          uhh++;
+      }
+      }
+      }*/ 
+  }
+}
+
+void DrawBody()
+{
+  RobotStandup();
+
+  //RobotWalk();
+
+  DrawFeet();
+}
+
+#endif
+

2016-ssc32-processing-port/Hexapod_NEW.ino

@@ -0,0 +1,53 @@
+// Version 1
+
+#include "Arduino.h"
+#include "CustomTimer.h"
+#include "HexapodClass.h"
+#include "Vector.h"
+#include "Global.h"
+
+//#include <avr/wdt.h>
+
+void setup()
+{
+  //wdt_enable(WDTO_1S);
+
+  walk1[0] = Vector(12, robotHeigth, 2);
+  walk1[1] = Vector(12, robotHeigth, 0);
+  walk1[2] = Vector(12, robotHeigth, -2);
+  walk1[3] = Vector(12, robotHeigth, -2);
+  walk1[4] = Vector(12, robotHeigth - 10, -2);
+  walk1[5] = Vector(12, robotHeigth - 10, 0);
+  walk1[6] = Vector(12, robotHeigth - 10, 2);
+  walk1[7] = Vector(12, robotHeigth, 2);
+
+  Serial.begin(115200);
+  Serial1.begin(115200);
+
+  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 loop()
+{
+  // the program is alive...for now.
+  //wdt_reset();
+
+  //  Rotation calculation, please use transformation matrixes instead.
+  //rotationX = ( counterXRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterXRotation % (rotationRange * 2) : rotationRange + (-1 * (counterXRotation % (rotationRange * 2))) );
+  //rotationY = ( counterYRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterYRotation % (rotationRange * 2) : rotationRange + (-1 * (counterYRotation % (rotationRange * 2))) );
+  //rotationZ = ( counterZRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterZRotation % (rotationRange * 2) : rotationRange + (-1 * (counterZRotation % (rotationRange * 2))) );
+
+  DrawBody();
+}

2016-ssc32-processing-port/Servo.h

@@ -0,0 +1,104 @@
+// Version 1
+
+//#ifndef Servo_H
+//#define Servo_H
+
+/*#include "FootClass.h";
+
+int ConverttoMS(int Position, int low, int high)
+{
+  int tmp = ( Position * ( ( high - low ) / 180 ) );
+  return low + tmp;
+}
+
+void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6)
+{
+  int smin0 = 600;
+  int smin1 = 960;
+  int smin2 = 600;
+  int smin3 = 600;
+  int smax0 = 2400;
+  int smax1 = 2100;
+  int smax2 = 2400;
+  int smax3 = 2400;
+
+  Serial1.print(" #"); 
+  Serial1.print(0);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(1);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(2);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+  Serial1.print(" #"); 
+  Serial1.print(4);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(5);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(6);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+  Serial1.print(" #"); 
+  Serial1.print(7);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(8);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(9);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+
+  Serial1.print(" #"); 
+  Serial1.print(16);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(17);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(18);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+  Serial1.print(" #"); 
+  Serial1.print(19);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(20);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(21);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+  Serial1.print(" #"); 
+  Serial1.print(22);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
+  Serial1.print(" #"); 
+  Serial1.print(23);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
+  Serial1.print(" #"); 
+  Serial1.print(24);
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
+
+  Serial1.print(" T");
+  Serial1.println(0);
+}*/
+
+//#endif
+

2016-ssc32-processing-port/Vector.h

@@ -0,0 +1,66 @@
+// Version 1
+
+#ifndef Servo_H
+#define Servo_H
+
+class Vector
+{
+  public:
+    float x, y, z;
+    Vector(float X, float Y, float Z)
+    {
+      x = X;
+      y = Y;
+      z = Z;
+    }
+    void Init(float X, float Y, float Z)
+    {
+      x = X;
+      y = Y;
+      z = Z;
+    }
+    Vector()
+    {
+
+    }
+    Vector Lerp(Vector start, Vector end, float percent)
+    {
+      Vector _new = (start + ((end - start) * percent));
+      x = _new.x;
+      y = _new.y;
+      z = _new.z;
+      return _new;
+    }
+    Vector operator + (const Vector other)
+    {
+      Vector _new = Vector();
+      _new.x = x + other.x;
+      _new.y = y + other.y;
+      _new.z = z + other.z;
+
+      return _new;
+    }
+    Vector operator - (const Vector other)
+    {
+      Vector _new = Vector();
+      _new.x = x - other.x;
+      _new.y = y - other.y;
+      _new.z = z - other.z;
+
+      return _new;
+    }
+    Vector operator * (const float other)
+    {
+      Vector _new = Vector(x * other, y * other, z * other);
+      
+      return _new;
+    }
+    String Print()
+    {
+      return String(x) + " " + String(y) + " " + String(z);
+    }
+};
+
+#endif
+
+

2019-teensy-nrf24/robot/Hexapod_Teensy.ino

@@ -0,0 +1,93 @@
+#include "Nrf.h"
+#include <Wire.h>
+#include <MPU6050_tockn.h>
+#include "PWMServo.h"
+#include "config.h"
+
+PWMServo servos[18];
+
+MPU6050 mpu (Wire);
+
+dataStruct2 data;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(115200);
+  //  while (!Serial) ; // wait for arduino serial monitor
+
+  for (int i = 0; i < 18; i++)
+    servos[i].attach(pins[i], servoMin[i] + servoCallibration[i],
+                     servoMax[i] + servoCallibration[i]); // 900-2100 for high torque servos
+//  while (true)
+//  {
+    for (int i = 0; i < 18; i++)
+      servos[i].write(90 + servoOffset[i]);
+//  }
+
+
+  Serial.println("Starting mpu6050");
+
+//  //   Initialization
+//  Wire.begin();
+//  mpu.begin();
+//
+//  mpu.calcGyroOffsets(true, 300, 1000);
+//
+//  // Calibration
+//  Serial.println("=====================================");
+//  Serial.println("Starting calibration...");
+//  mpu.calcGyroOffsets();
+//  Serial.println("Calibration complete!");
+
+  Serial.println("Starting nrf");
+  NrfInit();
+}
+
+void STOP()
+{
+  for (int i = 0; i < 18; i++)
+    pinMode(pins[i], INPUT);
+  while (true)
+  {}
+}
+
+int i = 0;
+
+void loop()
+{
+  network.update();
+
+  if (false)
+  {
+    mpu.update();
+    Serial.print("AngX = ");
+    Serial.print(mpu.getAngleX());
+    Serial.print("  /  AngY = ");
+    Serial.print(mpu.getAngleY());
+    Serial.print("  /  AngZ = ");
+    Serial.println(mpu.getAngleZ());
+  }
+
+  //    Serial.print("  /  AngX = ");
+  //    Serial.print(mpu.getGyroAngleX());
+  //    Serial.print("  /  AngY = ");
+  //    Serial.print(mpu.getGyroAngleY());
+  //    Serial.print("  /  AngZ = ");
+  //    Serial.println(mpu.getGyroAngleZ());
+
+  data = NrfGetData(data, true);
+
+//  if (data.STOP)
+//    STOP();
+
+  // test
+  //  data.servoPositions[0] = data.servoPositions[0] + 0.1f;
+  //  data = NrfSendData(data, false);
+
+  if (data.remotePasstroughControl == true)
+    for (int i = 0; i < 18; i++)
+      servos[i].write(data.servoPositions[i] + servoOffset[i]);
+
+  delay(20);
+}

2019-teensy-nrf24/robot/Nrf.h

@@ -0,0 +1,66 @@
+#include <SPI.h>
+#include "RF24.h"
+#include <RF24Network.h>
+#include "TransferDataStructure.h"
+
+// Nrf24 settings
+RF24 radio(39, 15); // CE, CSN
+RF24Network network(radio);
+
+const uint16_t this_node = 01;        // Address of our node in Octal format
+const uint16_t other_node = 00;       // Address of the other node in Octal format
+
+void NrfInit()
+{
+  SPI.setSCK(14);
+  SPI.setMOSI(11);
+  SPI.setMISO(12);
+
+  SPI.begin();
+  radio.begin();
+  network.begin(/*channel*/ 90, /*node address*/ this_node);
+
+  // Set the PA Level low to prevent power supply related issues since this is a
+  // getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
+  radio.setPALevel(RF24_PA_LOW);
+}
+
+dataStruct2 NrfGetData(dataStruct2 data, bool debug = false)
+{
+  //  network.update();
+  int timeout = millis() + 100;
+  while ( network.available() && timeout > millis() )
+  {
+    RF24NetworkHeader header;
+    network.read(header, &data, sizeof(data));
+
+    if (debug)
+    {
+      Serial.print("Received packet ");
+      Serial.println(data.servoPositions[0]);
+    }
+  }
+
+  return data;
+}
+
+dataStruct2 NrfSendData(dataStruct2 data, bool debug = false)
+{
+  //  network.update();
+
+  if (debug)
+    Serial.print("Sending...");
+
+  RF24NetworkHeader header(/*to node*/ other_node);
+  bool ok = network.write(header, &data, sizeof(data));
+
+  if (debug)
+  {
+    if (ok)
+      Serial.println("ok.");
+    else
+      Serial.println("failed.");
+  }
+
+  return data;
+}

2019-teensy-nrf24/robot/PWMServo.cpp

@@ -0,0 +1,226 @@
+#include "Arduino.h"
+#include "PWMServo.h"
+
+/*
+  PWMServo.cpp - Hardware Servo Timer Library
+  http://arduiniana.org/libraries/pwmservo/
+  Author: Jim Studt, jim@federated.com
+  Copyright (c) 2007 David A. Mellis.  All right reserved.
+  renamed to PWMServo by Mikal Hart
+  ported to other chips by Paul Stoffregen
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#define NO_ANGLE (0xff)
+
+#if defined(__AVR__)
+
+#include <avr/interrupt.h>
+
+uint8_t PWMServo::attachedA = 0;
+uint8_t PWMServo::attachedB = 0;
+#ifdef SERVO_PIN_C
+uint8_t PWMServo::attachedC = 0;
+#endif
+
+void PWMServo::seizeTimer1()
+{
+  uint8_t oldSREG = SREG;
+
+  cli();
+  TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
+  TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
+           | _BV(CS11) /* div 8 clock prescaler */
+           ;
+  OCR1A = 3000;
+  OCR1B = 3000;
+  ICR1 = clockCyclesPerMicrosecond() * (20000L / 8); // 20000 uS is a bit fast for the refresh, 20ms, but
+  // it keeps us from overflowing ICR1 at 20MHz clocks
+  // That "/8" at the end is the prescaler.
+#if defined(__AVR_ATmega8__)
+  TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
+#else
+  TIMSK1 &=  ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
+#endif
+
+  SREG = oldSREG;  // undo cli()
+}
+
+void PWMServo::releaseTimer1() {}
+
+PWMServo::PWMServo() : pin(0), angle(NO_ANGLE) {}
+
+uint8_t PWMServo::attach(int pinArg, int min, int max)
+{
+#ifdef SERVO_PIN_C
+  if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B && pinArg != SERVO_PIN_C) return 0;
+#else
+  if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B) return 0;
+#endif
+
+  min16 = min / 16;
+  max16 = max / 16;
+
+  pin = pinArg;
+  angle = NO_ANGLE;
+  digitalWrite(pin, LOW);
+  pinMode(pin, OUTPUT);
+
+#ifdef SERVO_PIN_C
+  if (!attachedA && !attachedB && !attachedC) seizeTimer1();
+#else
+  if (!attachedA && !attachedB) seizeTimer1();
+#endif
+
+  if (pin == SERVO_PIN_A) {
+    attachedA = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1);
+  }
+
+  if (pin == SERVO_PIN_B) {
+    attachedB = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1);
+  }
+
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) {
+    attachedC = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1C0)) | _BV(COM1C1);
+  }
+#endif
+  return 1;
+}
+
+void PWMServo::detach()
+{
+  // muck with timer flags
+  if (pin == SERVO_PIN_A) {
+    attachedA = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1);
+    pinMode(pin, INPUT);
+  }
+
+  if (pin == SERVO_PIN_B) {
+    attachedB = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1);
+    pinMode(pin, INPUT);
+  }
+
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) {
+    attachedC = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1C0) & ~_BV(COM1C1);
+    pinMode(pin, INPUT);
+  }
+#endif
+
+#ifdef SERVO_PIN_C
+  if (!attachedA && !attachedB && !attachedC) releaseTimer1();
+#else
+  if (!attachedA && !attachedB) releaseTimer1();
+#endif
+}
+
+void PWMServo::write(int angleArg)
+{
+  uint16_t p;
+
+  if (angleArg < 0) angleArg = 0;
+  if (angleArg > 180) angleArg = 180;
+  angle = angleArg;
+
+  // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
+  // That 8L on the end is the TCNT1 prescaler, it will need to change if the clock's prescaler changes,
+  // but then there will likely be an overflow problem, so it will have to be handled by a human.
+  p = (min16 * 16L * clockCyclesPerMicrosecond() + (max16 - min16) * (16L * clockCyclesPerMicrosecond()) * angle / 180L) / 8L;
+  if (pin == SERVO_PIN_A) OCR1A = p;
+  if (pin == SERVO_PIN_B) OCR1B = p;
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) OCR1C = p;
+#endif
+}
+
+uint8_t PWMServo::attached()
+{
+  if (pin == SERVO_PIN_A && attachedA) return 1;
+  if (pin == SERVO_PIN_B && attachedB) return 1;
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C && attachedC) return 1;
+#endif
+  return 0;
+}
+
+
+#elif defined(__arm__) && defined(TEENSYDUINO)
+
+uint32_t PWMServo::attachedpins[(NUM_DIGITAL_PINS + 31) / 32]; // 1 bit per digital pin
+
+PWMServo::PWMServo() : pin(255), angle(NO_ANGLE) {}
+
+uint8_t PWMServo::attach(int pinArg, int min, int max)
+{
+//  Serial.printf("attach, pin=%d, min=%d, max=%d\n", pinArg, min, max);
+  if (pinArg < 0 || pinArg >= NUM_DIGITAL_PINS) return 0;
+  if (!digitalPinHasPWM(pinArg)) return 0;
+  pin = pinArg;
+  analogWriteFrequency(pin, 50);
+  min16 = min >> 4;
+  max16 = max >> 4;
+  angle = NO_ANGLE;
+  digitalWrite(pin, LOW);
+  pinMode(pin, OUTPUT);
+  attachedpins[pin >> 5] |= (1 << (pin & 31));
+//  Serial.println("Succes!");
+  return 1;
+}
+
+void PWMServo::write(float angleArg)
+{
+  //Serial.printf("write, pin=%d, angle=%d\n", pin, angleArg);
+  if (pin >= NUM_DIGITAL_PINS) return;
+  if (angleArg < 0) angleArg = 0;
+  if (angleArg > 180) angleArg = 180;
+  angle = angleArg;
+
+  uint32_t us = (((max16 - min16) * 46603 * (uint8_t)angle) >> 11) + (min16 << 12); // us*256
+  //uint32_t duty = (us * 3355) >> 22;
+
+  float usec = (float)((max16 - min16) << 4) * ((float)angle / 180.0f) + (float)(min16 << 4);
+  uint32_t duty = (int)(usec / 20000.0f * 4096.0f);
+
+  if (false)
+    Serial.printf("pin=%d, angle=%.2f, usec=%.2f, us=%.2f, duty=%d, min=%d, max=%d\n",
+                  pin, (float)angle, usec, (float)us / 256.0f, duty, min16 << 4, max16 << 4);
+
+#if TEENSYDUINO >= 137
+  noInterrupts();
+  uint32_t oldres = analogWriteResolution(12);
+  analogWrite(pin, duty);
+  analogWriteResolution(oldres);
+  interrupts();
+#else
+  analogWriteResolution(12);
+  analogWrite(pin, duty);
+#endif
+}
+
+uint8_t PWMServo::attached()
+{
+  if (pin >= NUM_DIGITAL_PINS) return 0;
+  return (attachedpins[pin >> 5] & (1 << (pin & 31))) ? 1 : 0;
+}
+
+#endif

2019-teensy-nrf24/robot/PWMServo.h

@@ -0,0 +1,94 @@
+#ifndef PWMServo_h
+#define PWMServo_h
+
+/*
+  PWMServo.h - Hardware Servo Timer Library
+  http://arduiniana.org/libraries/pwmservo/
+  Author: Jim Studt, jim@federated.com
+  Copyright (c) 2007 David A. Mellis.  All right reserved.
+  renamed to PWMServo by Mikal Hart
+  ported to other chips by Paul Stoffregen
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <inttypes.h>
+
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) // Arduino
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // Arduino Mega
+  #define SERVO_PIN_A 11
+  #define SERVO_PIN_B 12
+  #define SERVO_PIN_C 13
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino
+  #define SERVO_PIN_A 13
+  #define SERVO_PIN_B 12
+#elif defined(__AVR_AT90USB162__) // Teensy 1.0
+  #define SERVO_PIN_A 17
+  #define SERVO_PIN_B 18
+  #define SERVO_PIN_C 15
+#elif defined(__AVR_ATmega32U4__) // Teensy 2.0
+  #define SERVO_PIN_A 14
+  #define SERVO_PIN_B 15
+  #define SERVO_PIN_C 4
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) // Teensy++
+  #define SERVO_PIN_A 25
+  #define SERVO_PIN_B 26
+  #define SERVO_PIN_C 27
+#elif defined(__arm__) && defined(TEENSYDUINO)
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+  // all PWM pins supported, but names not defined here...
+  // just use the actual PWM pin number with attach()
+#else
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+#endif
+
+class PWMServo
+{
+  private:
+    uint8_t pin;
+    float angle;       // in degrees
+    uint8_t min16;       // minimum pulse, 16uS units  (default is 34)
+    uint8_t max16;       // maximum pulse, 16uS units, 0-4ms range (default is 150)
+#if defined(__AVR__)
+    static void seizeTimer1();
+    static void releaseTimer1();
+    static uint8_t attachedA;
+    static uint8_t attachedB;
+    #ifdef SERVO_PIN_C
+    static uint8_t attachedC;
+    #endif
+#elif defined(__arm__) && defined(TEENSYDUINO)
+    static uint32_t attachedpins[]; // 1 bit per digital pin
+#endif
+  public:
+    PWMServo();
+    uint8_t attach(int pinArg) { return attach(pinArg, 544, 2400); }
+                             // pulse length for 0 degrees in microseconds, 544uS default
+                             // pulse length for 180 degrees in microseconds, 2400uS default
+    uint8_t attach(int pinArg, int min, int max);
+                             // attach to a pin, sets pinMode, returns 0 on failure, won't
+                             // position the servo until a subsequent write() happens
+                             // Only works for 9 and 10.
+    void detach();
+    void write(float angleArg); // specify the angle in degrees, 0 to 180
+    uint8_t read() { return angle; }
+    uint8_t attached();
+};
+
+#endif

2019-teensy-nrf24/robot/TransferDataStructure.h

@@ -0,0 +1,29 @@
+#ifndef dataStruct
+#define dataStruct
+
+struct __attribute__((__packed__)) dataStruct2
+{
+  bool STOP = false;
+
+  float walkSpeed = 10; //step size
+  float walkSpeed2 = 0; //gate speed
+  float walkRotation = 0 ;
+  float walkDirection = 0;
+
+  float tranform[3] = {0, 0, 0};
+  float tranform2[3] = {0, 0, 0};
+  float rotation[3] = {0, 0, 0};
+
+  bool remotePasstroughControl = true;
+  float servoPositions[18] = {
+    90, 90, 90,
+    90, 90, 90,
+    90, 90, 90,
+
+    90, 90, 90,
+    90, 90, 90,
+    90, 90, 90
+  };
+} dataStruct;
+
+#endif

2019-teensy-nrf24/robot/config.h

@@ -0,0 +1,44 @@
+byte pins[18] = {4, 3, 2,
+                 7, 6, 5,
+                 10, 9, 8,
+
+                 23, 22, 21,
+                 20, 36, 37,
+                 17, 16, 38
+                };
+
+int servoMin[18] = {553, 840, 553,
+                    553, 840, 553,
+                    553, 840, 553,
+
+                    553, 840, 553,
+                    553, 840, 553,
+                    553, 840, 553
+                   };
+
+int servoMax[18] = {2425, 2050, 2425,
+                    2425, 2050, 2425,
+                    2425, 2050, 2425,
+
+                    2425, 2050, 2425,
+                    2425, 2050, 2425,
+                    2425, 2050, 2425
+                   };
+
+int servoCallibration[18] = {0, 100, 0,
+                             0, 100, -30,
+                             0, 80, -50,
+
+                             -120, 0, 0,
+                             -10, 20, 0,
+                             0, 20, -80
+                            };
+
+float servoOffset[18] = {0, 0, 0,
+                         0, 0, 0,
+                         0, 0, 0,
+
+                         90, 0, 0,
+                         0, 0, 0,
+                         -90, 0, 0
+                        };