void Leg::moveTo(const Vector4 & point)
{
double x = point.x() - FBasePoint.x();
double y = point.y() - FBasePoint.y();
double z = point.z() - FBasePoint.z();
if (right()) x *= -1;
// Поворот плоскости ноги до целевой точки
double base_angle = atan2(y, x) * 180.0 / PI;
if (right()) base_angle *= -1;
// Координаты коленки в плоскости бедра и голени (относительно бедренного сустава)
double knee_x;
double knee_y;
// Координаты кончика ноги в той же системе
double x2 = sqrt(x*x + y*y) - FEMUR_LEDGE;
double y2 = -(point.z() - FEMUR_H);
if (intersectCircles(knee_x, knee_y, FEMUR_LENGTH, x2, y2, TIBIA_LENGTH))
{
double femur_angle = atan2(knee_y, knee_x) * 180.0 / PI;
double tibia_angle = femur_angle + atan2(-(y2 - knee_y), x2 - knee_x) * 180 / PI;
if (right())
{
femur_angle *= -1;
tibia_angle = 180 - tibia_angle;
}
setServoAngles(90 + base_angle, 90 + femur_angle, tibia_angle);
}
}
void hexapod::sit ()
{
int ii;
float target[3];
for (ii=0; ii<6; ii++)
{
target[0] = legpos[ii][0] + 10.0*cos(legang[ii]);
target[1] = legpos[ii][1] + 10.0*sin(legang[ii]);
target[2] = -5.0;
IKSolve(ii,target);
}
setServoAngles();
}
void hexapod::step (float dt)
{
int ii, modt;
float absspeed, speedsgn, ssfrac, ssfrac2;
float cycletime, xpos, ypos, zpos, target[3];
float legraise;
float turn_dist, thtpos, rpos, maxdist;
float dist, tht0, turn_step, speed_step;
// clamp speed and turning, just in case
hexlock.lock();
if (speed > MAX_SPEED) speed = MAX_SPEED;
if (speed < -MAX_SPEED) speed = -MAX_SPEED;
if (turning > 1.0) turning = 1.0;
if (turning < -1.0) turning = -1.0;
if (standheight < -2.0) standheight = -2.0;
if (standheight > 2.0) standheight = 2.0;
// make sure speed doesnt change too rapidly
speed_step = -(smoothspeed - speed);
if (speed_step > SPEED_SLEW*dt) speed_step = SPEED_SLEW*dt;
if (speed_step < -SPEED_SLEW*dt) speed_step = -SPEED_SLEW*dt;
smoothspeed += speed_step;
// cap the rate at which turning can change
turn_step = -(smoothturning - turning);
if (turn_step > TURN_SLEW*dt) turn_step = TURN_SLEW*dt;
if (turn_step < -TURN_SLEW*dt) turn_step = -TURN_SLEW*dt;
smoothturning += turn_step;
hexlock.unlock();
// to control walking, modify speed and turning
absspeed = fabs(smoothspeed);
speedsgn = 1.0;
if (smoothspeed < 0.0) speedsgn = -1.0;
// walking speed is influenced by leg sweep and movement speed
legraise = 1.0;
if (absspeed < 0.05)
legraise = absspeed/0.05;
if (absspeed < 0.2)
{
sweepmodifier = absspeed*0.8/0.2;
speedmodifier = 0.25;
} else if (absspeed < 0.8) {
sweepmodifier = 0.8;
speedmodifier = absspeed/sweepmodifier;
} else if (absspeed < 1.0) {
sweepmodifier = absspeed;
speedmodifier = 1.0;
} else {
sweepmodifier = 1.0;
speedmodifier = absspeed;
}
speedmodifier *= speedsgn;
if (ssrunning)
{
sstime += dt;
ssfrac = sstime/SS_DURATION;
for (ii=0; ii<6; ii++)
{
// compute final target
target[0] = legpos[ii][0]*1.5;
if (ii == 0 || ii == 2) target[1] = 14.0;
if (ii == 1) target[1] = 18.0;
if (ii == 3 || ii == 5) target[1] = -14.0;
if (ii == 4) target[1] = -18.0;
target[0] = legpos1[ii][0];
target[1] = legpos1[ii][1];
target[2] = -10. + standheight;
// given final target, turn into current target
if (ssfrac < 0.5)
{
ssfrac2 = ssfrac*2.0;
if (ii % 2 == 0)
{
target[0] = ssx0[ii] + ssfrac2*(target[0]-ssx0[ii]);
target[1] = ssy0[ii] + ssfrac2*(target[1]-ssy0[ii]);
target[2] = ssz0[ii] + ssfrac2*(target[2]-ssz0[ii]) +
2.0*pow(sin(3.1416*ssfrac2),2);
} else {
target[0] = ssx0[ii];
target[1] = ssy0[ii];
target[2] = ssz0[ii];
}
} else {
ssfrac2 = (ssfrac-0.5)*2.0;
if (ii % 2 == 0)
{
// don't modify targets
} else {
target[0] = ssx0[ii] + ssfrac2*(target[0]-ssx0[ii]);
target[1] = ssy0[ii] + ssfrac2*(target[1]-ssy0[ii]);
target[2] = ssz0[ii] + ssfrac2*(target[2]-ssz0[ii]) +
2.0*pow(sin(3.1416*ssfrac2),2);
}
}
IKSolve(ii,target);
}
if (sstime > SS_DURATION)
{
ssrunning = false;
sstime = 0.0;
}
} else {
// based on current turning, compute turning math
if (fabs(smoothturning) <= TURN_TOL)
turn_dist = tan((1.0-TURN_TOL)*3.1416/2.0)*50.;
else if (fabs(smoothturning) > TURN_TOL)
turn_dist = tan((1.0-smoothturning)*3.1416/2.0)*50.;
// compute dist between turn_dist and farthest leg
maxdist = 0.0;
for (ii=0; ii<6; ii++)
{
dist = sqrt(pow(legpos1[ii][0],2) + pow(legpos1[ii][1]-turn_dist,2));
if (dist > maxdist) maxdist = dist;
}
// each leg can only sweep so much, so use this farthest leg
// to determine the angle of sweep that every leg must do
maxsweep = 8.*sweepmodifier/maxdist;
if (turn_dist < 0.0) maxsweep = -maxsweep;
// maxsweep is the angle of sweep for every leg, in radians
// increment dead-reckoning position
// turning radius is "turn_dist"
dr_ang += dt*speedmodifier*maxsweep*2.0*0.83775/fdf;
if (dr_ang > PI) dr_ang -= 2.*PI;
if (dr_ang < -PI) dr_ang += 2.*PI;
dr_xpos += maxsweep*turn_dist*dt*speedmodifier*2.0*cos(dr_ang)*0.83775/fdf;
dr_ypos += maxsweep*turn_dist*dt*speedmodifier*2.0*sin(dr_ang)*0.83775/fdf;
// dr_ang has about 20% error, likely systematic
// dr_xpos,dr_ypos have more like 5% error, also likely systematic
// increment fake time
time += dt*speedmodifier;
if (time > 1.0) time -= 1.0;
if (time < 0.0) time += 1.0;
// loop through each leg to figure out where it should be right now
for (ii=0; ii<6; ii++)
{
// where is this leg in the cycle of stepping?
// the 0.5*ii is to completely de-sync legs
// the other part is to adjust it more
cycletime = fmod(time + 0.5*ii + (legpos[ii][0]-legpos[0][0])*0.0125, 1.0);
// use bezier curve to either be up or down
// b2d_walk_down goes between +/- 0.83775
if (cycletime < fdf) b2d_walk_down.getPos(cycletime/fdf, &thtpos, &zpos);
else b2d_walk_up.getPos((cycletime-fdf)/(1.-fdf), &thtpos, &zpos);
// convert thtpos into angle?
thtpos *= maxsweep;
// convert rpos to xpos,ypos
dist = sqrt(pow(legpos1[ii][0],2) + pow(legpos1[ii][1]-turn_dist,2));
tht0 = atan2(legpos1[ii][1]-turn_dist,legpos1[ii][0]);
xpos = dist*cos(thtpos+tht0);
ypos = turn_dist + dist*sin(thtpos+tht0);
// set up the IK target
target[0] = xpos;
target[1] = ypos;
target[2] = -10.0 + zpos*3.0*legraise + standheight;
if (standheight < 0.0) target[2] -= 1.7*zpos*standheight;
// perform IK solve
IKSolve(ii,target);
// TODO: add error handling if IK fails to converge
}
}
setServoAngles();
}
