|
|
我写的六足机器人程序,想实现每个按键作为控制一条腿的命令,大家帮忙看一下有什么问题,程序编译也没有问题,就是机器人没有按照我写的要求动作
/*
* 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_COUNT 6
#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[LEG_COUNT];
#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[RIGHT_FRONT].x = endpoints[RIGHT_FRONT].x +10;
endpoints[RIGHT_FRONT].y = 118;
endpoints[RIGHT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[RIGHT_FRONT].x = endpoints[RIGHT_FRONT].x -10;
endpoints[RIGHT_FRONT].y = 118;
endpoints[RIGHT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[RIGHT_FRONT].x = 52;
endpoints[RIGHT_FRONT].y = endpoints[RIGHT_FRONT].y +10;
endpoints[RIGHT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[RIGHT_FRONT].x = 52;
endpoints[RIGHT_FRONT].y = endpoints[RIGHT_FRONT].y -10;
endpoints[RIGHT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[RIGHT_FRONT].x = 52;
endpoints[RIGHT_FRONT].y = 118;
endpoints[RIGHT_FRONT].z = endpoints[RIGHT_FRONT].z -50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[RIGHT_FRONT].x = 52;
endpoints[RIGHT_FRONT].y = 118;
endpoints[RIGHT_FRONT].z = endpoints[RIGHT_FRONT].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[RIGHT_MIDDLE].x = endpoints[RIGHT_MIDDLE].x +10;
endpoints[RIGHT_MIDDLE].y = 118;
endpoints[RIGHT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[RIGHT_MIDDLE].x = endpoints[RIGHT_MIDDLE].x -10;
endpoints[RIGHT_MIDDLE].y = 118;
endpoints[RIGHT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[RIGHT_MIDDLE].x = 0;
endpoints[RIGHT_MIDDLE].y = endpoints[RIGHT_MIDDLE].y +10;
endpoints[RIGHT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[RIGHT_MIDDLE].x = 0;
endpoints[RIGHT_MIDDLE].y = endpoints[RIGHT_MIDDLE].y -10;
endpoints[RIGHT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[RIGHT_MIDDLE].x = 0;
endpoints[RIGHT_MIDDLE].y = 118;
endpoints[RIGHT_MIDDLE].z = endpoints[RIGHT_MIDDLE].z -50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[RIGHT_MIDDLE].x = 0;
endpoints[RIGHT_MIDDLE].y = 118;
endpoints[RIGHT_MIDDLE].z = endpoints[RIGHT_MIDDLE].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[RIGHT_REAR].x =endpoints[RIGHT_REAR].x +10;
endpoints[RIGHT_REAR].y = 118;
endpoints[RIGHT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[RIGHT_REAR].x = endpoints[RIGHT_REAR].x -10;
endpoints[RIGHT_REAR].y = 118;
endpoints[RIGHT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[RIGHT_REAR].x = -52;
endpoints[RIGHT_REAR].y = endpoints[RIGHT_REAR].y +10;
endpoints[RIGHT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[RIGHT_REAR].x = -52;
endpoints[RIGHT_REAR].y = endpoints[RIGHT_REAR].y -10;
endpoints[RIGHT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[RIGHT_REAR].x = -52;
endpoints[RIGHT_REAR].y = 118;
endpoints[RIGHT_REAR].z = endpoints[RIGHT_REAR].z -50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[RIGHT_REAR].x = -52;
endpoints[RIGHT_REAR].y = 118;
endpoints[RIGHT_REAR].z = endpoints[RIGHT_REAR].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[LEFT_FRONT].x = endpoints[LEFT_FRONT].x+10;
endpoints[LEFT_FRONT].y = -118;
endpoints[LEFT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[LEFT_FRONT].x = endpoints[LEFT_FRONT].x -10;
endpoints[LEFT_FRONT].y = -118;
endpoints[LEFT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[LEFT_FRONT].x = 52;
endpoints[LEFT_FRONT].y = endpoints[LEFT_FRONT].y +10;
endpoints[LEFT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[LEFT_FRONT].x = 52;
endpoints[LEFT_FRONT].y = endpoints[LEFT_FRONT].y -10;
endpoints[LEFT_FRONT].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[LEFT_FRONT].x = 52;
endpoints[LEFT_FRONT].y = -118;
endpoints[LEFT_FRONT].z = endpoints[LEFT_FRONT].z -50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[LEFT_FRONT].x = 52;
endpoints[LEFT_FRONT].y = -118;
endpoints[LEFT_FRONT].z = endpoints[LEFT_FRONT].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[LEFT_MIDDLE].x = endpoints[LEFT_MIDDLE].x +10;
endpoints[LEFT_MIDDLE].y = -118;
endpoints[LEFT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[LEFT_MIDDLE].x = endpoints[LEFT_MIDDLE].x-10;
endpoints[LEFT_MIDDLE].y = -118;
endpoints[LEFT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[LEFT_MIDDLE].x = 0;
endpoints[LEFT_MIDDLE].y = endpoints[LEFT_MIDDLE].y +10;
endpoints[LEFT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[LEFT_MIDDLE].x = 0;
endpoints[LEFT_MIDDLE].y = endpoints[LEFT_MIDDLE].y-10;
endpoints[LEFT_MIDDLE].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[LEFT_MIDDLE].x = 0;
endpoints[LEFT_MIDDLE].y = -118;
endpoints[LEFT_MIDDLE].z = endpoints[LEFT_MIDDLE].z-50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[LEFT_MIDDLE].x = 0;
endpoints[LEFT_MIDDLE].y = -118;
endpoints[LEFT_MIDDLE].z = endpoints[LEFT_MIDDLE].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[LEFT_REAR].x = endpoints[LEFT_REAR].x+10;
endpoints[LEFT_REAR].y = -118;
endpoints[LEFT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkV < -5) );
{
endpoints[LEFT_REAR].x = endpoints[LEFT_REAR].x-10;
endpoints[LEFT_REAR].y = -118;
endpoints[LEFT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH) > 5 );
{
endpoints[LEFT_REAR].x = 0;
endpoints[LEFT_REAR].y = endpoints[LEFT_REAR].y+10;
endpoints[LEFT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.walkH < -5) );
{
endpoints[LEFT_REAR].x = 0;
endpoints[LEFT_REAR].y = endpoints[LEFT_REAR].y-10;
endpoints[LEFT_REAR].z = 97;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH) > 5 );
{
endpoints[LEFT_REAR].x = 0;
endpoints[LEFT_REAR].y = -118;
endpoints[LEFT_REAR].z = endpoints[LEFT_REAR].z-50;
bioloid.poseSize = 3;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}
if((command.lookH < -5) );
{
endpoints[LEFT_REAR].x = 0;
endpoints[LEFT_REAR].y = -118;
endpoints[LEFT_REAR].z = endpoints[LEFT_REAR].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
|
|