//------------------------------------------------------------- /* read line sensors values Sensor Array Error Value 0 0 0 0 1 4 0 0 0 1 1 3 0 0 0 1 0 2 0 0 1 1 0 1 0 0 1 0 0 0 0 1 1 0 0 -1 0 1 0 0 0 -2 1 1 0 0 0 -3 1 0 0 0 0 -4 1 1 1 1 1 0 Robot found continuous line - test if an intersection or end of maze 0 0 0 0 0 0 Robot found no line: turn 180o */ void readLFSsensors() { LFSensor[0] = digitalRead(lineFollowSensor0); LFSensor[1] = digitalRead(lineFollowSensor1); LFSensor[2] = digitalRead(lineFollowSensor2); LFSensor[3] = digitalRead(lineFollowSensor3); LFSensor[4] = digitalRead(lineFollowSensor4); farRightSensor = analogRead(farRightSensorPin); farLeftSensor = analogRead(farLeftSensorPin); if ((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = CONT_LINE; error = 0;} else if((LFSensor[0]== 0 )&& (farRightSensor < THRESHOLD)) {mode = RIGHT_TURN; error = 0;} else if ((farLeftSensor < THRESHOLD) &&(LFSensor[4]== 0 )) {mode = LEFT_TURN; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = NO_LINE; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 4;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 3;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 2;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 1;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error =- 1;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -2;} else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -3;} else if((LFSensor[0]== 1 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -4;} Serial.print (farLeftSensor); Serial.print (" <== LEFT RIGH==> "); Serial.print (farRightSensor); Serial.print (" mode: "); Serial.print (mode); Serial.print (" error:"); Serial.println (error); } //----------------------------------------------------------- void readLateralSensors(void) { }