First commit
This commit is contained in:
Administrator committed
commit
1f1715c841
4 files changed
+418
No files matched your search
@@ -0,0 +1,37 @@
|
||||
*.wmv filter=lfs diff=lfs merge=lfs -text
|
||||
*.WMV filter=lfs diff=lfs merge=lfs -text
|
||||
*.mp4 filter=lfs diff=lfs merge=lfs -text
|
||||
*.MP4 filter=lfs diff=lfs merge=lfs -text
|
||||
*.MPG filter=lfs diff=lfs merge=lfs -text
|
||||
*.mpg filter=lfs diff=lfs merge=lfs -text
|
||||
*.jpg filter=lfs diff=lfs merge=lfs -text
|
||||
*.JPG filter=lfs diff=lfs merge=lfs -text
|
||||
*.jpeg filter=lfs diff=lfs merge=lfs -text
|
||||
*.png filter=lfs diff=lfs merge=lfs -text
|
||||
*.PNG filter=lfs diff=lfs merge=lfs -text
|
||||
*.bmp filter=lfs diff=lfs merge=lfs -text
|
||||
*.exr filter=lfs diff=lfs merge=lfs -text
|
||||
*.pdf filter=lfs diff=lfs merge=lfs -text
|
||||
*.docx filter=lfs diff=lfs merge=lfs -text
|
||||
*.vsdx filter=lfs diff=lfs merge=lfs -text
|
||||
*.dwg filter=lfs diff=lfs merge=lfs -text
|
||||
*.f3d filter=lfs diff=lfs merge=lfs -text
|
||||
*.zip filter=lfs diff=lfs merge=lfs -text
|
||||
*.zi filter=lfs diff=lfs merge=lfs -text
|
||||
*.stl filter=lfs diff=lfs merge=lfs -text
|
||||
*.STL filter=lfs diff=lfs merge=lfs -text
|
||||
*.HMI filter=lfs diff=lfs merge=lfs -text
|
||||
*.exe filter=lfs diff=lfs merge=lfs -text
|
||||
*.dll filter=lfs diff=lfs merge=lfs -text
|
||||
*.pdb filter=lfs diff=lfs merge=lfs -text
|
||||
*.idb filter=lfs diff=lfs merge=lfs -text
|
||||
*.obj filter=lfs diff=lfs merge=lfs -text
|
||||
*.ipch filter=lfs diff=lfs merge=lfs -text
|
||||
*.sdf filter=lfs diff=lfs merge=lfs -text
|
||||
*.suo filter=lfs diff=lfs merge=lfs -text
|
||||
*.db filter=lfs diff=lfs merge=lfs -text
|
||||
*.lnk filter=lfs diff=lfs merge=lfs -text
|
||||
*.ide filter=lfs diff=lfs merge=lfs -text
|
||||
*.ide-wal filter=lfs diff=lfs merge=lfs -text
|
||||
*.ide-shm filter=lfs diff=lfs merge=lfs -text
|
||||
2018-unity-sim/**/*.unity filter=lfs diff=lfs merge=lfs -text
|
||||
@@ -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
|
||||
|
||||
Reference in new issue
Block a user