mangavalk
fe472bfce9
From Electronics\Hexapod\Arduino sketches 2011-2012\hexapod_oud (banner 'Version 3.0').
92 lines
1.6 KiB
C++
92 lines
1.6 KiB
C++
// 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];
|
|
}*/
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|