public static void depart(){
   servo_set_position(joint[5],0.5f);
   servo_set_position(joint[15],0.5f);

   servo_set_position(joint[0],0.5f);
System.out.println(servo_get_position(joint[0]));

   servo_set_position(joint[6],0.2f);
   servo_set_position(joint[16],0.2f);
 }
public static void marche () {
    int cpt=0;
    int i;
    
    robot_step(1500);
    while (cpt <5){
	depart();
	
	cpt++;
    }
    
    
    robot_step(500);
    // Le robot lève la jambe droite
    while (cpt<20) {
	servo_set_position(joint[1],-0.8f);
	servo_set_velocity(joint[1],0.8f);
	
	servo_set_position(joint[7],-0.1f);
	servo_set_velocity(joint[7],0.8f);
	
	servo_set_position(joint[13],0.5f);
	servo_set_velocity(joint[13],0.3f);
	
	robot_step(150);   
	cpt++;
    }
        
    float p1 = servo_get_position(joint[7]);
       
    // Le robot pose le pied droit
    if (p1 == servo_get_position(joint[7])){
	servo_set_position(joint[1],0.1f);
	servo_set_velocity(joint[1],0.8f);
	
	servo_set_position(joint[7],0f);
	servo_set_velocity(joint[7],0.5f);
	
	servo_set_position(joint[0],0.5f);
	servo_set_velocity(joint[0],0.5f);
	
	servo_set_position(joint[6],0.5f);
	servo_set_velocity(joint[6],0.5f);
	
	servo_set_position(joint[16],0f);
	servo_set_velocity(joint[16],0.5f);
	
	servo_set_position(joint[12],-0.1f);
	servo_set_velocity(joint[12],0.5f); 
	
	
	cpt++;
    }
    
    robot_step(500);
    float p2 = servo_get_position(joint[0]);
    
    //le robot se met en équilibre sur la jambe droite
    if (cpt==21 && p2 == servo_get_position(joint[0])){
	servo_set_position(joint[1],0.8f);
	servo_set_velocity(joint[1],0.8f); 
	
	servo_set_position(joint[0],0.8f);
	servo_set_velocity(joint[0],0.5f);
	
	servo_set_position(joint[17],-0.1f);
	servo_set_velocity(joint[17],0.3f); 
	
	servo_set_position(joint[16],0.2f);
	servo_set_velocity(joint[16],0.3f); 
	
	cpt++;
    }
    
    robot_step(500);
    float p3 = servo_get_position(joint[16]);
    
    // Le robot bouge la jambe gauche
    if (cpt==22 && p3 == servo_get_position(joint[16])){
	servo_set_position(joint[3],1f);
	servo_set_velocity(joint[3],0.6f);
	
	servo_set_position(joint[0],0f);
	servo_set_velocity(joint[0],0.5f);
	
	servo_set_position(joint[1],0.2f);
	servo_set_velocity(joint[1],0.8f); 
	
	servo_set_position(joint[6],0f);
	servo_set_velocity(joint[6],0.3f);
	
	cpt++;
    }
    
    robot_step(2000);
    float p4 = servo_get_position(joint[3]);
    
    
    // Le robot pose le pied gauche
    if (cpt==23 && p4 == servo_get_position(joint[3])){
	servo_set_position(joint[1],0.05f);
	servo_set_velocity(joint[1],0.8f);
	
	servo_set_position(joint[17],0f);
	servo_set_velocity(joint[17],0.5f);
	
	servo_set_position(joint[0],0f);
	servo_set_velocity(joint[0],0.5f);
	
	servo_set_position(joint[16],0.5f);
	servo_set_velocity(joint[16],0.5f);
	
	servo_set_position(joint[6],0f);
	servo_set_velocity(joint[6],0.5f);
	
	cpt++;
    }
    
    robot_step(1500);
    float p5 = servo_get_position(joint[1]);
    
    //le robot se met en équilibre sur la jambe gauche
    if (cpt==24 && p5 == servo_get_position(joint[1])){
	servo_set_position(joint[1],-0.9f);
	servo_set_velocity(joint[1],0.8f); 
	
	servo_set_position(joint[0],0f);
	servo_set_velocity(joint[0],0.5f); 
	
	servo_set_position(joint[6],0.2f);
	servo_set_velocity(joint[6],0.3f);
	
	servo_set_position(joint[7],-0.08f);
	servo_set_velocity(joint[7],0.3f);
	cpt++;
    }
    
    robot_step(1500);
    float p6 = servo_get_position(joint[6]);
    
    if (cpt==25 && p6 == servo_get_position(joint[6])){
	servo_set_position(joint[13],1f);
	servo_set_velocity(joint[13],0.6f);
	
	servo_set_position(joint[0],0f);
	servo_set_velocity(joint[0],0.5f);
	
	servo_set_position(joint[1],-0.25f);
	servo_set_velocity(joint[1],0.3f); 
	
	servo_set_position(joint[16],0f);
	servo_set_velocity(joint[16],0.3f);
	
	servo_set_position(joint[12],-0.2f);
	servo_set_velocity(joint[12],0.3f);
	
	//servo_set_position(joint[2],0.3f);
	//servo_set_velocity(joint[2],0.3f);
	
	servo_set_position(joint[6],0f);
	servo_set_velocity(joint[6],0.3f);
	
	cpt++;
}
    
    robot_step(1500);
    float p7 = servo_get_position(joint[13]);
    if (cpt==26 && p7 == servo_get_position(joint[13]) ) {
	for(i=0;i<22;i++){
	    servo_set_position(joint[i],0);
	    servo_set_velocity(joint[i],0.8f);
	}
    }
}