00001
00002 #include <iostream>
00003
00004 #include "pi2d14/pi2d14_molecule_ligne.h"
00005
00006 #include "Atom.h"
00007 #include "ConfigReader.h"
00008 #include "Math.h"
00009
00010
00011 ALGOATOMD::MoleculeLigne::MoleculeLigne(int buildParam, int typeDep)
00012 {
00013 if(buildParam<1)
00014 {
00015 buildParam=1;
00016 }
00017 size = 2 + buildParam;
00018
00019 m1=false;
00020 m2=false;
00021 m3=false;
00022 m4=false;
00023 m5=false;
00024 indexInit=-1;
00025 index1=-1;
00026 index2=-1;
00027 index3=-1;
00028 index4=-1;
00029 index5=-1;
00030
00031 atoms = new API::Atom[size];
00032 for(int i = 0; i < size; i++)
00033 {
00034 atoms[i].util->setCartesianPosition(new API::CartesianPosition(0, 2*atomSize*i, atomSize));
00035 }
00036
00037 for(int i = 0; i < size - 1; i++)
00038 {
00039 API::Leg *tmpL1;
00040 API::Leg *tmpL2;
00041
00042 for(int j = 0; j < 6; j++)
00043 {
00044 tmpL1 = atoms[i].getLeg(j);
00045 for(int k = 1; k < size; k++)
00046 {
00047 for(int l = 0; l < 6; l++)
00048 {
00049 tmpL2 = atoms[k].getLeg(l);
00050 if(i != k)
00051 {
00052 if(!tmpL1->isLinked())
00053 {
00054 if(tmpL1->util->isConnectable(tmpL2))
00055 {
00056 tmpL1->setLink(tmpL2);
00057 }
00058 }
00059 }
00060 }
00061 }
00062 }
00063 }
00064
00065 typeDeplacement = typeDep;
00066
00067 switch(typeDeplacement)
00068 {
00069 case 1:
00070 initDep1();
00071 break;
00072 case 2:
00073 initDep2();
00074 break;
00075 default:
00076
00077 break;
00078 }
00079 }
00080
00081 ALGOATOMD::MoleculeLigne::~MoleculeLigne()
00082 {
00083 delete [] atoms;
00084 }
00085
00086
00087 void ALGOATOMD::MoleculeLigne::processMovement()
00088 {
00089 switch(typeDeplacement)
00090 {
00091 case 1:
00092 algoDep1();
00093 break;
00094 case 2:
00095 algoDep2();
00096 break;
00097 default:
00098
00099 break;
00100 }
00101 }
00102
00103 void ALGOATOMD::MoleculeLigne::renderMoleculeInfo()
00104 {
00105 }
00106
00107
00108
00109
00110
00111
00112 void ALGOATOMD::MoleculeLigne::initDep1()
00113 {
00114 indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00115 for(int i = 0; i < size; i++)
00116 {
00117 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition(191,KEEP,0,indexInit);
00118 atoms[i].getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,indexInit);
00119 }
00120
00121 for(int i = 0; i < size; i+=2)
00122 {
00123 for(double step=0; step<2*pi; step+=(double)(2*pi/NBR_STEP))
00124 {
00125 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition( 125+MV_RAYON, KEEP, 0, indexInit);
00126 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition( 125, KEEP, 0, indexInit);
00127 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition( 125, KEEP, 0, indexInit);
00128 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition( 125+MV_RAYON, KEEP, 0, indexInit);
00129 }
00130 }
00131
00132 for(int i = 1; i < size; i+=2)
00133 {
00134 for(double step=0; step<2*pi; step+=(double)(2*pi/NBR_STEP))
00135 {
00136 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition( 125-MV_RAYON, KEEP, 0, indexInit);
00137 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition( 125, KEEP, 0, indexInit);
00138 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition( 125, KEEP, 0, indexInit);
00139 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition( 125-MV_RAYON, KEEP, 0, indexInit);
00140 }
00141 }
00142 }
00143
00144 void ALGOATOMD::MoleculeLigne::algoDep1()
00145 {
00146 if(atoms[0].getMolecule()->isFinished(indexInit))
00147 {
00148 printf("ii\n");
00149 atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00150 indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00151
00152
00153 for(int i = 0; i < size; i+=2)
00154 {
00155 for(double step=0; step<2*pi; step+=(double)(2*pi/NBR_STEP))
00156 {
00157 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition( (int)(125+MV_RAYON*cos(step)), KEEP, 0, indexInit);
00158 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition( (int)(125+MV_RAYON*sin(step)), KEEP, 0, indexInit);
00159 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition( (int)(125+MV_RAYON*sin(step)), KEEP, 0, indexInit);
00160 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition( (int)(125+MV_RAYON*cos(step)), KEEP, 0, indexInit);
00161 }
00162 }
00163
00164 for(int i = 1; i < size; i+=2)
00165 {
00166 for(double step=0; step<2*pi; step+=(double)(2*pi/NBR_STEP))
00167 {
00168 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition( (int)(125+MV_RAYON*cos(pi+step)), KEEP, 0, indexInit);
00169 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition( (int)(125+MV_RAYON*sin(pi+step)), KEEP, 0, indexInit);
00170 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition( (int)(125+MV_RAYON*sin(pi+step)), KEEP, 0, indexInit);
00171 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition( (int)(125+MV_RAYON*cos(pi+step)), KEEP, 0, indexInit);
00172 }
00173 }
00174 }
00175 }
00176
00177
00178 void ALGOATOMD::MoleculeLigne::initDep2()
00179 {
00180 pair = 0;
00181 indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00182 for(int i = 0; i < size; i++)
00183 {
00184 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition(191,KEEP,0,indexInit);
00185 atoms[i].getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,indexInit);
00186 }
00187 }
00188
00189 void ALGOATOMD::MoleculeLigne::algoDep2()
00190 {
00191
00192 if(!m1 && !m2 && !m3 && !m4 && !m5)
00193 {
00194
00195 if(atoms[0].getMolecule()->isFinished(indexInit))
00196 {
00197 printf("11\n");
00198
00199 index1 = atoms[0].getMolecule()->getNextSyncIndex();
00200
00201
00202 for(int i = 0; i < size; i++)
00203 {
00204 if((i % 2) == pair)
00205 {
00206 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition(0,KEEP,0,index1);
00207 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index1);
00208 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition(250,KEEP,0,index1);
00209 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index1);
00210 }
00211 }
00212
00213
00214 m1=true;
00215 m2=false;
00216 m3=false;
00217 m4=false;
00218 m5=false;
00219
00220
00221 atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00222 }
00223 }
00224
00225 if(m1 && !m2 && !m3 && !m4 && !m5)
00226 {
00227 if(atoms[0].getMolecule()->isFinished(index1))
00228 {
00229 printf("22\n");
00230 index2 = atoms[0].getMolecule()->getNextSyncIndex();
00231
00232 for(int i = 0; i < size; i++)
00233 {
00234 if((i % 2) != pair)
00235 {
00236 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition(255,KEEP,0,index2);
00237 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00238 }
00239 }
00240
00241 m1=false;
00242 m2=true;
00243 m3=false;
00244 m4=false;
00245 m5=false;
00246
00247 atoms[0].getMolecule()->releaseSyncIndex(index1);
00248 }
00249 }
00250
00251 if(!m1 && m2 && !m3 && !m4 && !m5)
00252 {
00253 if(atoms[0].getMolecule()->isFinished(index2))
00254 {
00255 printf("33\n");
00256 index3 = atoms[0].getMolecule()->getNextSyncIndex();
00257
00258 for(int i = 0; i < size; i++)
00259 {
00260 if((i % 2) == pair)
00261 {
00262 atoms[i].getLeg(1)->getServoMotor(1)->setToHalfPosition(KEEP,0,index3);
00263 atoms[i].getLeg(1)->getServoMotor(0)->setToHalfPosition(KEEP,0,index3);
00264 atoms[i].getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,index3);
00265 atoms[i].getLeg(2)->getServoMotor(0)->setToHalfPosition(KEEP,0,index3);
00266 }
00267 }
00268
00269 m1=false;
00270 m2=false;
00271 m3=true;
00272 m4=false;
00273 m5=false;
00274
00275 atoms[0].getMolecule()->releaseSyncIndex(index2);
00276 }
00277 }
00278
00279 if(!m1 && !m2 && m3 && !m4 && !m5)
00280 {
00281 if(atoms[0].getMolecule()->isFinished(index3))
00282 {
00283 printf("44\n");
00284 index4 = atoms[0].getMolecule()->getNextSyncIndex();
00285
00286 for(int i = 0; i < size; i++)
00287 {
00288 if((i % 2) != pair)
00289 {
00290 atoms[i].getLeg(1)->getServoMotor(1)->setTargetPosition(0,KEEP,0,index4);
00291 atoms[i].getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index4);
00292 atoms[i].getLeg(2)->getServoMotor(1)->setTargetPosition(250,KEEP,0,index4);
00293 atoms[i].getLeg(2)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index4);
00294 }
00295 }
00296
00297 m1=false;
00298 m2=false;
00299 m3=false;
00300 m4=true;
00301 m5=false;
00302
00303 if(pair == 0)
00304 pair = 1;
00305 else
00306 pair = 0;
00307
00308 atoms[0].getMolecule()->releaseSyncIndex(index3);
00309 }
00310 }
00311
00312 if(!m1 && !m2 && !m3 && m4 && !m5)
00313 {
00314 if(atoms[0].getMolecule()->isFinished(index4))
00315 {
00316 printf("55\n");
00317 index5 = atoms[0].getMolecule()->getNextSyncIndex();
00318
00319
00320
00321 m1=false;
00322 m2=false;
00323 m3=false;
00324 m4=false;
00325 m5=true;
00326
00327 atoms[0].getMolecule()->releaseSyncIndex(index4);
00328 }
00329 }
00330
00331 if(!m1 && !m2 && !m3 && !m4 && m5)
00332 {
00333 if(atoms[0].getMolecule()->isFinished(index5))
00334 {
00335 printf("77\n");
00336 indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00337
00338
00339 m1=false;
00340 m2=false;
00341 m3=false;
00342 m4=false;
00343 m5=false;
00344
00345 atoms[0].getMolecule()->releaseSyncIndex(index5);
00346 }
00347 }
00348 }
00349
00350
00351 bool ALGOATOMD::MoleculeLigne::needMoreMouvement()
00352 {
00353 bool ret = false;
00354 API::CartesianPosition destination = *this->getDestination();
00355
00356 return ret;
00357 }