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);
}
}
