//move the arm back to the top left corner
void Robot::relaxArm(BioloidController bioloid)
{
penUp();
bioloid.poseSize = 2;
bioloid.readPose();//find where the servos are currently
bioloid.setNextPose(1, relaxed[0]);
bioloid.setNextPose(2, relaxed[1]);
bioloid.interpolateSetup(500); // setup for interpolation from current->next
while(bioloid.interpolating > 0)
{
// do this while we have not reached our new pose
bioloid.interpolateStep(); // move servos, if necessary.
delay(3);
}
}
void Robot::drawLine(int points[4], BioloidController bioloid)
{
InverseKinematics(points);
bioloid.poseSize = 2; // load two poses in, one for each vertex
bioloid.readPose();//find where the servos are currently
penUp();
bioloid.setNextPose(1, points[0]); //set the coordinates for the vertex to which the arm is moving first
bioloid.setNextPose(2, points[1]);
bioloid.interpolateSetup(5000); // setup for interpolation from current->next
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}
penDown();
//bioloid.readPose();//find where the servos are currently
bioloid.setNextPose(1, points[2]);
bioloid.setNextPose(2, points[3]);
bioloid.interpolateSetup(1000); // setup for interpolation from current->next over 1/2 a second
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}relaxArm(bioloid);
}
void doIK(){
int servo;
ik_req_t req, gait;
ik_sol_t sol;
gaitSetup();
// right front leg
gait = gaitGen(RIGHT_FRONT);
req = bodyIK(endpoints[RIGHT_FRONT].x+gait.x, endpoints[RIGHT_FRONT].y+gait.y, endpoints[RIGHT_FRONT].z+gait.z, X_COXA, Y_COXA, gait.r);
sol = legIK(endpoints[RIGHT_FRONT].x+req.x+gait.x,endpoints[RIGHT_FRONT].y+req.y+gait.y,endpoints[RIGHT_FRONT].z+req.z+gait.z);
servo = 368 + sol.coxa;
if(servo < maxs[RF_COXA-1] && servo > mins[RF_COXA-1])
bioloid.setNextPose(RF_COXA, servo);
else{
Serial.print("RF_COXA FAIL: ");
Serial.println(servo);
}
servo = 524 + sol.femur;
if(servo < maxs[RF_FEMUR-1] && servo > mins[RF_FEMUR-1])
bioloid.setNextPose(RF_FEMUR, servo);
else{
Serial.print("RF_FEMUR FAIL: ");
Serial.println(servo);
}
servo = 354 + sol.tibia;
if(servo < maxs[RF_TIBIA-1] && servo > mins[RF_TIBIA-1])
bioloid.setNextPose(RF_TIBIA, servo);
else{
Serial.print("RF_TIBIA FAIL: ");
Serial.println(servo);
}
// right rear leg
gait = gaitGen(RIGHT_REAR);
req = bodyIK(endpoints[RIGHT_REAR].x+gait.x,endpoints[RIGHT_REAR].y+gait.y, endpoints[RIGHT_REAR].z+gait.z, -X_COXA, Y_COXA, gait.r);
sol = legIK(-endpoints[RIGHT_REAR].x-req.x-gait.x,endpoints[RIGHT_REAR].y+req.y+gait.y,endpoints[RIGHT_REAR].z+req.z+gait.z);
servo = 656 - sol.coxa;
if(servo < maxs[RR_COXA-1] && servo > mins[RR_COXA-1])
bioloid.setNextPose(RR_COXA, servo);
else{
Serial.print("RR_COXA FAIL: ");
Serial.println(servo);
}
servo = 524 + sol.femur;
if(servo < maxs[RR_FEMUR-1] && servo > mins[RR_FEMUR-1])
bioloid.setNextPose(RR_FEMUR, servo);
else{
Serial.print("RR_FEMUR FAIL: ");
Serial.println(servo);
}
servo = 354 + sol.tibia;
if(servo < maxs[RR_TIBIA-1] && servo > mins[RR_TIBIA-1])
bioloid.setNextPose(RR_TIBIA, servo);
else{
Serial.print("RR_TIBIA FAIL: ");
Serial.println(servo);
}
// left front leg
gait = gaitGen(LEFT_FRONT);
req = bodyIK(endpoints[LEFT_FRONT].x+gait.x,endpoints[LEFT_FRONT].y+gait.y, endpoints[LEFT_FRONT].z+gait.z, X_COXA, -Y_COXA, gait.r);
sol = legIK(endpoints[LEFT_FRONT].x+req.x+gait.x,-endpoints[LEFT_FRONT].y-req.y-gait.y,endpoints[LEFT_FRONT].z+req.z+gait.z);
servo = 656 - sol.coxa;
if(servo < maxs[LF_COXA-1] && servo > mins[LF_COXA-1])
bioloid.setNextPose(LF_COXA, servo);
else{
Serial.print("LF_COXA FAIL: ");
Serial.println(servo);
}
servo = 500 - sol.femur;
if(servo < maxs[LF_FEMUR-1] && servo > mins[LF_FEMUR-1])
bioloid.setNextPose(LF_FEMUR, servo);
else{
Serial.print("LF_FEMUR FAIL: ");
Serial.println(servo);
}
servo = 670 - sol.tibia;
if(servo < maxs[LF_TIBIA-1] && servo > mins[LF_TIBIA-1])
bioloid.setNextPose(LF_TIBIA, servo);
else{
Serial.print("LF_TIBIA FAIL: ");
Serial.println(servo);
}
// left rear leg
gait = gaitGen(LEFT_REAR);
req = bodyIK(endpoints[LEFT_REAR].x+gait.x,endpoints[LEFT_REAR].y+gait.y, endpoints[LEFT_REAR].z+gait.z, -X_COXA, -Y_COXA, gait.r);
sol = legIK(-endpoints[LEFT_REAR].x-req.x-gait.x,-endpoints[LEFT_REAR].y-req.y-gait.y,endpoints[LEFT_REAR].z+req.z+gait.z);
servo = 368 + sol.coxa;
if(servo < maxs[LR_COXA-1] && servo > mins[LR_COXA-1])
bioloid.setNextPose(LR_COXA, servo);
else{
Serial.print("LR_COXA FAIL: ");
Serial.println(servo);
}
servo = 500 - sol.femur;
if(servo < maxs[LR_FEMUR-1] && servo > mins[LR_FEMUR-1])
bioloid.setNextPose(LR_FEMUR, servo);
else{
Serial.print("LR_FEMUR FAIL: ");
Serial.println(servo);
}
servo = 670 - sol.tibia;
if(servo < maxs[LR_TIBIA-1] && servo > mins[LR_TIBIA-1])
bioloid.setNextPose(LR_TIBIA, servo);
else{
Serial.print("LR_TIBIA FAIL: ");
Serial.println(servo);
}
step = (step+1)%stepsInCycle;
}
