How to make my robot move in a rectangular path along the black tape?

I am working on a robot, it's part of the summer robotics workshop in our college. We are using C-STAMP micro controllers by A-WIT. I was able to make it move, turn left, turn right, move backward. I have even managed to make it go along the black tape using a contrast sensor.

I send the robot at 30-45 degrees toward the black tape on the table and it aligns itself and starts to move along the black tape. It jerks a little, probably due to my programming logic below, it's running a while-loop and constantly checking if-statements, so it ends up trying to turn left and right every few milliseconds, which explains the jerking part. But it's okay, it works, not as smooth as I want it to work but it works! Problem is that开发者_高级运维 I can't make my robot go into a rectangular path of the black tape. As soon as it reaches the corner of the black tape it just keeps going straight instead of making left or right turn.

My 2 sensors are located right underneath the robot, next to the front wheel, almost at the floor level. It has "index" value ranging from 0 to 8. Eight being the brightest contrast and zero being the darkest contrast. So when the robot moves into the black-tape-zone, the index value drops, and based on that, I have an if-statement telling my robot to either turn left or right.

Here's my attempt. To avoid confusion I didn't post the entire source code, but only the logical part responsible for the movement of my robot along the black tape.

while(1) {

    // don't worry about these.
    // 10 and 9 represent Sensor's PIN location on the motherboard
    V = ANALOGIN(10, 1, 0, 0, 0);
    V2 = ANALOGIN(9, 1, 0, 0, 0);

    // i got this "formula" from the example in my Manual. 
    // V stands for voltage of the sensor.
    // it gives me the index value of the sensor. 0 = darkest, 8 = lightest.
    index = ((-(V - 5) / 5) * 8 + 0.5);
    index2 = ((-(V2 - 5) / 5) * 8 + 0.5);

    // i've tweaked the position of the sensors so index > 7 is just right number.
    // the robot will move anywhere on the table just fine with index > 7. 
    // as soon as it drops to or below 7 (i.e. finds black tape), the robot will 
    // either turn left or right and then go forward.

    // lp & rp represent left-wheel pin and right-wheel pin, 1 means run forever.
    // if i change it from 1 to 100, it will go forward for 100ms.
    if (index > 7 && index2 > 7)
        goForward(lp, rp, 1);

    if (index <= 7) {
        turnLeft(lp, rp, 1);
        goForward(lp, rp, 1);
        // this is the tricky part. i've added this code last minute
            // trying to make my robot turn, but i didn't work.
        if (index > 4) {
            turnLeft(lp, rp, 1);
            goForward(lp, rp, 1);
        }
    }

    else if (index2 <= 7) {
        turnRight(lp, rp, 1);
        goForward(lp, rp, 1);
            // this is also the last minute addition. it's same code as above
            // but it's for the 2nd sensor.
        if (index2 > 4) {
            turnRight(lp, rp, 1);
            goForward(lp, rp, 1);
        }
    }
}

I've spent the entire day trying to figure it out. I've pretty much exhausted all avenues. Asking for the solution on stackoverflow is my very last option now.

Thanks in advance! If you have any questions about the code, let me know, but comments should be self-explanatory.

This is my goForward function in case anyone wonders:

void goForward(BYTE lp, BYTE rp, WORD t)
{
    WORD i;

    for(i = 0; i < t; i = i + 1){
        PULSOUT(lp, 400, 1, 1);
        PULSOUT(rp, 800, 1, 1);
        PAUSE(17);
    }
}

UPDATE: Here's what I've come up with so far. I've erased all my if-statements that I've posted earlier and decided to write the logic from scratch:

// if there's enough bright light in both sensors at the same time
    // robot will move forward forever.
    if (index > 7 && index2 > 7)
        goForward(lp, rp, 1);

    // but if there's not enough bright light anymore (i.e. reached black tape)
    // proceed to the else-statement.
    else {
        // if left sensor detects the black tape then turn right
        // if doesn't detect the black tape then keep going forward
        if (index2 <= 7)
            turnRight(lp, rp, 1);
        else
            goForward(lp, rp, 1);

        // if right sensor detects the black tape then turn left
        // if it doesn't detect the black tape then keep going forward
        if (index <= 7) 
            turnLeft(lp, rp, 1);
        else 
            goForward(lp, rp, 1);
    }

    // The reason for turnLeft and turnRight is to keep robot re-alligning
    // to the black tape. It will happen so fast (every 1ms) that the jerking
    // shouldn't even be noticeable.

You need to trap the sudden occurrence: [cycle n] "I see the tape" -> [cycle n+1] "I don't see the tape" that happens when you encounter a corner.

A state machine is a good way of handling this type of problem. With a state machine the code for a specific state is isolated, it only runs when the state is true. This isolation prevents "cross-talk" and gives you one known code block for each state.

Given your example the flow might be something like:

:Loop

State == Moving forward and on tape.
  read sensors
  delta = abs(valueLastCycle - valueThisCycle);
  if (delta >= bigChangeValue){
    // the tape was just gone. 
    // change state and handle this situation.
    CurrentState = suddenValueChange;
  }else
    if (index <= 7) {
        turnLeft(lp, rp, 1);
        goForward(lp, rp, 1);
    else if (index2 <= 7) {
        turnRight(lp, rp, 1);
        goForward(lp, rp, 1);
    }
   ...
State == suddenValueChange
 ...
 code that handles sudden value change event
 maybe:
 Stop Robot;
 Move backwards slowly until on tape or distance x
 etc...

Goto Loop

Increasing the scan rate might seem to help....but the faster the robot moves the faster your scan rate....i.e. You might still jump from on tape -> off tape in which case your current code flounders.

My guess is that the goForward immediately after the turnLeft/TurnRight "cancels" the turning? It depends on how turning is implemented though

Make sure your "plan" makes sense before worrying about the code.

Start by moving the robot around by hand and observing when the sensors pass over black and white areas. Try to come up with a behaviour, and simulate it by hand. If this doesn't work as you want, then you need to revise the behaviour.

A suggestion: You may wish to add some more loops to ensure that if something seems wrong, the robot corrects itself before resuming normal behaviour. (E.g. rather than turning right/left for 100ms, do that behaviour as long as it takes for the sensors to see the right thing.)

Does your while loop run fast enough to catch a corner? If both sensors report that they are on the tape, and in the next cycle both sensors are off the tape, you can't detect that, right? The sensor reports the same values (8) for being on the tape and off the tape?

Try making your code faster. Without seeing the entire code, it's hard to make a recommendation, but it seems that you might be evaluating if statements that are not necessary. Add an ELSE after the first IF, once you know that you are going straight.

It seems that your goforward implementation is blocking the loop for too long. 1 does not mean run forever, it does one pass of the for-loop, and then you PAUSE 17 (msec?). What's the PAUSE for? Remove it. This probably causes the jerking, and it prevents the next set of sensor values from coming in.

Right now, by the looks of your code, every time the robot sees black, it goes forward forever. I'm not really sure how going forward is implemented.

In pseudocode, your code says:

if you see black
 go forward forever
if you don't see black
 turn left or right

Do you see how your robot would go forward forever with that kind of logic? Again, I have no idea how going forward is implemented so I could be wrong.