How make robot turning going straight?

Question

I have been struggling with this function for a while.

I want my robot to turn left until it detects black then turn right until it hits black again. Then, I will divide the angle in half and call rotateTo to make the robot move that angle. I don't know why it is not working the way I want it to.

The reason I do that is because after I turn left or right it is not exactly 90 degrees. That is why I want to make sure it is facing the right direction before it continues moving. Here is the code:

int numberTurn = 0;
int numLeft = 0;
int numRight = 0;
RegulatedMotor Left = Motor.A;
RegulatedMotor Right = Motor.C;
while(color_id!=7){
    moveForward(300, 50);
    color_id = color_test.getColorID();
}
while(color_id==7){
    moveForward(300, 50);
    color_id = color_test.getColorID();
}
color_id = color_test.getColorID();
Left.resetTachoCount();
Right.resetTachoCount();
while(color_id!=7){
    Left.rotate(100,true);
    Right.rotate(-100,true);
    color_id = color_test.getColorID();
}
Right.stop(true);
Left.stop(true);
numLeft += Left.getTachoCount();
while(color_id==7){
    Left.rotate(-100,true);
    Right.rotate(100,true);
    color_id = color_test.getColorID();
}
Right.stop(true);
Left.stop(true);
numRight += Left.getTachoCount();
while(color_id!=7){
    Left.rotate(-100,true);
    Right.rotate(100,true);
    color_id = color_test.getColorID();
}
Right.stop(true);
Left.stop(true);
numRight += Left.getTachoCount();
numberTurn = (numRight-numLeft)/2;
Left.rotateTo(-numberTurn,true);
Right.rotateTo(numberTurn,true);
while(Left.isMoving()){
}

Left and Right is motor. For color_id, 7 is black Anyone have any ideas why it doesn't work?

Answer 1

The main problems I see are the following:

1. The value in the numRight variable is wrong for two reason: The 'right' sensor is not reseted at the proper time and you read the 'left' sensor value instead of the 'right'.
2. The angle calculation is faulty
3. The half rotation is performed in the wrong direction

Here are my code comments:

//Move forward until you find black
[...]

//Move forward until you are not on black anymore
[...]

//Reset the rotation sensors
[...]
//   WARNING: The right sensor is reset for no reason   
Right.resetTachoCount();

//Turn left until you find black
[...]

//Get the left rotation sensor data  
[...]

//Turn right until you are not on black anymore
[...]

//Get the right rotation sensor data
//   ERROR: This action should not be done until you have turned all the way to the right   
//   ERROR: At this stage the right sensor should be reseted   
//   Note: ...and the 'left' data was put in the 'right' variable  
numRight += Left.getTachoCount();

//Turn right until you find black
[...]

//Get the right rotation sensor data
//   ERROR: This data should come from right.getTachoCount();  
numRight += Left.getTachoCount();

//Calculate the half angle
//   ERROR: The calculation should be: numberTurn = ((numRight + numLeft) / 2 ) / 2;
//   Explaination: By adding the two rotations and then dividing by 2 you get the average of the two.
//                 The result is then divided by 2 again to get the half angle.  
numberTurn = (numRight-numLeft)/2;
//Rotate back (should be to the left) to half the angle
//   ERROR: Rotation is performed to the right when it shoud be to the left 
Left.rotateTo(-numberTurn,true);
Right.rotateTo(numberTurn,true);

Recommendation: When dealing with multi steps processes like this one, add the steps one by one while re-testing each time. The test should validate that the previous steps are still working and that the added step is working as planned.