uint Aligner::checkBeginGreedy(const string& read, pair<kmer, uint>& overlap, vector<uNumber>& path, uint errors){ if(overlap.second==0){path.push_back(0);return 0;} string readLeft(read.substr(0,overlap.second)),unitig; auto rangeUnitigs(getEnd(overlap.first)); uint minMiss(errors+1),indiceMinMiss(0); bool ended(false); int offset(0); kmer nextOverlap(0); for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); if(unitig.size()-k+1>=readLeft.size()){ uint miss(missmatchNumber(unitig.substr(unitig.size()-readLeft.size()-k+1,readLeft.size()),readLeft, errors)); // if(miss==0){ // path.push_back(unitig.size()-readLeft.size()-k+1); // path.push_back(rangeUnitigs[i].second); // return 0; // } if(miss<minMiss){ minMiss=miss; indiceMinMiss=i; ended=true; offset=unitig.size()-readLeft.size()-k+1; } }else{ uint miss(missmatchNumber(unitig.substr(0,unitig.size()-k+1), readLeft.substr(readLeft.size()+k-1-unitig.size()), errors)); // if(miss==0){ // minMiss+=mapOnLeftEndGreedy(read, path, {nextOverlap,overlap.second-(nextUnitig.size()-k+1)},errors); // if(minMiss<=errors){ // path.push_back(rangeUnitigs[indiceMinMiss].second); // sucessML++; // } // } if(miss<minMiss){ kmer overlapNum(str2num(unitig.substr(0,k-1))); if(miss<minMiss){ ended=false; minMiss=miss; indiceMinMiss=i; nextOverlap=overlapNum; } } } } if(minMiss<=errors){ if(ended){ path.push_back(offset); path.push_back(rangeUnitigs[indiceMinMiss].second); return minMiss; } minMiss+=mapOnLeftEndGreedy(read, path, {nextOverlap,overlap.second-(rangeUnitigs[indiceMinMiss].first.size()-k+1)},errors-minMiss); if(minMiss<=errors){ path.push_back(rangeUnitigs[indiceMinMiss].second); sucessML++; } } return minMiss; }
uint Aligner::mapOnRightEndGreedy(const string &read, vector<uNumber>& path, const pair<kmer, uint>& overlap , uint errors){ // cout<<"moreg"<<endl; string unitig,readLeft(read.substr(overlap.second)),nextUnitig; // auto rangeUnitigs(getBeginOpti(overlap.first,path.back())); auto rangeUnitigs(getBegin(overlap.first)); uint miniMiss(errors+1), miniMissIndice(9); bool ended(false); // int offset(0); kmer nextOverlap(0); // cout<<"go"<<endl; // for(uint i(0); i<rangeUnitigs2.size(); ++i){ // cout<<(rangeUnitigs2[i].first)<<endl; // } // cout<<"true"<<endl; for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); // bool stop(false); // if(rangeUnitigs[i].first!=rangeUnitigs2[i].first){ // cout<<read<<endl; // cout<<"lol2"<<endl; // cout<<rangeUnitigs[i].first<<endl<<rangeUnitigs2[i].first<<endl; // cout<<rangeUnitigs[i].second<<" "<<rangeUnitigs2[i].second<<endl; // stop=true; // } // if(stop){cin.get();} // cout<<unitig<<endl; // cout<<rangeUnitigs[i].first<<endl; //case the rest of the read is too small if(readLeft.size()<=unitig.size()){ uint miss(missmatchNumber(unitig.substr(0,readLeft.size()), readLeft, errors)); if(miss<miniMiss){ miniMiss=miss; miniMissIndice=i; ended=true; // offset=unitig.size()-readLeft.size()-k+1; } }else{ //case the read is big enough we want to recover a true overlap uint miss(missmatchNumber(unitig, read.substr(overlap.second,unitig.size()), errors)); if(miss<miniMiss){ if(miss<miniMiss){ kmer overlapNum(str2num(unitig.substr(unitig.size()-k+1,k-1))); miniMiss=miss; miniMissIndice=i; nextUnitig=unitig; nextOverlap=overlapNum; } } } } // cout<<"end"<<endl; if(miniMiss<=errors){ path.push_back(rangeUnitigs[miniMissIndice].second); if (ended){return miniMiss;} miniMiss+=mapOnRightEndGreedy(read , path, {nextOverlap,overlap.second+(nextUnitig.size()-k+1)}, errors-miniMiss); } return miniMiss; }
uint Aligner::checkEndExhaustive(const string& read, pair<kmer, uint>& overlap, vector<uNumber>& path, uint errors){ string readLeft(read.substr(overlap.second+k-1)),unitig; vector<uNumber> path2keep; if(readLeft.empty()){ path.push_back(0); return 0; } auto rangeUnitigs(getBegin(overlap.first)); uint minMiss(errors+1),indiceMinMiss(9); bool ended(false); int offset(-2); if(partial & rangeUnitigs.empty()){ //if(!path.empty()){ return 0; //} } for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); if(unitig.size()-k+1>=readLeft.size()){ uint miss(missmatchNumber(unitig.substr(k-1,readLeft.size()),readLeft, errors)); if(miss<minMiss){ minMiss=miss; indiceMinMiss=i; ended=true; offset=readLeft.size()+k-1; } }else{ uint miss(missmatchNumber(unitig.substr(k-1),readLeft.substr(0,unitig.size()-k+1), errors)); if(miss<minMiss){ kmer overlapNum(str2num(unitig.substr(unitig.size()-k+1,k-1))); vector<uNumber> possiblePath; miss+=mapOnRightEndExhaustive(read, possiblePath, {overlapNum,overlap.second+(unitig.size()-k+1)},errors-miss); if(miss<minMiss){ path2keep=possiblePath; minMiss=miss; indiceMinMiss=i; ended=false; } } } } if(minMiss<=errors){ if(ended){ path.push_back(rangeUnitigs[indiceMinMiss].second); path.push_back(offset); }else{ path.push_back(rangeUnitigs[indiceMinMiss].second); path.insert(path.end(), path2keep.begin(),path2keep.end()); } } return minMiss; }
pair<uint,uint> Aligner::mapOnRightCache(const string &read, vector<uNumber>& path, const overlapStruct& overlap, const vector<overlapStruct>& listOverlap, bool& ended,uint start, uint errors){ string unitig, readLeft(read.substr(overlap.pos+k-1)),nextUnitig; if(readLeft.empty()){cout<<"should not appears"<<endl;exit(0);return {start,0};} auto rangeUnitigs(overlap.unitig); uint miniMiss(errors+1),miniMissIndice(9); uint next(start); kmer nextOverlapNum(0); for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i]); //case the rest of the read is too small if(readLeft.size() <= unitig.size()-k+1){ uint miss(missmatchNumber(unitig.substr(k-1,readLeft.size()), readLeft, errors)); if(miss<miniMiss){ ended=true; miniMiss=miss; miniMissIndice=i; } }else{ //case the read is big enough we want to recover a true overlap uint miss(missmatchNumber(unitig.substr(k-1), readLeft.substr(0,unitig.size()-k+1), errors)); if(miss<miniMiss){ kmer overlapNum(str2num(unitig.substr(unitig.size()-k+1,k-1))); if(miss<miniMiss){ ended=false; miniMiss=miss; miniMissIndice=i; nextOverlapNum=overlapNum; nextUnitig=unitig; next=start; for(uint j(start+1); j<listOverlap.size(); ++j){ if(overlapNum==listOverlap[j].seq and listOverlap[j].pos==overlap.pos+unitig.size()-k+1){ next=j; } } } } } } if(ended){ path.push_back(overlap.unitigNumbers[miniMissIndice]); return {start,miniMiss}; } if(miniMiss<=errors){ path.push_back(overlap.unitigNumbers[miniMissIndice]); if(next>start){ return {next,miniMiss}; } auto res(mapOnRightCache(read , path, {nextOverlapNum,overlap.pos+((uint)nextUnitig.size()-k+1)},listOverlap,ended,start, errors-miniMiss)); return {res.first,res.second+miniMiss}; } return {start,errors+1}; }
void drive_test(){ int leftDistance = readLeft(); if(leftDistance > 80) { leftBackward(30); rightBackward(30); } else if( leftDistance < 70 ) { rightForward(30); leftForward(30); } }
/* * testRangeFinders() * This function is used to test and classify the idealized function for the range finders, as well as * implementing the basic functionality. It will turn LEDs on and off depending on if an object comes * within a certain range (see spreadsheet for distance and readouts). Setting a breakpoint at the top * of the while loop while the code is running will give a good value for what the ADC reads at a particular * distance. */ void testRangeFinders() { BCSCTL1 = CALBC1_8MHZ; DCOCTL = CALDCO_8MHZ; initRangeFinders(); P1DIR |= LEFT_LED|RIGHT_LED; int fBuffer[BUFFER_LN]; int lBuffer[BUFFER_LN]; int rBuffer[BUFFER_LN]; int mf; int ml; int mr; fillBuffers(fBuffer, lBuffer, rBuffer); while(1) { readFront(fBuffer); // Place Breakpoint here readLeft(lBuffer); readRight(rBuffer); mf = median(fBuffer); // Medians were chosen as they are less influenced by outliers ml = median(lBuffer); // The user is free to change these functions to test and classify the mr = median(rBuffer); // means, however they should be close to the median values. if(mf > 0x01F0) { P1OUT |= (RIGHT_LED|LEFT_LED); } else if(ml > 0x0220) { P1OUT &= ~RIGHT_LED; P1OUT |= LEFT_LED; } else if(mr > 0x0220) { P1OUT &= ~LEFT_LED; P1OUT |= RIGHT_LED; } else { // Important reset condition, do not forget in robot mevement code P1OUT &= ~(LEFT_LED|RIGHT_LED); } } }
uint Aligner::mapOnLeftEndExhaustive(const string &read, vector<uNumber>& path, const pair<kmer, uint>& overlap , uint errors){ string unitig, readLeft(read.substr(0,overlap.second)); vector<uNumber> path2keep; if(readLeft.size()==0){return 0;} auto rangeUnitigs(getEnd(overlap.first)); uint miniMiss(errors+1),miniMissIndice(9); int offset(-2); bool ended(false); for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); //case the rest of the read is too small if(readLeft.size()+k-1 <= unitig.size()){ uint miss(missmatchNumber(unitig.substr(unitig.size()-readLeft.size()-k+1,readLeft.size()), readLeft, errors)); if(miss<miniMiss){ miniMiss=miss; miniMissIndice=i; offset=unitig.size()-readLeft.size()-k+1; ended=true; } }else{ //case the read is big enough we want to recover a true overlap uint miss(missmatchNumber(unitig.substr(0,unitig.size()-k+1), readLeft.substr(readLeft.size()-(unitig.size()-k+1)), errors)); if(miss<miniMiss){ kmer overlapNum(str2num(unitig.substr(0,k-1))); vector<uNumber> possiblePath; miss+=mapOnLeftEndExhaustive(read , possiblePath, {overlapNum,overlap.second-(unitig.size()-k+1)}, errors-miss); if(miss<miniMiss){ path2keep=possiblePath; miniMiss=miss; miniMissIndice=i; offset=-1; ended=false; } } } } if (miniMiss<=errors){ if(ended){ path.push_back(offset); }else{ path.insert(path.end(), path2keep.begin(),path2keep.end()); } path.push_back(rangeUnitigs[miniMissIndice].second); } return miniMiss; }
uint Aligner::mapOnLeftEndGreedy(const string &read, vector<uNumber>& path, const pair<kmer, uint>& overlap , uint errors){ // if(overlap.second==0){path.push_back(0);return 0;} string unitig,readLeft(read.substr(0,overlap.second)),nextUnitig; auto rangeUnitigs(getEnd(overlap.first)); uint miniMiss(errors+1),miniMissIndice(9); bool ended(false); int offset(0); kmer nextOverlap(0); for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); //case the rest of the read is too small if(readLeft.size()+k-1 <= unitig.size()){ uint miss(missmatchNumber(unitig.substr(unitig.size()-readLeft.size()-k+1,readLeft.size()), readLeft, errors)); if(miss<miniMiss){ miniMiss=miss; miniMissIndice=i; ended=true; offset=unitig.size()-readLeft.size()-k+1; } }else{ //case the read is big enough we want to recover a true overlap uint miss(missmatchNumber(unitig.substr(0,unitig.size()-k+1), readLeft.substr(readLeft.size()-(unitig.size()-k+1)), errors)); if(miss<miniMiss){ kmer overlapNum(str2num(unitig.substr(0,k-1))); if(miss<miniMiss){ ended=false; miniMiss=miss; miniMissIndice=i; nextUnitig=unitig; nextOverlap=overlapNum; } } } } if (miniMiss<=errors){ if(ended){ path.push_back(offset); path.push_back(rangeUnitigs[miniMissIndice].second); return miniMiss; } miniMiss+=mapOnLeftEndGreedy(read , path, {nextOverlap,overlap.second-(nextUnitig.size()-k+1)}, errors-miniMiss); path.push_back(rangeUnitigs[miniMissIndice].second); } return miniMiss; }
uint Aligner::checkEndGreedy(const string& read, pair<kmer, uint>& overlap, vector<uNumber>& path, uint errors){ string readLeft(read.substr(overlap.second+k-1)),unitig,nextUnitig; if(readLeft.size()<=k-1){return 0;} auto rangeUnitigs(getBegin(overlap.first)); uint minMiss(errors+1),indiceMinMiss(9); bool ended(false); kmer nextOverlap(0); for(uint i(0); i<rangeUnitigs.size(); ++i){ unitig=(rangeUnitigs[i].first); if(unitig.size()-k+1>=readLeft.size()){ uint miss(missmatchNumber(unitig.substr(k-1,readLeft.size()),readLeft, errors)); if(miss<minMiss){ minMiss=miss; indiceMinMiss=i; ended=true; } }else{ uint miss(missmatchNumber(unitig.substr(k-1),readLeft.substr(0,unitig.size()-k+1), errors)); if(miss<minMiss){ if(miss<minMiss){ kmer overlapNum(str2num(unitig.substr(unitig.size()-k+1,k-1))); minMiss=miss; indiceMinMiss=i; nextOverlap=overlapNum; nextUnitig=unitig; } } } } if(minMiss<=errors){ path.push_back(rangeUnitigs[indiceMinMiss].second); if(ended){ return minMiss; } minMiss+=mapOnRightEndGreedy(read, path, {nextOverlap,overlap.second+(nextUnitig.size()-k+1)},errors-minMiss); if(minMiss<=errors){ successMR++; } } return minMiss; }
void init(void) { int i; for(i=0;i<numSpheres;i++){ spheres[i][0] = (rand()%600)-300;//XPOS spheres[i][1] = (rand()%600)-300;//YPOS spheres[i][2] = rand()%400;//ZPOS spheres[i][3] = rand()%20;//SIZE spheres[i][4] = (rand()%20);//SPEED } MAXROT = 1.0/6.0*PI; glClearColor (0.0, 0.0, 0.0, 0.0); glEnable(GL_DEPTH_TEST); glShadeModel(GL_SMOOTH); pmodel = glmReadOBJ("city.obj"); if (!pmodel) fprintf(stderr,"Cannot parse vase.obj"); //glmUnitize(pmodel); // make model to fit in a unit cube glmFacetNormals(pmodel); // generate normals - is this needed? glmVertexNormals(pmodel, 90.0); // average joining normals - allow for hard edges. tmodel = glmReadOBJ("tower.obj"); glmFacetNormals(tmodel); // generate normals - is this needed? glmVertexNormals(tmodel, 90.0); // average joining normals - allow for hard edges. readTop(); readFront(); readBack(); readRight(); readLeft(); readFire(); glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glGenTextures(1, &front); glGenTextures(1, &top); glGenTextures(1, &left); glGenTextures(1, &right); glGenTextures(1, &back); glGenTextures(1, &fire); qsphere = gluNewQuadric(); }
/**** PID TEST ****/ void drive_straight_PID(void){ int offset = -40; static int previous_error = 0; static int previous_time = 0; static int last_big = 0; int error; //current error values int biggest; int current_time; //current time double total; int leftDiagSensor, rightDiagSensor; double kp = 0.5, kd = 0.5; leftDiagSensor = readLeft(); rightDiagSensor = readRight(); //debug print out sensor readings //Serial.print("IR left diag: "); //Serial.print(leftDiagSensor); //Serial.print(" IR right diag: "); //Serial.print(rightDiagSensor); if(!previous_time) { previous_time = millis(); return; } leftDiagSensor = readLeft(); rightDiagSensor = readRight(); if( 1 )//temporarily for walls on both sides only |x| { error = rightDiagSensor - leftDiagSensor + offset; } total = error *kp; previous_time = current_time; //analogWrite(R_fwd, HIGH - total); //analogWrite(L_fwd, HIGH + total); //what the PID will do (because motor functions are not done) if(total > 25) total=0.5*total; if(total > 50 ) total = 0; if(total<-50) total=0; Serial.print("total error: "); Serial.println(total); rightForward(15+total); leftForward(25-total); if( error == 0 ){ Serial.print(" Mouse is straight: "); Serial.println(error); } if( error > 0 ){ Serial.print(" Mouse is veering right: "); Serial.println(error); } if( error < 0 ){ Serial.print(" Mouse is veering left: "); Serial.println(error); } }//end drive_straight_PID
/* * main.c * Author: Ian R Goodbody * Function: Implements maze navigation */ void main(void) { WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer initRangeFinders(); initMotors(); stop(); P1DIR |= LEFT_LED|RIGHT_LED; P1OUT &= ~(LEFT_LED|RIGHT_LED); int fBuffer[BUFFER_LN]; // Code probably unnecessary but it cannot hurt int lBuffer[BUFFER_LN]; int rBuffer[BUFFER_LN]; int fMedian; int lMedian; int rMedian; unsigned int lWheelSpeed = 4040; unsigned int rWheelSpeed = 4000; unsigned int runTime = 350; int i; fillBuffers(fBuffer, lBuffer, rBuffer); while(1) { P1OUT &= ~(LEFT_LED|RIGHT_LED); // turn off LEDs stop(); for (i = BUFFER_LN; i > 0; i--) { readFront(fBuffer); waitMiliseconds(1); readLeft(lBuffer); waitMiliseconds(1); readRight(rBuffer); waitMiliseconds(1); } fMedian = median(fBuffer); lMedian = median(lBuffer); rMedian = median(rBuffer); if(fMedian < 0x01F0) // Crash into wall test; ~2.5 inch threshold { if(lMedian < 0x01FF) // There is no wall remotely close to the left side, { // Initiate sweeping turn lWheelSpeed = 2600; rWheelSpeed = 5000; } else if(lMedian < 0x0266) // Getting too far from the wall start turning in { P1OUT |= LEFT_LED; // Red LED on, green off P1OUT &= ~RIGHT_LED; rWheelSpeed = 4270; lWheelSpeed = 4040; } else if(lMedian > 0x02E4) // Getting too close to the wall start turning out { P1OUT |= RIGHT_LED; // Green LED on, red off P1OUT &= ~LEFT_LED; rWheelSpeed = 3900; lWheelSpeed = 4040; } else // Acceptable distance from wall, cruise forward normally { //P1OUT |= RIGHT_LED; P1OUT &= ~(RIGHT_LED|LEFT_LED); rWheelSpeed = 4000; lWheelSpeed = 4040; } setLeftWheel(lWheelSpeed, FORWARD); setRightWheel(rWheelSpeed, FORWARD); runTime = 350; } else { // About to run into a wall, initiate hard right turn P1OUT |= RIGHT_LED|LEFT_LED; setLeftWheel(5080,FORWARD); setRightWheel(5260, BACKWARD); runTime = 450; } go(); waitMiliseconds(runTime); } }