void followLine(void){ // put the values of the line sensor into the 'line' array LineSensorReadArray(gls, line); if((line[3]>0.5)&&(line[4]>0.5)){ SetMotor(leftMotor, 1); SetMotor(rightMotor, 1); } else if (line[5]>0.5&&line[4]>0.5){ SetMotor(leftMotor, .8); SetMotor(rightMotor, .4); } else if (line[2]>0.5&&line[3]>0.5){ SetMotor(leftMotor, .4); SetMotor(rightMotor, .8); } else if (line[7]>0.5&&line[6]>0.5){ SetMotor(leftMotor, .3); SetMotor(rightMotor, -.25); } else if (line[0]>0.5&&line[1]>0.5){ SetMotor(leftMotor, -.25); SetMotor(rightMotor, .3); } else if (line[6]>0.5&&line[5]>0.5){ SetMotor(leftMotor, .6); SetMotor(rightMotor, .1); } else if (line[1]>0.5&&line[2]>0.5){ SetMotor(leftMotor, .6); SetMotor(rightMotor, .1); } }
bool mostDark(void){ int i = 0; int count = 0; LineSensorReadArray(gls, line); for (i = 0; i<8; i+=1){ if(line[i]>0.5){count += 1;} } if(count>4&&count<9) {return true;} else {return false;} }
bool linePresent(void){ int i = 0; int count = 0; LineSensorReadArray(gls, line); for (i = 0; i<8; i+=1){ if(line[i]>0.5){count += 1;} } if(count>1&&count<4) {return true;} else {return false;} }
int turnline(void){ float line[8]; LineSensorReadArray(gls, line); int i=0; int total = 0; for (i = 0; i < 8; i++) { if(line[i] > 1000) total+=1; } return total; }
void edgefollow(){ float line[8]; LineSensorReadArray(gls, line); if(line[0]>100) { setm(.2f,.2f);ledGreen();} else if(line[7]>100){ setm(.6f,-.1f);ledBlue();} else{ setm(-.1f,.6f);ledRed();} }
int main(void) { initIRSensor(); initMotor(); initGPIOLineSensor(); initServo(); SetServo(servo,0); stage = 0; //while(1){followLine();} //while(1){followWall();} //SetMotor(leftMotor, 1); SetMotor(rightMotor, 1); while(true){ LineSensorReadArray(gls, line); if (stage==0){ //start state if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) { followWall(); }else{ followLine(); } }else if(stage==1){ //once 90degree turn passed // SetMotor(leftMotor, 1); // SetMotor(rightMotor, -1); SetPin(PIN_F2, 1); followWall(); if (wallPresent()) {followWall();} else { findLine(); } SetPin(PIN_F2, 0); }else if (stage==2){ //once line found again after walled section SetPin(PIN_F1, 1); if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)|| (mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5) findEnd(); }else{ followLine(); }; SetPin(PIN_F1, 0); }else{//end of course look for flag findObject(); break; } } }
int online(){ float line[8]; LineSensorReadArray(gls, line); int total = 0; int i=0; for (i = 0; i < 8; i++) { Printf("%.2f ", line[i]); if(line[i] > 1000) total+=1; } if(total > 5){ ledGreen(); return 1;} else{ // ledGreen(); return 0; } }
void lineSensorDemo(void) { Printf("Press:\nany key-read line sensor\n"); Printf("any key after read begins-quit\n"); while(!KeyWasPressed()) { int i; float line[8]; LineSensorReadArray(ls, line); Printf("Line Sensor: ["); for (i=0; i < 8; i++) { Printf("%01.4f ", line[i]); } Printf("\b]\r"); } Printf("\n"); }
void gpioLineSensorDemo(void) { Printf("Press any key to quit\n"); // loop as long as the user doesn't press a key while (!KeyWasPressed()) { int i; float line[8]; // put the values of the line sensor into the 'line' array LineSensorReadArray(gls, line); Printf("Line Sensor: ["); for (i = 0; i < 8; i++) { Printf("%.2f ", line[i]); } Printf("\b] \r"); } Printf("\n"); }
void gpioLineSensorDemo(void) { Printf("Press:\nany key-read line sensor\n"); Printf("any key after read begins-quit\n"); while(!KeyWasPressed()) { int i; float line[8]; char lineSensorBits = LineSensorRead(gls, 0.8f); LineSensorReadArray(gls, line); //Printf("Line Sensor: [%d]\r", lineSensorBits); Printf("\r"); for (i=0; i < 8; i++) { Printf("%d ", lineSensorBits & (1 << i) ? 1 : 0); //Printf("%1.4f ", line[i]); } Printf(" "); } //Printf("\n"); }
void lineFollo(void){ int i; float line[8]; float total = 0.0f; float tot=0.0f; LineSensorReadArray(gls,line); for (i = 0; i < 4; i++) { total+=((i*1.0f))*line[i]; } for (i = 4; i < 8; i++) { tot+=((i*1.0f))*line[i]; } if(total > 1000) { if(tot<1000){ setm(.5f,.1f); ledRed(); } else{ setm(.2f,.2f); ledGreen(); } } else{ if(tot > 1000){ setm(.1f,.5f); ledBlue(); } else{ setm(.2f,.2f); ledGreen(); } } }
void lineFollow(void) { int i; float line[8]; float total = 0.0f; LineSensorReadArray(gls, line); Printf("Line Sensor: ["); for (i = 0; i < 8; i++) { Printf("%.2f ", line[i]); if(line[i] < 1000){ total+=((i*1.0f)-3.5f)*line[i]; } else{ total+=((i*1.0)-3.5f)*5; } } Printf("\b] \r"); if(total == 0){ turn=0; ledGreen(); setm(.25f,.3f); } else if(total < 0){ if(total > (-20.0f)){ if(turn==0){ ledGreen(); setm(.25f,.30f); } else{ turn=0; } } else if(turn ==1){ ledBlue(); if (total < (-30)) { ledBlue();setm(.45f, .00f);} else if(total > (-25)) { setm(.35f,.00f);ledBlue();} else { setm(.25f, .00f);ledBlue();} } else{ turn=1; } } else{ if(total < (20.0f)){ if(turn==0){ ledGreen(); setm(.25f,.30f); } else{ turn=0; } } else{ ledRed(); if(turn==2){ if(total > 30) { ledRed();setm(.00f,.45f);} else if(total < 25) { setm(.00f,.35f);ledRed();} else { setm(.00f,.25f);ledRed();} } else{ turn=2; } } } Printf("\n"); }