Hey everyone,
I’m designing a robot and I need some help programming it. The robot has some autonomouse functions that require it to move at a certain speed. The robot has 2 encoders and is four wheel driven. How do I use the encoders to keep the robots speed constant at 3m/s? I’m using VEX’s ROBOTC program to program a VEX Cortex microcontroller. It uses 4 CIM motors to drive each wheel.
Thanks for everyone’s help
In RobotC:
-First setup the sensors. There is a configuration tool in RobotC to setup the sensors and motors. It has tabs to configure what is connected to each port of the Cortex.
-In code, RobotC only returns the distance traveled by the encoder. The easiest way to get speed from distance is to find the derivative (dx/dt). The easiest way to do this is to keep dt constant (by using a Wait1Msec(20); for 20ms loop time) and calculate dx by comparing the value of the sensor now to the value of the sensor cached last iteration.
-By calculating the number of ticks per meter (you would need to know the radius of the wheel, gear ratio between wheel and encoder, and number of lines in the encoder to calculate this) and knowing dt you can calculate the velocity. I would just keep velocity in ticks/iteration and calculate the setpoint based on your desired speed in the units you wish (This reduces processor load while the code is running)
-You would then use a feedback controller to adjust the motor power based on the calculated velocity. The most complete way to do this is via a PID controller, although you can get decent performance (with a longer settling time) by just using the integral term. In this case, you would do something like this:
//Outside of the function, as a global (this cannot be reset each iteration)
int integrator = 0;
//Inside the function, running every 20ms
int velocity_set = 3000;//Speed you are trying to achieve - 3000 is just an example, you would need to calculate this in ticks/iteration
float ki = 1;//This parameter needs to be calibrated
int velocity_error = velocity_set - velocity_cur;
int power_output = velocity_error * ki
integrator += power_output;
//Limit the integrator to the valid range of +-127 to reduce windup, I'll let you figure this out
motor[1] = integrator;
If you have an encoder on both sides, I would recommend separate feedback controllers for each side. They should share the same setpoint and ki, but use different sensor velocities and integrators.
This entire post explains the methodology of the code, and the code example above is not complete enough to use as-is. If you would like more specific code/syntax help, feel free to ask.
Thanks!
ok so my
wheel radius is 4 in = .1016m making the wheel circumference .2032*pi meters
my gear ratio is 12.75:1
encoder is 250 ticks (US Digital encoder from AndyMark)
so to get ticks per meter it would be 250 ticks/(.2032*pi)meter but I don’t know where the gear ratio falls into this? My instinct is to either multiply or divide by the gear ratio.
Now in the code that you provided are only the variables in the timer loop? does the rest of your code happen after the 20ms? If you don’t mind showing me a bit more code I would really appreciate it
Sorry I couldn’t reply sooner, I was at the IRI.
-The gear ratio from the transmission output to the wheel is the only ratio you need. If you direct-drive the wheel with the same shaft as the encoder, or have a 1:1 chain or gear ratio between the encoder’s shaft and the wheel, you don’t need to include that ratio in the calculation. If you have any chain reduction between the wheel and the encoder, you will need to include it. The ratio between the wheel and encoder is NOT the same ratio as between the wheel and motors.
-The RobotC code would look something like this:
int setpoint = 9999; //This is the ticks/iteration you are trying to achieve
int integrator = 0; //This is the integrator value
int enc_last = 0; //This is the last encoder value
int enc_cur = 0; //This is the current encoder value
int vel_cur = 0; //This is the current velocity
int vel_error = 0; //This is the error in velocity
int pwm_out = 0; //This is the motor output value
float ki = 0.01; //This is the gain constant - You MUST tune this value
task main()
{
while(1) {
//Read the sensor into a cache var
enc_cur = SensorValue[MyEncoder];//You need to name the encoder in the sensors window
vel_cur = enc_cur - enc_last;
enc_last = enc_cur;//Store current encoder value for next iteration
//Calculate the error
vel_error = setpoint - vel_cur;
//Add to the integrator
integrator += (vel_error * ki);
//Cap the integrator to +-127
if(integrator > 127) integrator = 127;
if(integrator < -127) integrator = -127;
//Set the integrator to the motor
pwm_out = integrator
motors[1] = pwm_out;//I might have the syntax slightly off for the motors, check RobotC help
Wait1Msec(20);
}//Loop
}//End of task main
-These calculations are for one wheel. If you have two encoders (one per side), you should do two sets of calculations, one for each side. Make sure to re-name the variables to accomodate this. You can use the same task and loop.
-You should add more code so it dosen’t just run forward always (possibly a state-machine for the drive action?)
-The distance per count would be:
(wheel radius)(2)(pi)
(number of counts)
-The counts/iteration the program needs would be:
(desired m/sec) * (1/50) * (1/dist per count)
The 1/50 is the scaling for the time (as the loop is running at 50hz)
The 1/dist per count is the counts per distance
No worries I’m just greatful for the help. So a lot of this makes sense to me now. I am starting to see how this all works.
Now that I have the speed. To make it go a certain distance I would put the PID code into a while loop and this would keep looping until the encoders reach a certain distance kinda like this:
while (SensorValue[leftEncoder] < distance)
{
PID();
}
where PID() is similar to the example code you supplied, and distance is specified by the user. Is this right?
There are two ways to drive for distance:
-The way you show (except your example can only drive forwards)
-By controlling the speed setpoint based on another controller. In this way, you will ramp down the speed setpoint as you approach the target distance, to gracefully stop. You would do this with a simple proportional controller and range coerce, which is conceptually like this:
while(not_at_target) {
distance_error = distance_target - distance_traveled
int setpoint = distance_error * kp
if(setpoint > 9000) setpoint = 9000; //limit setpoint to a maximum velocity
do_speed_control();
}
Like the speed control, I would do this for each wheel (but condition the while loop based on either the maximum or average distance). This allows you to command a higher velocity to whichever side lags.
Since the example above does not have an integral term, I usually do the following:
-Lead the setpoint that the controller tries to achieve by setting it a few inches/feet in front of the point where you actually want to go, so you’re still applying a little power at the point you actually want to go (you can’t get stuck with too little power to move).
-Gracefully stop with a separate call to the velocity controller - I had a separate controller to allow me to tune the decel acceleration, so the robot would not jump as it quickly stopped. I also did this primarily with a PI controller since I cared more about stopping than holding a velocity of 0.
Both of those are more advanced topics which I am providing as food for thought. The example you showed will work fine (provided you think about negative distances).
another question.
does PID() keep the robot going straight? both are making the wheels spin so the robot travels the desired speed. So I think that this has both the right and left wheels spinning the same rate making the robot go straight. right?
and then for the in your while loop “while(1)” does that mean its always true? it’ll keep running this code?
In C and most languages I know of, the while() operator looks for a value equal to 0 or not equal to 0. A comparison operation (this == that) results in a 0 if they do not equal, or a 1 if they do equal, so that is one possibility. Another is an equate operation (this = that), which sets this to the value of that, and if the process does not fail (which it won’t unless you have a memory error) the result is 1, thus the while loop runs and you get angry trying to debug. So, a while(1) is the shortest syntax to make a while loop that always runs.
If both wheels are going the same speed, you will drive in a straight line. That is why you speed-control both sides separately, so they both go the same speed even if there are differences mechanically.
If the left and right speeds differ by one percent and you’ve got a 24" trackwidth, you’ll be 6 feet off the centerline by the time you go the length of the field.
so if i use the PID() code given earlier, would just changing
motor[1] = pwm_out
to
motor [1] = -pwm_out
would that make the motors go backwards? or how can this be done?
Not exactly. If the sign of the output does not match the sign of the sensor variable, you will end up with a negative feedback loop as the loop will forever get farther from it’s target.
The easiest way would be to invert the setpoint and let the control loop handle the motor inversion.