|
/*
|
|
Raydium - CQFD Corp.
|
|
http://raydium.org/
|
|
Released under both BSD license and Lesser GPL library license.
|
|
See "license.txt" file.
|
|
*/
|
|
|
|
// Small "in-game" model viewer, with mouse look. Use --model option.
|
|
|
|
#include "raydium/index.c"
|
|
|
|
GLfloat sun[]={1.0,0.9,0.5,1.0};
|
|
|
|
GLfloat camx=10;
|
|
GLfloat camy=10;
|
|
GLfloat camz=20;
|
|
|
|
GLfloat carx=0;
|
|
GLfloat cary=0;
|
|
GLfloat carz=0;
|
|
GLfloat carangle=0;
|
|
|
|
|
|
void create_drone(void)
|
|
{
|
|
|
|
int a; int joint1; int joint2; int joint3; int joint4;
|
|
/*faire apparaittre un objet*/
|
|
|
|
/*if(raydium_key_last==1000);*/
|
|
|
|
a=raydium_ode_object_create("Drone");
|
|
|
|
raydium_ode_object_box_add("drone",a,1,1,1,1,RAYDIUM_ODE_STANDARD,0,"drone.tri");
|
|
raydium_ode_object_move_name_3f("Drone",0,0,5);
|
|
|
|
|
|
raydium_ode_object_sphere_add("helice1",a,0.01,2,RAYDIUM_ODE_STANDARD,0,"helice1.tri");
|
|
raydium_ode_object_sphere_add("helice2",a,0.01,2,RAYDIUM_ODE_STANDARD,0,"helice1.tri");
|
|
raydium_ode_object_sphere_add("helice3",a,0.01,2,RAYDIUM_ODE_STANDARD,0,"helice1.tri");
|
|
raydium_ode_object_sphere_add("helice4",a,0.01,2,RAYDIUM_ODE_STANDARD,0,"helice1.tri");
|
|
|
|
raydium_ode_element_move_name_3f("helice1",5,4.5,6.7);
|
|
raydium_ode_element_move_name_3f("helice2",-5,4.5,6.7);
|
|
raydium_ode_element_move_name_3f("helice3",5,-4.5,6.7);
|
|
raydium_ode_element_move_name_3f("helice4",-5,-4.5,6.7);
|
|
|
|
joint1=raydium_ode_joint_attach_hinge_name("joint1","drone","helice1",5,4.5,6.7,0,0,1);
|
|
joint2=raydium_ode_joint_attach_hinge_name("joint2","drone","helice2",-5,4.5,6.7,0,0,1);
|
|
joint3=raydium_ode_joint_attach_hinge_name("joint3","drone","helice3",5,-4.5,6.7,0,0,1);
|
|
joint4=raydium_ode_joint_attach_hinge_name("joint4","drone","helice4",-5,-4.5,6.7,0,0,1);
|
|
|
|
// AVG =1 , AVD = 3 , ARG = 2 , ARD = 4
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void display(void)
|
|
{
|
|
float consigne, hdrone, xdrone, ydrone;
|
|
float k1, k2, Eps, force, Epsdev, fp, fd;
|
|
float force_1,force_2,force_3,force_4;
|
|
float k1_x,k2_x,Eps_x,Epsdev_x,angdev_x,angprop_x,ang_x; //var pour stabilisation ? un anngle sur x
|
|
float k1_y,k2_y,Eps_y,Epsdev_y,angdev_y,angprop_y,ang_y,consigne_y; //var pour stabilisation ? un anngle sur y
|
|
float k1_consigne_angle_x,k2_consigne_angle_x,consignedev_ang_x,consigneprop_ang_x, consigne_ang_x,Eps_consigne_x,Epsdev_consigne_x; //var pour changer la consigne d'angle en fonction de la position du drone sur axe x
|
|
static float EpsOld=0,EpsOld_x=0,EpsOld_y=0,EpsOld_consigne_x=0,consigne_x=0,consigne_pos_x=0;
|
|
dReal *pos, *pos1;
|
|
|
|
consigne=50;
|
|
|
|
//relev? position drone
|
|
pos=raydium_ode_element_pos_get_name("drone");
|
|
|
|
hdrone=pos[2];
|
|
ydrone=pos[1];
|
|
xdrone=pos[0];
|
|
|
|
|
|
//relv? des angles
|
|
|
|
dReal rotx, roty, rotz;
|
|
raydium_ode_element_eulerxyz_get_name("drone", &rotx, &roty, &rotz);
|
|
rotx = rotx/3.14159*180;
|
|
roty = roty/3.14159*180;
|
|
rotz = rotz/3.14159*180;
|
|
|
|
|
|
//appuyer sur F2 pour aveancer
|
|
if(raydium_key[GLUT_KEY_F2])
|
|
{
|
|
consigne_x=20;
|
|
pos1=raydium_ode_element_pos_get_name("drone");
|
|
consigne_pos_x=pos1[0];
|
|
}
|
|
else
|
|
{
|
|
Eps_consigne_x=xdrone-consigne_pos_x;
|
|
|
|
Epsdev_consigne_x=Eps_consigne_x-EpsOld_consigne_x;
|
|
|
|
k1_consigne_angle_x=2;
|
|
k2_consigne_angle_x=15;
|
|
|
|
consigneprop_ang_x=k1_consigne_angle_x*Eps_consigne_x;
|
|
consignedev_ang_x=k2_consigne_angle_x*Epsdev_consigne_x;
|
|
consigne_ang_x=consignedev_ang_x+consigneprop_ang_x;
|
|
consigne_ang_x=-consigne_ang_x;
|
|
|
|
|
|
if (-20<consigne_ang_x<20)
|
|
{consigne_x=consigne_ang_x;}
|
|
else
|
|
{
|
|
if(consigne_ang_x<-20)
|
|
{consigne_x=-20;}
|
|
else
|
|
{consigne_x=20;}
|
|
}
|
|
|
|
}
|
|
consigne_y=0;
|
|
|
|
|
|
|
|
if(raydium_key_last==1027) exit(0);
|
|
|
|
raydium_joy_key_emul(); /* Lance l'?milateur de joystick */
|
|
|
|
/* D?placement camera */
|
|
camx+=raydium_joy_x;
|
|
camy+=raydium_joy_y;
|
|
|
|
if(raydium_key[GLUT_KEY_PAGE_DOWN]) camz--;
|
|
if(raydium_key[GLUT_KEY_PAGE_UP] ) camz++;
|
|
|
|
//if(raydium_key_last==1032);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//application des forces sur chaque helices
|
|
|
|
|
|
//asservissement hauteur
|
|
Eps=consigne-hdrone;
|
|
|
|
Epsdev=Eps-EpsOld;
|
|
|
|
k1=5;
|
|
k2=25;
|
|
|
|
fp=k1*Eps;
|
|
fd=k2*Epsdev;
|
|
force=fp+fd;
|
|
|
|
|
|
|
|
//asservissement stabilit? x angle axe y
|
|
|
|
Eps_x=consigne_x-roty;
|
|
|
|
Epsdev_x=Eps_x-EpsOld_x;
|
|
|
|
k1_x=0.12;
|
|
k2_x=7;
|
|
|
|
angprop_x=k1_x*Eps_x;
|
|
angdev_x=k2_x*Epsdev_x;
|
|
ang_x=angprop_x+angdev_x;
|
|
|
|
|
|
//asservissement stabilit? y angle axe x
|
|
|
|
Eps_y=consigne_y-rotx;
|
|
|
|
Epsdev_y=Eps_y-EpsOld_y;
|
|
|
|
k1_y=0.12;
|
|
k2_y=7;
|
|
|
|
angprop_y=k1_y*Eps_y;
|
|
angdev_y=k2_y*Epsdev_y;
|
|
ang_y=angprop_y+angdev_y;
|
|
|
|
|
|
|
|
|
|
force_1=force;
|
|
force_2=force;
|
|
force_3=force;
|
|
force_4=force;
|
|
|
|
|
|
//if (force<0)
|
|
//force = 0;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//raydium_osd_printf(20,20,20,0.7,"font-lcdmono.tga","%f","%f","%f","%f",hdrone,rotx,roty,rotz);
|
|
raydium_osd_printf(20,20,20,0.7,"font-lcdmono.tga","%f",hdrone);
|
|
//raydium_log("hdrone=%f hhelice1=%f hhelice2=%f hhelice3=%f hhelice4=%f" ,hdrone,hhelice1,hhelice2,hhelice3,hhelice4);
|
|
//sraydium_log("rotx=%f roty=%f rotz=%f",rotx,roty,rotz);
|
|
raydium_log("Eps_consigne_x=%f Epsdev_consigne_x=%f ",Eps_consigne_x,Epsdev_consigne_x);
|
|
raydium_log("consigne_ang_x=%f consigneprop_ang_x=%f consignedev_ang_x=%f",consigne_ang_x,consigneprop_ang_x,consignedev_ang_x);
|
|
//raydium_log("force1=%f force2=%f force3=%f force4=%f",force_1,force_2,force_3,force_4);
|
|
//raydium_log("angdev=%f angprop=%f ang=%f ",angdev_y,angprop_y,ang_y);
|
|
raydium_log("pos x=%f pos y=%f ",xdrone,ydrone);
|
|
//if(hdrone<consigne)
|
|
//{
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//if(raydium_key[GLUT_KEY_F2]){
|
|
|
|
//raydium_ode_element_addrelforce_name_3f("helice1",0,0,force_1*1.1);
|
|
//raydium_ode_element_addrelforce_name_3f("helice2",0,0,force_2*0.9);
|
|
//raydium_ode_element_addrelforce_name_3f("helice3",0,0,force_3*1.1);
|
|
//raydium_ode_element_addrelforce_name_3f("helice4",0,0,force_4*0.9);
|
|
//}
|
|
//else{
|
|
raydium_ode_element_addrelforce_name_3f("helice1",0,0,force-ang_x);
|
|
raydium_ode_element_addrelforce_name_3f("helice2",0,0,force+ang_x);
|
|
raydium_ode_element_addrelforce_name_3f("helice3",0,0,force-ang_x);
|
|
raydium_ode_element_addrelforce_name_3f("helice4",0,0,force+ang_x);
|
|
|
|
//raydium_ode_element_addrelforce_name_3f("helice1",0,0,force-ang_x+ang_y);
|
|
//raydium_ode_element_addrelforce_name_3f("helice2",0,0,force+ang_x+ang_y);
|
|
//raydium_ode_element_addrelforce_name_3f("helice3",0,0,force-ang_x-ang_y);
|
|
//raydium_ode_element_addrelforce_name_3f("helice4",0,0,force+ang_x-ang_y);
|
|
|
|
EpsOld=Eps;
|
|
EpsOld_x=Eps_x;
|
|
EpsOld_y=Eps_y;
|
|
EpsOld_consigne_x=Eps_consigne_x;
|
|
|
|
raydium_rendering_finish();
|
|
|
|
raydium_clear_frame();
|
|
//raydium_camera_look_at(camx,camy,camz,0,0,50);
|
|
raydium_camera_freemove(RAYDIUM_CAMERA_FREEMOVE_NORMAL);
|
|
raydium_camera_freemove_speed=2;
|
|
|
|
/* affichage sol*/
|
|
/* raydium_object_draw_name("cocorobix.tri"); */
|
|
|
|
raydium_ode_draw_all(0);
|
|
if(raydium_key[GLUT_KEY_F1]) raydium_ode_draw_all(1);
|
|
|
|
/*
|
|
raydium_camera_replace();
|
|
glTranslatef(carx,cary,carz);
|
|
glRotatef(carangle,0,0,1);
|
|
raydium_object_draw_name("drone.tri");*/
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
raydium_init_args(argc,argv);
|
|
raydium_window_create(1280,960,RAYDIUM_RENDERING_WINDOW,"Mon premier test !");
|
|
/*raydium_window_create(1280,960,RAYDIUM_RENDERING_FULLSCREEN,"Mon premier test !");*/
|
|
raydium_texture_filter_change(RAYDIUM_TEXTURE_FILTER_TRILINEAR);
|
|
raydium_projection_near=0.01;
|
|
raydium_projection_far=1000;
|
|
raydium_projection_fov=60;
|
|
raydium_window_view_update();
|
|
raydium_light_on(0);
|
|
memcpy(raydium_light_color[0],sun,raydium_internal_size_vector_float_4);
|
|
raydium_light_intensity[0]=1000000;
|
|
|
|
raydium_light_position[0][0]=50;
|
|
raydium_light_position[0][1]=150;
|
|
raydium_light_position[0][2]=200;
|
|
|
|
raydium_light_update_all(0);
|
|
raydium_background_color_change(sun[0],sun[1],sun[2],sun[3]);
|
|
raydium_fog_disable();
|
|
|
|
/*cr?ation du sol*/
|
|
raydium_ode_ground_set_name("SOL1.tri");
|
|
|
|
/*appel de la fonction cr?ation d'objet drone*/
|
|
create_drone();
|
|
|
|
raydium_callback(&display);
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// EOF
|
|
|