我自己写的一段控制六足机器人的程序,不能正确运行
我写的六足机器人程序,想实现每个按键作为控制一条腿的命令,大家帮忙看一下有什么问题,程序编译也没有问题,就是机器人没有按照我写的要求动作/*
* 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 */
#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
#ifdef AX18_HEXAPOD
#define TOP_SPEED 12
#endif
void setup(){
pinMode(0,OUTPUT);
// setup IK
setupIK();
gaitSelect(AMBLE_SMOOTH);
// setup serial
Serial.begin(38400);
// wait, then check the voltage (LiPO safety)
// delay (1000);
// float voltage = (ax12GetRegister (1, AX_PRESENT_VOLTAGE, 1)) / 10.0;
//Serial.print ("System Voltage: ");
// Serial.print (voltage);
//Serial.println (" volts.");
//if (voltage < 10.0)
//while(1);
// 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.ReadMsgs() > 0){
digitalWrite(0,HIGH-digitalRead(0));
// select legs
if(command.buttons&BUT_R1)
{
RIGHT_FRONT;
multiplier=AMBLE_SPEED;
Serial.println("####################");
Serial.println("Initializing RIGHT_FRONTLeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
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) );
{
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 );
{
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) );
{
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.buttons&BUT_R2);
{
RIGHT_MIDDLE;
multiplier=AMBLE_SPEED;
Serial.println("####################");
Serial.println("Initializing RIGHT_MIDDLELeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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.buttons&BUT_R3);
{
RIGHT_REAR;
multiplier=AMBLE_SPEED;
Serial.println("####################");
Serial.println("Initializing RIGHT_REARLeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
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) );
{
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 );
{
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) );
{
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.buttons&BUT_L4)
{
LEFT_FRONT;
multiplier=AMBLE_SPEED;
Serial.println("####################");
Serial.println("Initializing Left_FRONTLeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
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) );
{
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 );
{
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) );
{
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.buttons&BUT_L5)
{
LEFT_MIDDLE;
multiplier=TRIPOD_SPEED;
Serial.println("####################");
Serial.println("Initializing Left_MIDDLELeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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.buttons&BUT_L6){
LEFT_REAR;
multiplier=TOP_SPEED;
Serial.println("####################");
Serial.println("Initializing Left_REARLeg Tests");
Serial.println("####################");
delay(500);
if((command.walkV) > 5 );
{
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) );
{
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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 );
{
endpoints.x = 0;
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) );
{
endpoints.x = 0;
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);
}
}
}
}
}
// set select leg
// if((command.walkV) > 5 || (command.walkV < -5) )
//{
// Xspeed = (multiplier*command.walkV)/2;
// }
//else
//{
//Xspeed = 0;
// }
// if((command.walkH) > 5 || (command.walkH < -5) ){
// Yspeed = (multiplier*command.walkH)/2;
// }
// else
// {
// Yspeed = 0;
// }
// if((command.lookH) > 5 || (command.lookH < -5) ){
// Rspeed = -(command.lookH)/100.0;
// }
// else
// {
//Rspeed = 0;
// }
// Use the phoenix code if you want pretty body rotation. :)
//
// if((command.buttons&BUT_LT) > 0){
// bodyRotY = (((float)command.lookV))/300.0;
// bodyRotZ = ((float)command.lookH)/300.0;
// bodyRotX = ((float)command.walkH)/300.0;
// Rspeed = 0;
// Xspeed = 0;
// Yspeed = 0;
// }
//}
// if our previous interpolation is complete, recompute the IK
仔细检查，如果给别人看，添加注释 好的,谢谢,我把注释加上 樓主在玩的, 是 Bioloid 的套件機械人嗎?
是的,由18个AX-12的舵机组成的一个六足机器人 yuanfangssj 发表于 2015-1-14 11:39 static/image/common/back.gif
是的,由18个AX-12的舵机组成的一个六足机器人
18 個 AX-12 舵機, 用的是 ArbotiX RoboController, 真的不簡單.應該不會便宜吧.
這樣高級的配置, 廠家應該有一定的支援吧. 没有，是直接从美国代购买回来的，一万多吧。只有网站上的一些原demo，我现在想实现单腿控制模式，就写了上面那个程序，结果没调试同呢，愁啊
页:
[1]