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