To illustrate the use of the traces we set up seven traces: one for each leg that show the footholds used by each leg and another to show the path followed by the robot.
In this simple controller, we assume that the robot is reseted every time it is stopped.
We start the definition of the controller making a new controller directory in the simulator directory typing:
Then we have to edit the user.h and user.c files. In the user.h file we include the definition of some constants (step height, length, time to perform step movements,...) and of the t_user type. In this case, this type only contains one integer that represents the state (Sa to Sf).
In the user.c file, four functions have to be defined:
The contents of the user.h and user.c files is at the end of this section and they are provided with the simulator package so that you can test and modify them. For instance you can change the step length (AEP) to get a Follow the leader gait (you can validate whether or not a given AEP produces such a gait displaying the leg foothold traces). You can also try a negative AEP to see how the robot walks backward.
Once the controller is programmed, we have to configure the simulator so that it loads the correct robot, ground, and landmarks files at start up. This is done editing the sim.cfg file in the simulator root directory. In this case this file should contain:
This makes the simulator load the hexapod robot, an empty set of landmarks and a flat ground model (all these files are provided with the simulator).
Once this is done we only have to compile the controller typing
and execute it typingWhen the simulator starts, just push the Run button to see how the robot evolves under the control of the routines just described.
i :ii ii ii ii #ifndef CONTROLLERH
#define CONTROLLERH 1
#include "robot.h"
/*Movements*/
#define UP 0
#define ADVANCE 1
#define DOWN 2
/*step constants*/
/*height*/
#define HEIGHT 10
/*length (relative to the reference position)*/
#define AEP 15
/*Time to perform the movements*/
#define T_UP 5
#define T_ADVANCE 10
#define T_DOWN 5
/*States*/
#define Sa 0
#define Sb 1
#define Sc 2
#define Sd 3
#define Se 4
#define Sf 5
typedef struct {
unsigned int current_state;
} t_user;
void usr_init(t_robot *r,t_user *u);
void usr_reset(t_robot *r,t_user *u);
void usr_step(t_robot *r,t_user *u);
void usr_close(t_robot *r,t_user *u);
#endif
i :ii ii ii ii ii ii ii ii ii ii ii ii #include "user.h"
#include "transform.h" /*for TX,TY,... constants*/
#include "boolean.h" /*TRUE, FALSE,...*/
#include "math.h"
/*Private functions header*/
void advance_robot(t_robot *r,t_user *u);
void step_legs(t_robot *r,t_user *u);
/*Private functions definitions*/
void advance_robot(t_robot *r,t_user *u)
{
unsigned int i;
unsigned int n;
double b;
/*Only legs on the ground proppel the body
(i.e. are affected by the gesture to advance)*/
n=0;
for(i=0;i<6;i++)
{
if (touch_sensor(i,r))
{
gesture_effect(i,TRUE,r);
n++;
}
else
gesture_effect(i,FALSE,r);
}
b=get_balance(TX,r);
if (fabs(b)>1.0)
exec_gesture(TX,-b/n,r);
}
void step_legs(t_robot *r,t_user *u)
{
double pos[DIM_R3];
double ref[DIM_R3];
unsigned int set1[3],set2[3];
unsigned int mov;
unsigned int i;
/*See which is the movement to perform*/
switch(u->current_state)
{
case Sa:
case Sd:
mov=UP;
break;
case Sb:
case Se:
mov=ADVANCE;
break;
case Sc:
case Sf:
mov=DOWN;
break;
}
/*Select the set of legs that perform the step (set1)*/
/*and the set of legs that proppel the body (set2)*/
if ((u->current_state==Sa)||(u->current_state==Sb)||(u->current_state==Sc))
{set1[0]=0;set1[1]=3;set1[2]=4;
set2[0]=1;set2[1]=2;set2[2]=5;}
else
{set1[0]=1;set1[1]=2;set1[2]=5;
set2[0]=0;set2[1]=3;set2[2]=4;}
switch(mov)
{
case UP:
/*If the previous movement is finished*/
if ((position_achieved(set2[0],r))&&
(position_achieved(set2[1],r))&&
(position_achieved(set2[2],r)))
{
for(i=0;i<3;i++)
{
/*Before raising the leg, mark the foothold*/
get_world_position(set1[i],pos,r);
add_point(set1[i],pos,r);
/*If leg 0 is raised, mark the body position also*/
if (set1[i]==0)
{
trs_get_translation(robot_to_world(r),pos);
add_point(6,pos,r);
}
/*Raise the leg*/
get_position(set1[i],pos,r);
pos[AXIS_Z]+=HEIGHT;
move_to(set1[i],pos,T_UP,r);
}
u->current_state=(u->current_state+1)%6; /*Next State*/
}
break;
case ADVANCE:
/*If the previous movement is finished*/
if ((position_achieved(set1[0],r))&&
(position_achieved(set1[1],r))&&
(position_achieved(set1[2],r)))
{
for(i=0;i<3;i++)
{
/*Advance the leg AEP in front of the reference positon*/
get_position(set1[i],pos,r);
get_balance_reference(set1[i],ref,r);
pos[AXIS_X]=ref[AXIS_X]+AEP;
move_to(set1[i],pos,T_ADVANCE,r);
}
u->current_state=(u->current_state+1)%6; /*Next State*/
}
break;
case DOWN:
/*If the previous movement is finished*/
if ((position_achieved(set1[0],r))&&
(position_achieved(set1[1],r))&&
(position_achieved(set1[2],r)))
{
for(i=0;i<3;i++)
{
/*Down the leg*/
get_position(set1[i],pos,r);
pos[AXIS_Z]-=HEIGHT;
move_to(set1[i],pos,T_DOWN,r);
}
u->current_state=(u->current_state+1)%6; /*Next State*/
}
break;
}
}
/*public functions*/
void usr_init(t_robot *r,t_user *u)
{
unsigned int i;
char name[20];
/*Ensure six legs*/
if (number_of_legs(r)!=6)
exit(-1);
/*Start the traces (footholds of each leg and body path)*/
for(i=0;i<6;i++)
{
sprintf(name,"Leg %u Footholds",i);
start_trace(i,name,(double)i/5.0,1.0-(double)i/5.0,0.0,5.0,r);
}
start_trace(6,"Robot Path",0.0,0.0,0.0,10.0,r);
usr_reset(r,u);
}
void usr_reset(t_robot *r,t_user *u)
{
unsigned int i;
/*Re-Start the traces*/
for(i=0;i<7;i++)
restart_trace(i,r);
u->current_state=Sa;
}
void usr_step(t_robot *r,t_user *u)
{
step_legs(r,u);
advance_robot(r,u);
}
void usr_close(t_robot *r,t_user *u)
{
unsigned int i;
/*Delete the traces*/
for(i=0;i<7;i++)
delete_trace(i,r);
}
![\begin{figure}
\centerline{
\includegraphics [width=0.9\linewidth]{images/controller.eps}
}\end{figure}](img74.gif)