LEGACY: older hexapod sketches (Hexapod_NEW, HexapodRobot)
From https://mangavalk.com/Hexapod/Hexapod @ 7945005 (its Hexapod_Teensy is identical to the copy already in LEGACY). Earlier electronics generation: on-board IK driving an external SSC-32-style servo controller. Notes on what still applies are in CLAUDE.md. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
5ff85c4eb8
commit
09f7424714
11 files changed
+1318
-1
No files matched your search
@@ -0,0 +1,317 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef HEX
|
||||
#define HEX
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "SerD.h"
|
||||
#include <QueueList.h>
|
||||
|
||||
// Angle to Pulse Length converter.
|
||||
#define smin0 600
|
||||
#define smin1 960
|
||||
#define smin2 600
|
||||
#define smin3 600
|
||||
#define smax0 2400
|
||||
#define smax1 2100
|
||||
#define smax2 2400
|
||||
#define smax3 2400
|
||||
|
||||
// Task Pipeline
|
||||
#define H_TASK 001
|
||||
#define H_IK 002
|
||||
#define H_IKER 003
|
||||
#define H_ATPC 004
|
||||
|
||||
// ProgramControl
|
||||
#define PC_EMPTY 101
|
||||
#define PC_NEEDDATA 102
|
||||
|
||||
// LAYOUT
|
||||
// _U_
|
||||
// 0==/ \==3
|
||||
// >***<
|
||||
// 1===|---|===4
|
||||
// >***<
|
||||
// 2==\___/==5
|
||||
|
||||
class Hexapod
|
||||
{
|
||||
private:
|
||||
// Task Pipeline.
|
||||
QueueList<int> TaskQueue;
|
||||
QueueList<int> Parameter;
|
||||
|
||||
// Program Control
|
||||
int PcCommand;
|
||||
|
||||
// Timing.
|
||||
int OldTime;
|
||||
int TimeBetweenNewPosition;
|
||||
|
||||
/* Servo Data
|
||||
# Including coordinate, Angle and Pulse Length.
|
||||
*/
|
||||
boolean DataReady;
|
||||
SerD ServoData[6];
|
||||
|
||||
// Inverse Kinemetics.
|
||||
float s;
|
||||
float l3;
|
||||
#define l2 15
|
||||
#define l1 10
|
||||
|
||||
void RL3 (int i)
|
||||
{
|
||||
float x = ServoData[i].x;
|
||||
float z = ServoData[i].z;
|
||||
|
||||
if ((x == 0) && (z == 0))
|
||||
l3 = 0;
|
||||
else l3 = hypot(z, x);
|
||||
|
||||
if (x < 0)
|
||||
l3 *= -1;
|
||||
}
|
||||
|
||||
float O1 (int i)
|
||||
{
|
||||
RL3(i);
|
||||
|
||||
float x = ServoData[i].x;
|
||||
float z = ServoData[i].z;
|
||||
float r;
|
||||
|
||||
if ((x != 0) & (z != 0)){
|
||||
double a = (z / x);
|
||||
return atan(a);
|
||||
}
|
||||
else
|
||||
return atan(0.0); // Math Error Protection.
|
||||
}
|
||||
|
||||
float T1 (int i)
|
||||
{
|
||||
float x = ServoData[i].x;
|
||||
float y = ServoData[i].y;
|
||||
float z = ServoData[i].z;
|
||||
float x2 = x * x;
|
||||
float y2 = y * y;
|
||||
float z2 = z * z;
|
||||
float l32 = l3 * l3;
|
||||
|
||||
s = sqrt(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(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2)));
|
||||
}
|
||||
|
||||
void setleg (int i)
|
||||
{
|
||||
if (i < 3)
|
||||
{
|
||||
float a = ((float)90 - degrees(O1(i)));
|
||||
float b = ((float)180 - degrees(T1(i)));
|
||||
float c = degrees(T3());
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
ServoData[i].o1 = a;
|
||||
ServoData[i].o2 = b;
|
||||
ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
TaskQueue.push(H_IKER);
|
||||
Parameter.push(i);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float a = ((float)90 + degrees(O1(i)));
|
||||
float b = degrees(T1(i));
|
||||
float c = ((float)180 - degrees(T3()));
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
ServoData[i].o1 = a;
|
||||
ServoData[i].o2 = b;
|
||||
ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
TaskQueue.push(H_IKER);
|
||||
Parameter.push(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IK()
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
setleg(i);
|
||||
}
|
||||
|
||||
// Angle to Pulse Length converter.
|
||||
|
||||
int ConverttoMS(int Position, int low, int high)
|
||||
{
|
||||
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
||||
return low + tmp;
|
||||
}
|
||||
|
||||
void ConvertAngleToPulseLength()
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
ServoData[i].t1 = ConverttoMS(ServoData[i].o1, smin0, smax0);
|
||||
ServoData[i].t2 = ConverttoMS(ServoData[i].o2, smin1, smax1);
|
||||
ServoData[i].t3 = ConverttoMS(ServoData[i].o3, smin2, smax2);
|
||||
}
|
||||
}
|
||||
|
||||
void ProgramControl( int Command )
|
||||
{
|
||||
// Drop task from queue
|
||||
TaskQueue.pop();
|
||||
|
||||
// Check Command.
|
||||
switch (Command)
|
||||
{
|
||||
case PC_EMPTY:
|
||||
{
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case PC_NEEDDATA:
|
||||
{
|
||||
/*
|
||||
Queue = {
|
||||
Get Move
|
||||
Do IK
|
||||
Convert To MS
|
||||
Send }
|
||||
*/
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
// Task Pipeline.
|
||||
void loop()
|
||||
{
|
||||
PcCommand = 0;
|
||||
do{
|
||||
/*
|
||||
Runmonitoing methods and
|
||||
Timingdependent methods.
|
||||
*/
|
||||
if (millis() >= (OldTime + TimeBetweenNewPosition))
|
||||
{
|
||||
OldTime = millis();
|
||||
}
|
||||
|
||||
//Add Task toqueue
|
||||
TaskQueue.push(H_TASK);
|
||||
|
||||
//task queue.
|
||||
|
||||
switch ( TaskQueue.peek() )
|
||||
{
|
||||
|
||||
case H_TASK:
|
||||
{
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case H_IK: // Do InverseKinemetics.
|
||||
{
|
||||
IK();
|
||||
break;
|
||||
}
|
||||
|
||||
case H_IKER: // Inverse Kinemetics Error.
|
||||
{
|
||||
while ( !Parameter.isEmpty() )
|
||||
{
|
||||
// Go troughtparameters.
|
||||
/*
|
||||
# W
|
||||
# I
|
||||
# P
|
||||
*/
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case H_ATPC: // Angel to pulse length converter.
|
||||
{
|
||||
ConvertAngleToPulseLength();
|
||||
DataReady = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case H_SEND:
|
||||
{
|
||||
if (DataReady)
|
||||
{
|
||||
// Send
|
||||
DataReady = false;
|
||||
// Get New Move
|
||||
}
|
||||
else
|
||||
{
|
||||
PcCommand = PC_NEEDDATA;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default: // TaskList was empty.
|
||||
{
|
||||
// Do something when there is no task.
|
||||
PcCommand = PC_EMPTY;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//Program Control.
|
||||
ProgramControl( PcCommand );
|
||||
} while (1);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
// Version 1
|
||||
#include "Hexapod.h"
|
||||
#include <QueueList.h>
|
||||
|
||||
void setup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
Hexapod hexapod;
|
||||
|
||||
void loop()
|
||||
{
|
||||
hexapod.loop();
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef SerD_H
|
||||
#define SerD_H
|
||||
|
||||
class SerD
|
||||
{
|
||||
public:
|
||||
|
||||
// Coordinate
|
||||
int x;
|
||||
int y;
|
||||
int z;
|
||||
|
||||
// Angles
|
||||
int o1;
|
||||
int o2;
|
||||
int o3;
|
||||
|
||||
// Pulse Length.
|
||||
int t1;
|
||||
int t2;
|
||||
int t3;
|
||||
|
||||
//Input
|
||||
SerD& operator() (int X, int Y, int Z)
|
||||
{
|
||||
x = X;
|
||||
y = Y;
|
||||
z = Z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerD& operator= (SerD& R)
|
||||
{
|
||||
x = R.x;
|
||||
y = R.y;
|
||||
z = R.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
Reference in new issue
Block a user