My Integrated Encoder method

VEX V5 General Discussion General Forum
1

Hello again!

I thought I’d post the code for my encoder method for review on here and in case anyone else would find this code useful.
Any suggestions would be welcome!

Note: “LBW” stands for “Left Back Wheel”


void motor_speed(int speed, float rotation) {
	nMotorEncoder[LBW] = 0;
	string Rotation = "Rotation";
	if(rotation < 0) {
		Rotation = "Negative";
	}
	else if(rotation > 0) {
		Rotation = "Positive";
	}
	else {
		Rotation = "Rotation";
	}
	float total_rotation = rotation*410;
	if(Rotation == "Positive") {
		while(nMotorEncoder[LBW] < total_rotation)
		{
			//Move forward
			motor[RFW] = speed;
			motor[RBW] = speed;
			motor[LBW] = speed;
			motor[LFW] = speed;
		}
	}
	else if(Rotation == "Negative") {
		while(nMotorEncoder[LBW] > total_rotation)
		{
			//Move backward
			motor[RFW] = -speed;
			motor[RBW] = -speed;
			motor[LBW] = -speed;
			motor[LFW] = -speed;
		}
	}
	else {
		//Nothing
	}
	motor[RFW] = 0;
	motor[RBW] = 0;
	motor[LBW] = 0;
	motor[LFW] = 0;
}

Basically, the method receives two parameters: the number of revolutions for the wheels, and the speed of the motors. It determines if the rotation value is + or -, multiplies the rotation value by the one rotation constant (410 ticks is read by the encoder when the wheel is rotated one full revolution), and makes the motors run until the encoder reaches the number of wheel rotations specified in the method.

This is programmed in RobotC.

Thanks!

2

Good Job! The code looks neat and is easy to follow, I also like what you did with the “rotation*420”, probably faster to program in distances then straight encoder values!

Personally, I would get rid the if-else statements that assign the string “Rotation” to the positive/negative/rotation value, I feel it doesn’t make it any easier to read the code in this instance (while in many cases, that could be a good idea). I also believe it could make your life a bit easier if you create a function that looks similar to this:


void setMotorPWM(int leftSpeed, int rightSpeed)
{
	motor[RFW] = rightSpeed;
	motor[RBW] = rightSpeed;
	motor[LBW] = leftSpeed;
	motor[LFW] = leftSpeed;
}

This would leave you with a simplified function like the following, which I think makes things a tad easier to look through :slight_smile:


void motor_speed(int speed, float rotation)
{
	nMotorEncoder[LBW] = 0;
	float total_rotation = rotation*410;

	//Negative!
	if(rotation < 0){
		while(nMotorEncoder[LBW] < total_rotation){
			setMotorPWM(-speed, -speed);
		}
	}

	//Positive!
	if(rotation > 0){
		while(nMotorEncoder[LBW] < total_rotation){
			setMotorPWM(speed, speed);
		}
	}
}

If you want to dive into something more complicated, check out PID Controllers
Direct Link
Hope that helps!

3

Thanks for the advice!
But what are PID Controllers, and what is their purpose? The link on the original post of the thread leads to a 404 error.

Edit:

Later on the original poster provides another link, so nevermind about the dead link! I’ll start reading it right away.

Edit 2:
Oh I see what you mean now. I have kind of implemented that in the main autonomous task by calling a CancelDrift function that sets the motors in reverse for 200-300 milliseconds. But the guide describes a much more advanced method. I will definitely consider using this for the Programming Skills challenge (the regular autonomous zones work perfectly in the way I have them set up, and I’d rather not change them now).

4

Yeah, if you want to get into PSC, you will want to look into PID controllers for sure. For your drive, you could get away with a PD or even a P controller and get consistent results.

To go into a bit of detail about P/PD a controller, P stands for “Current Error” and D stands for “Change in Error”. By adding your “Error” and “Change in Error” together, you can more precise control over your positioning.
So to put some code behind that:


while(true)
{
  //Find how far from your target position you are.  
  float P = nMotorEncoder[LBW] - your_target_encoder_value;

  //How much change since our last update?
  float D = P - previous_P_value;
  previous_P_value = P;
  
  //How important is our "error"?
  float kP= 0.1;

  //How important is our "change in error"?
  float kD = 0.5;  
  
  //Assign the motor value by adding the "error" and "change in error" together!
  int your_motor_speed = P*kP + D*kD;
  setMotorPWM(your_motor_speed, your_motor_speed);

  //Stop running once we get close enough to our position
  if(abs(error) < some_error_tolerance)
    break;
}

Read up on George’s guide linked in my last post for more details on how everything works, but hopefully that helps explain the basic concept!

5

Yes, that greatly helps a lot, thanks so much!

Best of luck to you!