Page principale | Liste des namespaces | Hiérarchie des classes | Liste par ordre alphabétique | Liste des composants | Liste des fichiers | Composants | Déclarations

pi2d14_roue1.h

Aller à la documentation de ce fichier.
00001 #ifndef _TEST_ROUE_OLIVIER_H_
00002 #define _TEST_ROUE_OLIVIER_H_
00003 
00004 #include "pi2d14/pi2d14_maindeclaration.h"
00005 
00006 CR_DECLARE (TARGET_FPS);
00007 CR_DECLARE (GROUP_POSITION);
00008 CR_DECLARE (ATOM_POSITION);
00009 CR_DECLARE (GROUP_ATOM_COUNT);
00010 CR_DECLARE (GROUP_ATOM_PER_ROW);
00011 CR_DECLARE (GROUP_ATOM_PER_LINE);
00012 
00013 CR_DECLARE (KERNELSIDE);
00014 CR_DECLARE (SPHERESIDE);
00015 CR_DECLARE (LEGRADIUS);
00016 CR_DECLARE (LEGSIDE);
00017 
00018 #define NBR_ATOM        16
00019 
00020 API::Atom* test_Atoms[NBR_ATOM];
00021 dJointID atomsFixedWorld[NBR_ATOM];
00022 bool locked;
00023 
00024 void createRoue()
00025 {
00026         for(int i = 0; i < NBR_ATOM; i++)
00027         {
00028                 test_Atoms[i] = new API::Atom ();
00029         }
00030 
00031         double l1 = (DVAR(KERNELSIDE) + 2 * DVAR(LEGSIDE));
00032         double l2 = 2 * DVAR(LEGSIDE) + 0.017;
00033 
00034         test_Atoms[0]->util->setCartesianPosition(new API::CartesianPosition(0, 0, l2));
00035         test_Atoms[1]->util->setCartesianPosition(new API::CartesianPosition(0, l1, l2));
00036         test_Atoms[2]->util->setCartesianPosition(new API::CartesianPosition(l1, 0, l2));
00037         test_Atoms[3]->util->setCartesianPosition(new API::CartesianPosition(l1, l1, l2));
00038 
00039         test_Atoms[4]->util->setCartesianPosition(new API::CartesianPosition(l1 + l2, 0, l2 + l2));
00040         test_Atoms[5]->util->setCartesianPosition(new API::CartesianPosition(l1 + l2, l1, l2 + l2));
00041         test_Atoms[6]->util->setCartesianPosition(new API::CartesianPosition(l1 + l2, 0, l2 + l2 + l1));
00042         test_Atoms[7]->util->setCartesianPosition(new API::CartesianPosition(l1 + l2, l1, l2 + l2 + l1));
00043 
00044 
00045         test_Atoms[8]->util->setCartesianPosition(new API::CartesianPosition(0, 0, l2 + l2 + l1 + l2));
00046         test_Atoms[9]->util->setCartesianPosition(new API::CartesianPosition(0, l1, l2 + l2 + l1 + l2));
00047         test_Atoms[10]->util->setCartesianPosition(new API::CartesianPosition(l1, 0, l2 + l2 + l1 + l2));
00048         test_Atoms[11]->util->setCartesianPosition(new API::CartesianPosition(l1, l1, l2 + l2 + l1 + l2));
00049 
00050         test_Atoms[12]->util->setCartesianPosition(new API::CartesianPosition(-l2, 0, l2 + l2 + l1));
00051         test_Atoms[13]->util->setCartesianPosition(new API::CartesianPosition(-l2, l1, l2 + l2 + l1));
00052         test_Atoms[14]->util->setCartesianPosition(new API::CartesianPosition(-l2, 0, l2 + l2));
00053         test_Atoms[15]->util->setCartesianPosition(new API::CartesianPosition(-l2, l1, l2 + l2));
00054 
00055 }
00056 
00057 void createStep_1()
00058 {
00059         test_Atoms[0]->getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP);
00060         test_Atoms[1]->getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP);
00061         test_Atoms[2]->getLeg(0)->getServoMotor(0)->setTargetPosition(255,KEEP);
00062         test_Atoms[3]->getLeg(0)->getServoMotor(0)->setTargetPosition(255,KEEP);
00063 
00064         test_Atoms[4]->getLeg(2)->getServoMotor(1)->setTargetPosition(0,KEEP);
00065         test_Atoms[5]->getLeg(2)->getServoMotor(1)->setTargetPosition(0,KEEP);
00066         test_Atoms[6]->getLeg(3)->getServoMotor(1)->setTargetPosition(255,KEEP);
00067         test_Atoms[7]->getLeg(3)->getServoMotor(1)->setTargetPosition(255,KEEP);
00068 
00069         test_Atoms[8]->getLeg(1)->getServoMotor(0)->setTargetPosition(255,KEEP);
00070         test_Atoms[9]->getLeg(1)->getServoMotor(0)->setTargetPosition(255,KEEP);
00071         test_Atoms[10]->getLeg(0)->getServoMotor(0)->setTargetPosition(0,KEEP);
00072         test_Atoms[11]->getLeg(0)->getServoMotor(0)->setTargetPosition(0,KEEP);
00073 
00074         test_Atoms[12]->getLeg(3)->getServoMotor(1)->setTargetPosition(0,KEEP);
00075         test_Atoms[13]->getLeg(3)->getServoMotor(1)->setTargetPosition(0,KEEP);
00076         test_Atoms[14]->getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP);
00077         test_Atoms[15]->getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP);
00078 }
00079 
00080 void linkLegs()
00081 {
00082         for(int i = 0; i < NBR_ATOM; i++)
00083         {
00084                 API::Leg *tmpL1;
00085                 API::Leg *tmpL2;
00086 
00087                 for(int j = 0; j < 6; j++)
00088                 {
00089                         if(!test_Atoms[i]->getLeg(j)->isLinked())
00090                         {
00091                                 tmpL1 = test_Atoms[i]->getLeg(j);
00092                                 for(int k = 1; k < NBR_ATOM; k++)
00093                                 {
00094                                         for(int l = 0; l < 6; l++)
00095                                         {
00096                                                 if(!test_Atoms[k]->getLeg(l)->isLinked())
00097                                                 {
00098                                                         tmpL2 = test_Atoms[k]->getLeg(l);
00099                                                         if(i != k)
00100                                                         {
00101                                                                 if(tmpL1->util->isConnectable(tmpL2))
00102                                                                 {
00103                                                                         tmpL1->setLink(tmpL2);
00104                                                                 }
00105                                                         }
00106                                                 }
00107                                         }
00108                                 }
00109                         }
00110                 }
00111         }
00112 }
00113 
00114 void deleteRoue()
00115 {
00116         for(int i = 0; i < NBR_ATOM; i++)
00117         {
00118                 delete test_Atoms[i];
00119                 test_Atoms[i] = 0;
00120         }
00121 }
00122 
00123 void moveStep_1()
00124 {
00125         test_Atoms[0]->getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP);
00126         test_Atoms[1]->getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP);
00127 }
00128 
00129 void moveStep_2()
00130 {
00131         
00132 }
00133 
00134 void setJoint()
00135 {
00136         if (!locked)
00137         {
00138                 locked=true;
00139 
00140                 for(int i = 0; i < NBR_ATOM; i++)
00141                 {
00142                         if (atomsFixedWorld[i])
00143                         {
00144                                 dJointDestroy(atomsFixedWorld[i]);
00145                                 printf("**Atoms[%i] subit l'effet de la gravit�**\n",i);
00146                         }
00147                         else
00148                         {
00149                                 printf("**Atoms[%i] ne peut pas ne pas subir l'effet de la gravit�**\n",i);
00150                         }
00151                 }
00152         }
00153         else
00154         {
00155                 locked=false;
00156 
00157                 for(int i = 0; i < NBR_ATOM; i++)
00158                 {
00159                         atomsFixedWorld[i]= dJointCreateFixed (Simulator::getInstance ()->getWorld (), 0);
00160                         dJointAttach (atomsFixedWorld[i],0, test_Atoms[i]->util->getSimulatedObject()->getKernel()->getBody());
00161                         dJointSetFixed (atomsFixedWorld[i]);
00162                         printf("**Atoms[%i] ne subit plus l'effet de la gravite**\n",i);
00163                 }
00164         }
00165 }
00166 
00167 #endif

Généré le Fri Mar 26 13:02:03 2004 pour AlgoAtomD par doxygen 1.3.5