I have been having some problems with my autonomous code recently, and now, one part of my code won’t stop moving, but the next lines of code start running even when this line of code is still running. The picture below is the part of my code that won’t stop running. I do know that my encoders are properly set up in the motors and sensors setup and work fine in the other portions of my code. Can anyone help? Thanks in advance.
It’s really hard for us to help with these types of questions without seeing the robot moving and having the debugger open in front of us. With the benefit of seeing how the encoder counts are changing it should be easy to fix, however, one part of the code is almost guaranteed not to work.
while (nMotorEncoder[BL] <146 && nMotorEncoder[FR] <146 && nMotorEncoder[BR] ==0 && nMotorEncoder[FL] == 0 )
The chance that an encoder will stay exactly at 0 as the other wheels move is 0%, the encoders will move very slightly, perhaps a count of 1 or 2. As the BR & FL motors are not involved in the movement why not simplify the code to be.
motor[BR] = 0;
motor[FL] = 0;
while (nMotorEncoder[BL] <146 && nMotorEncoder[FR] <146)
{
if(nMotorEncoder[BL] > nMotorEncoder[FR])
{
motor[BL] = 127/2;
motor[FR] = 127;
}
else if (nMotorEncoder[FR] > nMotorEncoder[BL] )
{
motor[BL] = 127;
motor[FR] = 127/2;
}
else if (nMotorEncoder[FR] == nMotorEncoder[BL] )
{
motor[BL] = 127;
motor[FR] = 127;
}
}
Additionally, the chance of the two motor encoders being equal is also unlikely so you will be always in one of the other two conditional statements, I would do something like (untested code).
int delta;
motor[BR] = 0;
motor[FL] = 0;
while (nMotorEncoder[BL] <146 && nMotorEncoder[FR] <146)
{
delta = nMotorEncoder[FR] - nMotorEncoder[BL];
if( abs(delta) < 10 ) // close to being the same +/- 10
{
motor[BL] = 127;
motor[FR] = 127;
}
else
if( delta > 0 ) // FR faster
{
motor[BL] = 127;
motor[FR] = 127/2;
}
else // BL Faster
{
motor[BL] = 127/2;
motor[FR] = 127;
}
}
or finally, if you really want to get an A for programming, start using functions (see my post here)
Create a function for the drive motors
void
DriveMotors( int f_left, int f_right, int b_left, int b_right )
{
motor[FL] = f_left;
motor[FR] = f_right;
motor[BL] = b_left;
motor[BR] = b_right;
}and then the code can start to be much shorter and easier (subjectively I guess) to read.
while (nMotorEncoder[BL] <146 && nMotorEncoder[FR] <146)
{
delta = nMotorEncoder[FR] - nMotorEncoder[BL];
if( abs(delta) < 10 ) // close to being the same +/- 10
DriveMotors( 0, 127, 127, 0 );
else
if( delta > 0 ) // FR faster
DriveMotors( 0, 127/2, 127, 0 );
else // BL Faster
DriveMotors( 0, 127, 127/2, 0 );
}
I understand completely that it is very hard for you to help with these kinds of questions, but I greatly appreciate your help!! I can understand if you can’t help me with the little information that I am able to provide. Anyway, I tried those programs that you provided and I had more progress with the original code I used. At least with the original it was going in the right direction, but it just wouldn’t stop moving. With these that you suggested the robot drifted off to the left when it should have moved in the north west direction, all motors were moving when only two should have been moving, but it did stop at the right time. Also I have one question you should be able to answer, When the autonomous is running should the lights on the encoders be solid green throughout, or are they supposed to go off and on? Thank you soooooo much for your help!!!
Then there must have been more code above what you posted that effects movement. How many motors are supposed to be running? You send 0 to BR and FL in each condition yet also compare their counts, no idea what you are trying to do there, are you trying to rotate the robot with only two motors and drag the other two wheels around with it?
The green leds will flash faster as the motor moves faster until eventually they become solid green (IIRC).
I am trying to get the X drive to move north west by making the right front wheel and the back left wheel to move, while the other wheels stay put. I got rid of that code comparing the zero valued motors because like you said, it makes no sense to put that in there.
Ah, ok, I’m more used to mecanum drive. What happens if you just try some very simple code and remove the “drive straight” code you have included.
motor[BR] = 0;
motor[FL] = 0;
motor[BL] = 0;
motor[FR] = 0;
while (nMotorEncoder[BL] <146 && nMotorEncoder[FR] <146)
{
motor[BL] = 127;
motor[FR] = 127;
}
motor[BL] = 0;
motor[FR] = 0;
Update: I figured out that the “drift” that was happening was actually the code above the one I thought I was having trouble with which is this:
while (nMotorEncoder[BL] >-565 && nMotorEncoder[FR] <565 && nMotorEncoder[BR] <565 && nMotorEncoder[FL] >-565)//while the encoders are all less than -84 counts
{
if(nMotorEncoder[BL] < nMotorEncoder[FR] && nMotorEncoder[BR] && nMotorEncoder[FL])//if the back left motor counts are greater than the rest
{
motor(BL) = -127/2;//motor will move slower
motor(FR) = 127;//motor will move
motor(BR) = 127;//motor will move
motor(FL) = -127;//motor will move
}
else if (nMotorEncoder[FR] > nMotorEncoder[BL] && nMotorEncoder[BR] && nMotorEncoder[FL])//if the front right motor counts are greater than the rest
{
motor(BL) = -127;//motor will move
motor(FR) = 127/2;//motor will move slower
motor(BR) = 127;//motor will move
motor(FL) = -127;//motor will move
}
else if (nMotorEncoder[BR] > nMotorEncoder[BL] && nMotorEncoder[FR] && nMotorEncoder[FL])//if the back right motor counts are greater than the rest
{
motor(BL) = -127;//motor will move
motor(FR) = 127;//motor will move
motor(BR) = 127/2;//motor will move slower
motor(FL) = -127;//motor will move
}
else if (nMotorEncoder[FL] < nMotorEncoder[BL] && nMotorEncoder[FR] && nMotorEncoder[BR])//if the front left motor counts are greater than the rest
{
motor(BL) = -127;//motor will move
motor(FR) = 127;//motor will move
motor(BR) = 127;//motor will move
motor(FL) = -127/2;//motor will move slower
}
else if (nMotorEncoder[FL] == nMotorEncoder[BL] == nMotorEncoder[FR] == nMotorEncoder[BR])//if all counts are equal
{
motor(BL) = -127;//motor will move
motor(FR) = 127;//motor will move
motor(BR) = 127;//motor will move
motor(FL) = -127;//motor will move
}//move left
}
And the code below that I needed to move north west just had too small of a value to do anything, that is why I mistook it for the above code.
while (nMotorEncoder[BL] <246 && nMotorEncoder[FR] <246)//while the encoders are all less than 146 counts
{
if(nMotorEncoder[BL] > nMotorEncoder[FR])//if the back left motor counts are greater than the rest
{
motor(BL) = 127/2;//motor will move slower
motor(FR) = 127;//motor will move
}
else if (nMotorEncoder[FR] > nMotorEncoder[BL])//if the front right motor counts are greater than the rest
{
motor(BL) = 127;//motor will move
motor(FR) = 127/2;//motor will move slower
}
else if (nMotorEncoder[FR] == nMotorEncoder[BL])//if all counts are equal
{
motor(BL) = 127;//motor will move
motor(FR) = 127;//motor will move slower
}//move north west
}
But that first piece of code isn’t working. Even though it is turning left, it would look as though one motor stops and three drift left, but that can’t be possible. Do you see anything wrong with the code?
When I took out the code and made a simple go forward until, like you suggested above, it worked, but the other part, that drifts left still happened, proving my theory that I had two pieces of code mixed up. So all along, it was the code above that travel north west code. I still can’t figure out what is wrong with that code though.
Well, lets look at what the code is doing.
You start with the while loop.
while (nMotorEncoder[BL] >-565 && nMotorEncoder[FR] <565 && nMotorEncoder[BR] <565 && nMotorEncoder[FL] >-565)
It looks like you intend BL and FL to go negative, and FR and BR to go positive.
However, the first if statement
if(nMotorEncoder[BL] < nMotorEncoder[FR] && nMotorEncoder[BR] && nMotorEncoder[FL])
compares BL and FR, if BL is going negative and FR is going positive then BL will always be less than FR. You then have two logical and conditions nMotorEncoder[BR] and nMotorEncoderFL), but these are not compared to anything so if not 0 they will be true and basically inconsequential to the test.
So BL is always less than FR and you therefore set the BL motor to -63, much slower than the other three. Depending on the forces on this motor (I have no idea what your gearing is) it may not move much, is this what you are experiencing?
Ok, I understand. Then would this code in the picture be correct? (Edit:forgot to attach it in this post, look on next post)
Thanks again!
Here is the code that I forgot to attach to my last post.
My bad again, that code has an error in it. Here is the code that I programmed with jpearman’s suggestion. I tested it and it still drifts. Any other suggestions? Thanks in advance!!!