Driving Straight

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

Hello,

I am currently programming my autonomous and am not sure if this is the best way to drive straight a set amount of ticks on the encoder.

void DriveStraight(int straightTicks)
{
clearDriveEncoders();

while(SensorValue[LeftEncoder] < straightTicks)
{

 if(SensorValue[LeftEncoder] > SensorValue[RightEncoder])
{
  RDrivePower = 63;
  LDrivePower = 50;
}

if (SensorValue[RightEncoder] > SensorValue[LeftEncoder])
{
  RDrivePower = 50;
  LDrivePower = 63;
}

if (SensorValue[RightEncoder] == SensorValue[LeftEncoder])
{
  RDrivePower = 63;
  LDrivePower = 63;
}

}

}

Any tips on how to improve this? Thanks

#2

Hmm… you’re heading in the right direction!
I would say, you probably want to make something closer to

if((SensorValue[LeftEncoder] - SensorValue[RightEncoder]) > 10)
{
RDrivePower = 63;
LDrivePower = 50;
}

if ((SensorValue[RightEncoder] - SensorValue[LeftEncoder]) > 10)
{
RDrivePower = 50;
LDrivePower = 63;
}

if (((SensorValue[LeftEncoder] - SensorValue[RightEncoder]) < 10) && ((SensorValue[RightEncoder] - SensorValue[LeftEncoder]) < 10))
{
RDrivePower = 63;
LDrivePower = 63;
}
}

That way it’s not so jittery!
It’ll still be a bang-bang control loop though… you might eventually want to move to a P or PID controller which would look more like:

int Error = SensorValue[RightEncoder] - SensorValue[LeftEncoder];
int TargetPower = 50;
int PropConst = .1;
RDrivePower = TargetPower - PropConst * Error;
LDrivePower = TargetPower + PropConst * Error;

I haven’t tested that code, but that should be the basic principle behind it.

I hope you get your straight drive figured out and working!

#3

Thanks for the reply. I’m going to have a go at the PID loop. Is there a better way of looping it instead of using only one encoder?

#4

Yes; the easy solution would be to just average the left and right encoder.
However, if either encoder fails or is off (which can happen), then your robot will be driving straight for a lot longer than you intended.
Another option would be to say, “If either encoder’s value is greater than the value of the target point then stop”, and that might work a bit better.
Ideally though, you might think of a way to combine these two approaches into one. Something like, “If the average of the left and right encoders’ values is greater than the target value OR if either encoder’s value plus fudge constant X is greater than the target value then stop.”

#5

I am currently testing this part and the code does not seem to stop. It is going way over the amount of ticks I set. What am I doing wrong?

while(((SensorValue[RightEncoder] + SensorValue[LeftEncoder]) / 2) < 200)
{
int Errorz = SensorValue[RightEncoder] - SensorValue[LeftEncoder];
int TargetPower = 50;
int PropConst = .1;
RDrivePower = TargetPower - PropConst * Errorz;
LDrivePower = TargetPower + PropConst * Errorz;
}

#6

Your code segment also does not appear to apply the motor power to the motors,
nor does it stop the motors when the while() loop is complete.
Perhaps those are in the code elsewhere?

I don’t see any encoder initialization also.

And something about this line doesn’t look right;


  int PropConst = .1;
#7

I forgot to stop the motors >___> . This is only apart of the code. L and rmotorpower is defined elsewhere. Not sure wha Propconst does myself…

#8

If you don’t know how your own code works then don’t post it. PropConst is presumably the proportional constant and should be defined as a float if you want to initialize it to 0.1.

#9

P

Simmons 2.0 above posted it. This is why I posted it to ask for help

#10

Thanks jpearman!
Yes, PropConst would stand for Proportional Constant (I’ve also seen Kp used)
And “int PropConst = .1;” should be “float PropConst = .1”
That part doesn’t need to be in the loop, because it’s only declared once, so your program might look something more like:

//Initialization stuff happens here
float PropConst = .1;

//Function Declaration
void DriveStraight(int Distance, int Power)
{
clearDriveEncoders();
while(((SensorValue[RightEncoder] + SensorValue[LeftEncoder]) / 2) < Distance)
{
Error = SensorValue[RightEncoder] - SensorValue[LeftEncoder];
RDrivePower = Power - PropConst * Error;
LDrivePower = Power + PropConst * Error;
//Drive motor commands go here
}
//Stop motor commands go here
}

This is only written from memory, so it may contain bugs. Your mileage may vary.

This page (as many have already suggested on this forum) will help you with PID-Controllers:
http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

#11

Ok, sorry, I reread my post and I did bite your head off there. My fault for not reading the whole thread.

#12

@jpearman
no worries

Thanks Simmons for all the help! Got it working :smiley:

#13