This is an old revision of the document!
Draica Andrei 335CA
Robot pe roti care rezolva labirinturi simple - Maze Solver
Lista de piese:
Se retin virajele pe care le face robotul. Acestea sunt limitate la L (stanga), R (dreapta) si B (inapoi). Dupa ce ruleaza, se elimina infundaturile, se pozitioneaza manual pe pozitia initiala si retine drumul corect pana la rezolvarea labirintului. <hidden Code here!>
// The Pololu QTR Library and portions of the Pololu sample maze solving // code have been used here. #include <QTRSensors.h> // Pololu QTR Library //QRTSensors folder must be placed in your arduino libraries folder //line sensor defines #define NUM_SENSORS 8 // number of sensors used #define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low #define EMITTER_PIN QTR_NO_EMITTER_PIN // emitter control pin not used. If added, replace QTR_NO_EMITTER_PIN with pin# //line sensor declarations // sensors 0 through 7 are connected to digital pins 2 through 10, respectively (pin 3 is skipped and use for motor control) QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4, A5, 6, 7}, //A6 A7 nu pot fi folositi ca digitali pe NANO NUM_SENSORS, TIMEOUT, EMITTER_PIN); unsigned int sensorValues[NUM_SENSORS]; unsigned int line_position=0; // value from 0-7000 to indicate position of line between sensor 0 - 7 // motor driver vars // pwm_a/b sets speed. Value range is 0-255. For example, if you set the speed at 100 then 100/255 = 39% duty cycle = slow int pwm_a = 10; //PWM control for motor outputs 1 and 2 is on digital pin 10 (Left motor) int pwm_b = 11; //PWM control for motor outputs 3 and 4 is on digital pin 11 (Right motor) int IN1 = 2;//direction control for motor outputs 1 and 2 is on digital pin 12 (Left motor) int IN2 = 3; int IN3 = 4; int IN4 = 5; // motor tuning vars for maze navigating int calSpeed = 45; // tune value motors will run while auto calibration sweeping turn (0-255) int turnSpeed = 60; // tune value motors will run while turning (0-255) int turnSpeedSlow = 55; // tune value motors will run as they slow down from turning cycle to avoid overrun (0-255) int drivePastDelay = 200; // tune value in mseconds motors will run past intersection to align wheels for turn int motorspeed=65; // pid loop vars float error=0; float lastError=0; float PV =0 ; float kp = 0; // tune value in follow_line() function //float ki = 0; // ki is not currently used float kd =0; // tune value in follow_line() function int m1Speed=0; int m2Speed=0; // The path variable will store the path that the robot has taken. It // is stored as an array of characters, each of which represents the // turn that should be made at one intersection in the sequence: // 'L' for left // 'R' for right // 'S' for straight (going straight through an intersection) // 'B' for back (U-turn) // You should check to make sure that the path_length of your // maze design does not exceed the bounds of the array. char path[100] = ""; unsigned char path_length = 0; // the length of the path void setup() { pinMode(pwm_a, OUTPUT); //Set control pins to be outputs pinMode(pwm_b, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(6, INPUT); pinMode(7, INPUT); Serial.begin(9600); analogWrite(pwm_a, 0); //set both motors to stop at (100/255 = 39)% duty cycle (slow) analogWrite(pwm_b, 0); Serial.begin(9600); Serial.println("Zagros Robotics, Inc."); Serial.println("Maze Solver Demo\n "); Serial.println("Version 06162015a - PD"); Serial.println("Calibrating sensor"); // calibrate line sensor, determines min/max range of sensed values for the current course delay(2000); for (int i = 0; i <= 100; i++) // begin calibration cycle to last about 2.5 seconds (100*25ms/call) { // auto calibration sweeping left/right, tune 'calSpeed' motor speed at declaration if (i==0 || i==60) // slow sweeping turn right to pass sensors over line { digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, calSpeed); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); analogWrite(pwm_b, calSpeed); } else if (i==20 || i==100) // slow sweeping turn left to pass sensors over line { digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); analogWrite(pwm_a, calSpeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, calSpeed); } qtrrc.calibrate(); // reads all sensors with the define set 2500 microseconds (25 milliseconds) for sensor outputs to go low. } // end calibration cycle // read calibrated sensor values and obtain a measure of the line position from 0 to 7000 // To get raw sensor values, call: // qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues); line_position = qtrrc.readLine(sensorValues); while (sensorValues[6] < 200) // wait for line position to near center { line_position = qtrrc.readLine(sensorValues); } // slow down speed analogWrite(pwm_a, turnSpeedSlow); analogWrite(pwm_b, turnSpeedSlow); // find center while (line_position > 4350) // wait for line position to find center { line_position = qtrrc.readLine(sensorValues); } // stop both motors analogWrite(pwm_b, 0); // stop right motor first which kinda helps avoid over run analogWrite(pwm_a, 0); // print calibration results // print the calibration minimum values measured when emitters were on for (int i = 0; i < NUM_SENSORS; i++) { Serial.print(qtrrc.calibratedMinimumOn[i]); Serial.print(' '); } Serial.println(); // print the calibration maximum values measured when emitters were on for (int i = 0; i < NUM_SENSORS; i++) { Serial.print(qtrrc.calibratedMaximumOn[i]); Serial.print(' '); } Serial.println(); Serial.println(); Serial.println("Calibration Complete"); delay(1000); } // end setup void loop() { // read calibrated sensor values and obtain a measure of the line position from 0 to 7000 // To get raw sensor values, call: // qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues); unsigned int line_position = qtrrc.readLine(sensorValues); //Serial.println(); // uncomment this line if you are using raw values Serial.print(line_position); // comment this line out if you are using raw values Serial.print(" "); Serial.print(sensorValues[0]); Serial.print(" "); Serial.println(sensorValues[7]); Serial.println(); //delay(2000); // begin maze solving MazeSolve(); // comment out and run serial monitor to test sensors while manually sweeping across line Serial.print("::"); Serial.print(error); Serial.print('\t'); Serial.print(PV); Serial.print('\t'); Serial.print(m1Speed); Serial.print('\t'); Serial.print(m2Speed); Serial.print('\t'); Serial.print(lastError); Serial.print('\t'); } // end main loop //line following subroutine // PD Control void follow_line() //follow the line { lastError = 0; while(1) { line_position = qtrrc.readLine(sensorValues); error = (float)line_position - 3500; error = constrain(error,-3500, 3500); // set the motor speed based on proportional and derivative PID terms // kp is the a floating-point proportional constant (maybe start with a value around 0.5) // kd is the floating-point derivative constant (maybe start with a value around 1) // note that when doing PID, it's very important you get your signs right, or else the // control loop will be unstable kp=0.5; kd=1; PV = kp * error + kd * (error - lastError); lastError = error; //Cod line follower: m1Speed = motorspeed - PV; m2Speed = motorspeed + PV; m1Speed = constrain(m1Speed, -motorspeed, motorspeed); // motorspeed > m1Speed > -motorspeed m2Speed = constrain(m2Speed, -motorspeed, motorspeed); if (m1Speed <= 0) { digitalWrite(IN1, 0); digitalWrite(IN2, 1); } else { digitalWrite(IN1, 1); digitalWrite(IN2, 0); } if (m2Speed <= 0) { digitalWrite(IN3, 0); digitalWrite(IN4, 1); } else { digitalWrite(IN3, 1); digitalWrite(IN4, 0); } analogWrite(pwm_a, abs(m1Speed)); analogWrite(pwm_b, abs(m2Speed)); // We use the inner six sensors (1 thru 6) to // determine if there is a line straight ahead, and the // sensors 0 and 7 if the path turns. if(sensorValues[1] < 100 && sensorValues[2] < 100 && sensorValues[3] < 100 && sensorValues[4] < 100 && sensorValues[5] < 100 && sensorValues[6] < 100) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. return; } else if(sensorValues[0] > 200 || sensorValues[7] > 200) { // Found an intersection. return; } } } // end follow_line // Turns to the sent variable of // 'L' (left), 'R' (right), 'S' (straight), or 'B' (back) // Tune 'turnSpeed' at declaration void turn(char dir) { switch(dir) { // Turn left 90deg case 'L': digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); analogWrite(pwm_a, turnSpeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, turnSpeed); line_position = qtrrc.readLine(sensorValues); while (sensorValues[6] <200) // wait for outer most sensor to find the line { line_position = qtrrc.readLine(sensorValues); } // slow down speed analogWrite(pwm_a, turnSpeedSlow); analogWrite(pwm_b, turnSpeedSlow); // find center while (line_position > 4350) // tune - wait for line position to find near center { line_position = qtrrc.readLine(sensorValues); } // stop both motors analogWrite(pwm_b, 0); // stop right motor first to better avoid over run analogWrite(pwm_a, 0); break; // Turn right 90deg case 'R': digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, turnSpeed); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); analogWrite(pwm_b, turnSpeed); line_position = qtrrc.readLine(sensorValues); while (sensorValues[1] <200) // wait for outer most sensor to find the line { line_position = qtrrc.readLine(sensorValues); } // slow down speed analogWrite(pwm_a, turnSpeedSlow); analogWrite(pwm_b, turnSpeedSlow); // find center while (line_position < 3250) // tune - wait for line position to find near center { line_position = qtrrc.readLine(sensorValues); } // stop both motors analogWrite(pwm_a, 0); analogWrite(pwm_b, 0); break; // Turn right 180deg to go back case 'B': digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, turnSpeed); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); analogWrite(pwm_b, turnSpeed); line_position = qtrrc.readLine(sensorValues); while (sensorValues[1] <200) // wait for outer most sensor to find the line { line_position = qtrrc.readLine(sensorValues); } // slow down speed analogWrite(pwm_a, turnSpeedSlow); analogWrite(pwm_b, turnSpeedSlow); // find center while (line_position < 3250) // tune - wait for line position to find near center { line_position = qtrrc.readLine(sensorValues); } // stop both motors analogWrite(pwm_a, 0); analogWrite(pwm_b, 0); break; // Straight ahead case 'S': // do nothing break; } } // end turn // This function decides which way to turn during the learning phase of // maze solving. It uses the variables found_left, found_straight, and // found_right, which indicate whether there is an exit in each of the // three directions, applying the "left hand on the wall" strategy. char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) { // Make a decision about how to turn. The following code // implements a left-hand-on-the-wall strategy, where we always // turn as far to the left as possible. if(found_left) return 'L'; else if(found_straight) return 'S'; else if(found_right) return 'R'; else return 'B'; } // end select_turn // Path simplification. The strategy is that whenever we encounter a // sequence xBx, we can simplify it by cutting out the dead end. For // example, LBL -> S, because a single S bypasses the dead end // represented by LBL. void simplify_path() { // only simplify the path if the second-to-last turn was a 'B' if(path_length < 3 || path[path_length-2] != 'B') return; int total_angle = 0; int i; for(i=1;i<=3;i++) { switch(path[path_length-i]) { case 'R': total_angle += 90; break; case 'L': total_angle += 270; break; case 'B': total_angle += 180; break; } } // Get the angle as a number between 0 and 360 degrees. total_angle = total_angle % 360; // Replace all of those turns with a single one. switch(total_angle) { case 0: path[path_length - 3] = 'S'; break; case 90: path[path_length - 3] = 'R'; break; case 180: path[path_length - 3] = 'B'; break; case 270: path[path_length - 3] = 'L'; break; } // The path is now two steps shorter. path_length -= 2; } // end simplify_path // This function is called once, from the main loop void MazeSolve() { // Loop until we have solved the maze. while(1) { // FIRST MAIN LOOP BODY follow_line(); // Drive straight a bit. This helps us in case we entered the // intersection at an angle. digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, motorspeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, motorspeed); delay(25); // These variables record whether the robot has seen a line to the // left, straight ahead, and right, whil examining the current // intersection. unsigned char found_left=0; unsigned char found_straight=0; unsigned char found_right=0; // Now read the sensors and check the intersection type. line_position = qtrrc.readLine(sensorValues); // Check for left and right exits. if(sensorValues[0] > 200) found_right = 1; if(sensorValues[7] > 200) found_left = 1; // Drive straight a bit more - this is enough to line up our // wheels with the intersection. digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, motorspeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, motorspeed); delay(drivePastDelay); line_position = qtrrc.readLine(sensorValues); if(sensorValues[1] > 200 || sensorValues[2] > 200 || sensorValues[3] > 200 || sensorValues[4] > 200 || sensorValues[5] > 200 || sensorValues[6] > 200) found_straight = 1; // Check for the ending spot. // If all six middle sensors are on dark black, we have // solved the maze. if(sensorValues[1] > 600 && sensorValues[2] > 600 && sensorValues[3] > 600 && sensorValues[4] > 600 && sensorValues[5] > 600 && sensorValues[6] > 600) break; // Intersection identification is complete. // If the maze has been solved, we can follow the existing // path. Otherwise, we need to learn the solution. unsigned char dir = select_turn(found_left, found_straight, found_right); // Make the turn indicated by the path. turn(dir); // Store the intersection in the path variable. path[path_length] = dir; path_length ++; // Simplify the learned path. simplify_path(); } // Solved the maze! // Now enter an infinite loop - we can re-run the maze as many // times as we want to. while(1) { // maybe you would like to add a blinking led or a beeper. // we just have it waiting patiently to be placed back on the starting line. analogWrite(pwm_a, 0); // stop both motors analogWrite(pwm_b, 0); // while(1){}; // uncomment this line to cause infinite loop to test if end was found if your robot never seems to stop // hold motors while robot is sitting on end point line_position = qtrrc.readLine(sensorValues); while(sensorValues[1] > 200 && sensorValues[2] > 200 && sensorValues[3] > 200 && sensorValues[4] > 200 && sensorValues[5] > 200 && sensorValues[6] > 200) { line_position = qtrrc.readLine(sensorValues); delay(50); } // hold until the start line is seen again when the robot has been placed there again while(1) { line_position = qtrrc.readLine(sensorValues); if(sensorValues[2] > 200 || sensorValues[3] > 200 || sensorValues[4] > 200 || sensorValues[5] > 200 && sensorValues[0] < 200 && sensorValues[1] < 200 && sensorValues[6] < 200 && sensorValues[7] < 200) break; delay(50); } // delay to give you time to let go of the robot delay(2000); // Re-run the now solved maze. It's not necessary to identify the // intersections, so this loop is really simple. int i; for(i=0;i<path_length;i++) { // SECOND MAIN LOOP BODY follow_line(); // drive past intersection slightly slower and timed delay to align wheels on line digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, motorspeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, motorspeed); delay(drivePastDelay); // tune time to allow wheels to position for correct turning // Make a turn according to the instruction stored in // path[i]. turn(path[i]); } // Follow the last segment up to the finish. follow_line(); // drive past intersection slightly slower and timed delay to align wheels on line digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(pwm_a, motorspeed); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(pwm_b, motorspeed); delay(drivePastDelay); // tune time to allow wheels to position for correct turning // Now we should be at the finish! Now move the robot again and it will re-run this loop with the solution again. } // end running solved } // end MazeSolve
</hidden>