Let's look at your algorithm first. I have changed your mode variables to hard coded numbers which makes the code more obvious.
Code:
/* Add your own autonomous code here. */
switch (Step)
{
case 1:
{
stop = 2000;
crawl = 1800; // are you sure these
slow = 1900; // are in the correct order?
Left_Count = Get_Left_Encoder_Count();
Right_Count = Get_Right_Encoder_Count();
if (Left_Count < 1900)
pwm03 = 220;
if (Left_Count > 1900 && Left_Count < 1800)
pwm03 =200;
if (Left_Count > 1800 && Left_Count < 2000)
pwm03 =150;
if (Left_Count >= 2000)
pwm03 =127;
printf("Left Counter %ld\r", Left_Count);
if (Right_Count < 1900)
pwm04 =220;
if (Right_Count > 1900 && Right_Count < 1800)
pwm04 =200;
if (Left_Count > 1800 && Right_Count < 2000)
pwm04 =150;
if (Right_Count >= 2000)
pwm04 =127;
printf("Right Counter %ld\r", Right_Count);
Notice that this shows problems with initialization of the crawl and slow values. Some lines are unreachable and some condition statements are executed twice. A simpler and more manageable way to use multiple if statements on the same variables is this form:
Code:
if (count < slow) {
// fast
}
else if (count < crawl) {
// slow
}
else if (count < stop) {
// crawl
}
else {
// stop
}
There is nothing in your algorithm preventing a pwm of 127 being set if the count is actually incrementing. If your printf statements are being reached and show the counts go over 2000 then the next thing to check is if setting those pwm's to 127 really does stop the motors.