急！帮帮我
哪位高手帮帮我，告诉我哪里出错了，谢谢！这是一个在webots中可以走的机器狗的程序，因为是由我改动的，根本运行不通，原本是用java的代码，让我改成c语言，机器狗连动都不动，帮帮我！
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <device/robot.h>
#include <device/servo.h>
#define SIMULATION_STEP 32
/*
* You may want to add defines macro here.
*/
static void die(void);
static void reset(void);
static int run(int);
static float sin_oscillation(float,float,float,float);
static void walk(float);
static int LEFT_FORE=0,
LEFT_HIND=1,
RIGHT_FORE=2,
RIGHT_HIND=3;
static int SWING=0, // J1 joint
FLAP=1, // J2 joint
KNEE=2; // J3 joint
// ears
static int LEFT=0,
RIGHT=1;
// Device tags
static int leg_servo[4][3],
head_pan_servo,
head_tilt1_servo,
head_tilt2_servo,
mouth_servo,
ear_servo[2],
emitter;
static float leg_servo_accumulator[4][3]={{0,0,0},{0,0,0},{0,0,0},{0,0,0}};
static int leg_servo_counter[4][3]={{0,0,0},{0,0,0},{0,0,0},{0,0,0}};
static float time = 0.0f;
static void die(void) {
robot_console_print("die method called");
}
static void reset(void) {
leg_servo[LEFT_FORE][SWING] = robot_get_device("PRM:/r2/c1-Joint2:21");
leg_servo[LEFT_FORE][FLAP] = robot_get_device("PRM:/r2/c1/c2-Joint2:22");
leg_servo[LEFT_FORE][KNEE] = robot_get_device("PRM:/r2/c1/c2/c3-Joint2:23");
leg_servo[LEFT_HIND][SWING] = robot_get_device("PRM:/r3/c1-Joint2:31");
leg_servo[LEFT_HIND][FLAP] = robot_get_device("PRM:/r3/c1/c2-Joint2:32");
leg_servo[LEFT_HIND][KNEE] = robot_get_device("PRM:/r3/c1/c2/c3-Joint2:33");
leg_servo[RIGHT_FORE][SWING]= robot_get_device("PRM:/r4/c1-Joint2:41");
leg_servo[RIGHT_FORE][FLAP] = robot_get_device("PRM:/r4/c1/c2-Joint2:42");
leg_servo[RIGHT_FORE][KNEE] = robot_get_device("PRM:/r4/c1/c2/c3-Joint2:43");
leg_servo[RIGHT_HIND][SWING]= robot_get_device("PRM:/r5/c1-Joint2:51");
leg_servo[RIGHT_HIND][FLAP] = robot_get_device("PRM:/r5/c1/c2-Joint2:52");
leg_servo[RIGHT_HIND][KNEE] = robot_get_device("PRM:/r5/c1/c2/c3-Joint2:53");
head_tilt1_servo = robot_get_device("PRM:/r1/c1-Joint2:11");
head_pan_servo = robot_get_device("PRM:/r1/c1/c2-Joint2:12");
head_tilt2_servo = robot_get_device("PRM:/r1/c1/c2/c3-Joint2:13");
mouth_servo = robot_get_device("PRM:/r1/c1/c2/c3/c4-Joint2:14");
ear_servo[LEFT] = robot_get_device("PRM:/r1/c1/c2/c3/e5-Joint4:15");
ear_servo[RIGHT] = robot_get_device("PRM:/r1/c1/c2/c3/e6-Joint4:16");
emitter = robot_get_device("emitter");
}
static float sin_oscillation(float target,float period,
float amplitude,float phase) {
float v = target +
amplitude * (float)Math.sin(phase + 2*Math.PI*time/period);
v *= Math.PI/180.0; // convert degrees to radians
return v;
}
static void walk(float speed) {
float coef[]={1f,0.5f}; // periods
float mean[][]={{-17.36f,9.96f,80.5f},
{-28.8f,11.1f,90.3f}}; // mean values
// amp and phase: [front/back][joint][sinnumber]
float amp[][][]={{{14.63f,2.49f},{2.20f,0.88f},{13.50f,5.31f}},
{{14.65f,3.38f},{2.35f,1.05f},{11.56f,5.27f}}};
float phase[][][]={{{1.81f,-1.09f},{-2.33f,1.78f},{-1.73f,1.26f}},
{{0.16f,-1.09f},{-2.55f,2.01f},{-2.55f,2.75f}}};
float phase4[][][][]=new float[2][2][3][2];
float phasemod=1; // -1 if changing direction
int x,y,j,d;
for(x=0;x<2;x++) for(y=0;y<2;y++) for(j=0;j<3;j++) for(d=0;d<2;d++)
phase4[x][y][j][d]=phase[x][j][d]+(float)Math.PI/2; // sin and not cos
// front-back shift
for(x=0;x<2;x++) for(y=0;y<2;y++) for(j=0;j<3;j++) for(d=0;d<2;d++)
phase4[x][y][j][d]-=phase[x][0][0] * (d+1);
// left-right phase shift
for(x=0;x<2;x++) for(j=0;j<3;j++)
phase4[x][1][j][0]+=Math.PI;
for(x=0;x<2;x++) for(y=0;y<2;y++) for(j=0;j<3;j++) {
for(d=0;d<2;d++) {
leg_servo_accumulator[x+y*2][j]+=
sin_oscillation(mean[x][j], // target
speed*coef[d], // period
amp[x][j][d]*4, // amplitude
(float)(phasemod*phase4[x][y][j][d]+ // ...
Math.PI*(phasemod-1)/2) // phase
);
}
leg_servo_counter[x+y*2][j]+=2;
}
}
static int run(int ms)
{
for(int l=0;l<4;l++) for(int j=0;j<3;j++) if (leg_servo_counter[l][j]!=0) {
servo_set_position(leg_servo[l][j],
leg_servo_accumulator[l][j]/leg_servo_counter[l][j]);
leg_servo_accumulator[l][j]=0.0f;
leg_servo_counter[l][j]=0;
}
time+=(float)SIMULATION_STEP/1000f;
return SIMULATION_STEP; /* this is the time step value, in milliseconds. */
}
/*
* This is the main program which sets up the reset and run function.
*/
int main()
{
robot_live(reset);/* initialization */
robot_die(die);
robot_run(run); /* starts the controller */
walk(1);
return 0;
}
