帮忙看一下这段程序，有什么问题
/** Auto-Generated by NUKE!
* http://arbotix.googlecode.com
*/
#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "nuke.h"
// Define one or the other depending upon which servo type you are using.
#define AX12_HEXAPOD
//#define AX18_HEXAPOD
Commander command = Commander();
int multiplier;
int id;
#define LEG_COUNT6
#define RIPPLE_SPEED 1
#define AMBLE_SPEED 3
#define TRIPOD_SPEED 5
/*Servo IDs */ /定义每条腿的关节舵机id/
#define RM_TIBIA 18
#define RF_COXA 2
#define LR_TIBIA 11
#define LF_FEMUR 3
#define RF_TIBIA 6
#define RM_FEMUR 16
#define RM_COXA 14
#define RR_COXA 8
#define LF_TIBIA 5
#define LF_COXA 1
#define LR_FEMUR 9
#define RR_FEMUR 10
#define LM_TIBIA 17
#define RF_FEMUR 4
#define LM_FEMUR 15
#define RR_TIBIA 12
#define LM_COXA 13
#define LR_COXA 7
/* Actual positions, and indices of array. */
extern ik_req_t endpoints;
#define RIGHT_FRONT 0
#define RIGHT_REAR 1
#define LEFT_FRONT 2
#define LEFT_REAR 3
#define RIGHT_MIDDLE 4
#define LEFT_MIDDLE 5
#ifdef AX12_HEXAPOD
#define TOP_SPEED 10
#endif
void setup(){
pinMode(0,OUTPUT);
// setup IK
setupIK();
gaitSelect(AMBLE_SMOOTH);
// setup serial
Serial.begin(38400);
// stand up slowly
bioloid.poseSize = 18;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
multiplier = AMBLE_SPEED;
}
void loop()
{
// take commands
if(command.buttons&BUT_L4) / /如果按下L4，选择左前腿//
{
LEFT_FRONT;
multiplier=AMBLE_SPEED;
Serial.println("####################");
Serial.println("Initializing Left_FRONTLeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 ); //左前腿向前移动100mm//
{
endpoints.x = endpoints.x+10;
endpoints.y = -118;
endpoints.z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) ); //左前腿向后移动100mm//
{
endpoints.x = endpoints.x -10;
endpoints.y = -118;
endpoints.z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 ); //左前腿向左移动100mm//
{
endpoints.x = 52;
endpoints.y = endpoints.y +10;
endpoints.z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) ); //左前腿向右移动100mm//
{
endpoints.x = 52;
endpoints.y = endpoints.y -10;
endpoints.z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 ); //左前腿抬高移动50mm//
{
endpoints.x = 52;
endpoints.y = -118;
endpoints.z = endpoints.z -50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) ); //左前腿降低移动50mm//
{
endpoints.x = 52;
endpoints.y = -118;
endpoints.z = endpoints.z+50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
}
页:
[1]