Now that all five of the MyBlocks are finished, it is time to put them together to make the navigation program:
The first thing we need to do is define some variables. Create a new logic variable (set the mode to Write | Logic) and name it exit. Set its starting value to false. This variable's value changes to true when the GPS Car reaches its destination; this stops the navigation loop and ends the program:
We need two more numeric variables named destLat and destLong. You will use these variables to set the destination coordinates. After these three variables, insert the steerCenter MyBlock so the GPS Car calibrates its steering mechanism before navigating. Lastly, add a loop. Later, we will change the loop's exit case so that the program exits when the exit variable's value changes to true:
Refer back to the example coordinates for the Washington Monument that we acquired earlier. Its latitude is 38.889479 degrees, and its longitude is -77.035250 degrees. The latitude value will be entered into destLat as 38889479 and the longitude value will be entered into destLong as -77035250. When the values are entered into the variables, the EV3 software puts them into scientific notation and rounds the last digit. So, the coordinates become 3.888948e+07 for destLat and -7.703525e+07 for destLong:
The first piece of code that we will place inside the loop is a move steering block (On, steering = 0, power = 75 percent). This turns on the drive motors so the GPS Car continuously powers forward while it navigates:
Next, we need to read our GPS position data. Insert two dGPS sensor blocks; set one to measure latitude and the second to measure longitude. Then, add two variable blocks with their modes set to Read | Numeric to retrieve the values stored in destLat and detLong:
Add the getAngle MyBlock to the program and assign the sensor block outputs and variable values to the corresponding input on getAngle. The dGPS sensor block that reads latitude should plug into input A on getAngle; the dGPS block that reads longitude should plug into input B. The value of destLat should be assigned to input C and destLong should be assigned to input D. Using these input data, getAngle calculates an angle heading value, which indicates the direction that the GPS Car must turn to drive towards its destination; the getAngle output parameter, angle, stores the result of the MyBlock's calculations:
Now, it is time to put that heading value to use! Add a HiTechnic compass sensor block and change its mode to Measure | Absolute Heading. The programming block will reconfigure to include an input parameter; this input is used to set the target heading:
Plug the output of getAngle into the target input on the compass block. This sets the heading angle calculated using the GPS data as the compass's target heading. The compass will return a relative heading angle based on the target heading. Create a new numeric variable named compassRelHead and use it to store the compass's relative heading:
Earlier in the chapter, we established that the sign of the relative heading returned by the compass indicates the direction that the car must turn to reach its destination. If the relative heading is positive (greater than zero), the GPS Car must turn right; if the heading is negative (less than zero), the GPS Car must turn left. If the relative heading equals zero, no adjustment is required.
The program will first check to see if the relative heading is positive. This will require the usual programming with a compare block, which checks to see if the value is greater than zero, and a logic switch:
If the compare block returns true, the GPS Car must turn right to remain on course to the destination. Add the steerRight MyBlock to the true case of the switch:
If the compare block returns a false value, then the program will check to see if the relative heading is negative. In the false case of the switch, set up a variable that reads the value of compassRelHead, checks to see if it is less than zero using a compare block, and returns its value to control a nested logic switch:
Place the steerLeft MyBlock in the true case of the most recent switch. The false case executes if the relative heading is equal to zero; place the steerReCenter MyBlock here:
To prevent the EV3 from oversampling the dGPS, insert a wait block that will pause the program for a duration of one second after the switches:
Only one more section of the program remains! The last piece of code compares the destination programmed by the user to the current GPS position and stops the program when it determines that the destination has been reached. It reads the values stored in latDiff and longDiff (recall that these values are calculated as an intermediate step within the getAngle MyBlock) and checks to see if they are within a certain range. This works because latDiff and longDiff are the distances to the destination in one dimension each. If both latDiff and longDiff are small, this indicates that the car is close enough, so the car stops and the program ends.
The code will check the value of latDiff first. It reads the stored value and inserts it as the test value of a range block. The range block compares the test value to a predefined condition and returns a true or false value to indicate whether the test value meets the condition. Set the mode of the range block to Inside and set the lower and upper bounds to -10 and 10, respectively. In practice, this means that a true value will be the result if the car's current latitude is within roughly one meter in either direction of the destination latitude. The logic result of the range block will control a logic switch. You may set the switch to tabbed view because we will only write code in its true case. The code for the first half of the destination check looks like this:
The second half of the destination check reads the value stored in longDiff, but otherwise, the code is the same. The second half of the check returns a true value if the car's current longitude is within one meter in either direction of the destination. The two halves of the destination check are nested as so:
If both of these checks return true, then the car is close enough to the destination and the EV3 can exit the navigation loop. Add a variable block (mode set to Write | Logic) that changes the value of the exit variable to true:
Change the exit case on the main loop to Logic. A loop in this mode will stop repeating when it receives a true value. Directly before the loop's exit case, add a variable block (mode set to Read | Logic) that reads the value stored in the exit variable. Plug the data wire from the variable block into the input of the loop block. This exit case occurs so that the navigation program repeats until the destination check changes the value of exit to true, causing the program to end:
Finally, place two blocks outside the main loop: a move steering block (Off) and steerReCenter. When the car reaches its destination, it will stop and return its steering mechanism to the center position:
