Re: Can anyone please help me with this code
Let's separate out a couple of sections of your code:
Section 1, when placed inside a loop like you have it, will keep track of the heading of the robot. This part looks fine
Section 2, it should be noted, is running an infinite while loop inside your main loop. This means that once the program reaches this part of the code, it won't ever exit and return to Section 1. Section 2 appears to be a good, if a bit verbose, movement controller. The problem here is that you are controlling the movement using the raw readings from the gyro, which is not returning the heading of the robot (you need the section 1 code for that), but the rate
of angular movement, i.e. how fast the robot is turning.
So the first correction is to change your movement controller so that it uses the calculated robot heading instead: change the instances of SensorValue(HTGYRO) in Section 2 to instead reference the heading variable.
The problem now is that Section 2 is still an infinite loop so your program will never return to Section 1 to update the robot heading. Thus, we need to find a way to combine the two sections so they run more-or-less simultaneously (usually said to be executing concurrently, in computer science jargon). The easiest way to do this is to remove your inner loop while(true) loop and then change the while loops that checking the value of SensorValue(HTGYRO) (which we've changed to check the value of heading instead) into if statements.
This will lead to the following program flow:
- Wait for the next multiple of 20ms to have passed
- Read the current rate of rotation from the gyro and use that to update the calculated robot heading
- Use the new calculated heading value to figure out if we should drive left, right or straight for the next 20ms interval
Hopefully that made sense.