I have been asked by a few people about how we managed to accurately use the VEX gyroscope for our autonomous routines that we used at the world championship this year so I decided to share the ROBOTC code I used for reading and filtering the gyro’s rate of change and controlling our motors with PID. I have also included some example code that shows how to use it to turn a desired number of degrees.
I also would like to thank VEX for a great world championship, and everyone who helps make all of it happen. Shoutout to AURA, BNS and VCAT as well, the elimination matches were a blast and the finals were the best and hardest matches that I have had in my six years that I have been involved with the competition.
Can you explain the pid.epsilon variable and how it is used? It seems like it is a way to exclude the integral calculation when you are close to the target. This is different than a simple integral capped value.
Hello, as I’m sure Jason is still up on cloud nine right now, I can atleast answer this question for you :). Having worked with him a bit(former teammate) he believes that Ki is not very useful for vex motor control. I constants in vex tend to add oscillation to the motors, and you can be just as accurate without it. However, he created it as a pid loop to allow those who want to use all 3 constants to easily do so while using his library. Hope that answered. Also, thanks for the shoutout jay
The scale factor is because the actual gyro chip on the VEX product is actually operating at lower nominal voltage and scaled up to a voltage that can be used with the VEX Cortex controller. According to this post by Vamfun this voltage is 3V scaled to 4.533V, which is where that 1.511 comes from. I had a factor of 1.5152 during world championship this past week but I obtained that number from pretty crude voltage readings while testing with an Arduino UNO.
As far as how my Epsilon constant works as asked by Team80, you’re right, it is used to stop summing your error once you’re in a desired range of your setpoint. You don’t necessarily always have to sum, it makes sense to stop once your steady state error that your sensors are reading is small enough. It’s like telling the PID controller what range it can consider itself “there” at and then stabilize within that range.
Those constants that are in the example are the ones I used for the gyro. The kP constant had to be increased for smaller turns (less than 40 degrees). Greg was correct in stating that I find that the kI constant is better for applications other than VEX motors. Since many of the applications of motors that teams use them for do not have a constant steady state error except for perhaps linkages lifting an end effector, it is not that important to the system. What a PD loop is good at is getting you to your set point quickly and then the rate of change will swap signs at your set point and give your motors a jolt to stop them.
This jolt does use a bit of battery voltage, and we made sure that we used freshly charged batteries after every match (I would go for no less than 8.3V, but we only had 8.2V at the ready in the finals). We did have 8 motor drives though and they worked perfectly fine with that high of a kD constant. I was finding that most of the time the turning that the robots were doing was accurate to within a degree so the stress on the mechanical system and batteries was worth it for us.
The scaling factor of 1.511 for the VEX gyro comes from the fact that the chip on the VEX sensor operates at a nominal voltage of 3V but is scaled to 4.533V for use with the VEX Cortex microcontroller. See Vamfun’s Post for more details. I actually had a factor of 1.5152 at world championship this past week, which I obtained from crude analog reads from an Arduino UNO. That factor worked but was not the correct value.
The use of the epsilon constant as asked by Team80 does just what you thought it did, the controller stops summing once it is in a certain range of the set point. This allows you to define what range the controller should consider itself at the set point, and start to stabilize there. The output contributed by the kI constant will not go away, it will just not change while inside the range defined by the epsilon constant. It helps remove oscillation.
I did in fact use a PD loop. Greg made a pretty accurate claim about my opinions on when to use what constants. I find that the kI constant should only be used in situations were a good sized steady state error exists, such as a linkage or end effector that is being weighted down by some object that it is lifting, or temperature control. PD is fast and for us very accurate, we needed our auto to be fast and reliable but it was at the cost of stress on the mechanical systems of the robot and the use of a larger amount of power.
The way the turning worked for us is the motors would run at full speed through most of the turn and ramp down a bit when closer to the set point, then would receive a “jolt” in the other direction then start to stabilize and sometimes oscillate around the set point. The fast shaking that can happen is due to a high kD constant, while slower shaking is due to a high kP constant. This jolt uses a lot of power but we did not have an issue with consistency from it. We always used freshly charged batteries for each match (I would use no less than 8.3V but only had 8.2V ready for the finals) which allowed us to get away with using so much power on our 8 motor drives. We also had power expanders, so for teams that do not have them you may want to try slowing the controller down with a slew rate or tuning differently.
I would assume from their successful world final runs that either their codes really worked or they worked hard to make sure battery voltage was the same every time, which was not difficult to do for matches, but definitely difficult in practice.
Unofficial response: noticed that too and I am surprised… I always liked integral and I find some fine tuning with integral constant can increase precision noticeably. Well, with heavy control on the cumulative and explosive value. But I guess both AURA and QCC2 took out the I part simply because they do not have time to spare for I to make precise adjustments.
So for those out of the loop, I’m maintaining my ROBOTC controls lib here currently. I’ve recently pushed some experimental motion planning algorithms that were inspired by some of the new info on v5, the new victor SPX controllers and some of my own research. It will likely be deprecated in favor of the v5 onboard motor motion planning but I thought this would be a good learning experience and contribution for the time being.
I’m working on documentation for the new code, so expect that sometime soon.
Basically, there’s a single file that you can include to your program (motionPlanner.c) which is designed to provide precise position and velocity control of any motor/sensor pair in a background task in ROBOTC with minimal user effort/implementation.
It generates trapezoidal/triangular/s curves for motor output, which is theoretically the (fastest?) way to get a robot from one position to another (assuming no wheel slippage).
Code to set up a profile for a motor/sensor looks like the following:
#pragma config(Sensor, dgtl1, driveEncoderLeft, sensorQuadEncoder)
#pragma config(Motor, port2, driveMotorLeft, tmotorVex393_MC29, openLoop, encoderPort, dgtl1)
#include "./motionPlanner.c"
task main () {
startTask (motionPlanner);
createMotionProfiler (port2, getRawSensor (dgtl1), 720, 0.015, 600, 300, 30, 4);
setPositionController (port2, 7.0, 0.0, 0.50, 30, 150);
setVelocityController (port2, 0.1764, 0.0, 0.0, 50, 400);
setPosition (port2, 2000); // set desired position of motor
while (true) {
// you can do other things here without blocking the motion planning.
// if you set a position and leave it the algorithms will try and keep that position until asked to do
// something else
}
}
The function definition for createMotionProfiler is:
void createMotionProfiler (int port, int *sensor, int vMax, float Ka, int t1, int t2, int cycleTime, int positionCycles);
port - the port of the motor to set a profile for (ex. port2, driveMotorLeft)
sensor - a pointer to an integer representing a sensor. To get ROBOTC sensors, use the function:
getRawSensor (sensorPort)
vMax - the maximum velocity of the motor/sensor, in values per second (ticks per second on encoder for example)
Ka - acceleration constant, this gets added to motor output when accelerating/decelerating to help it follow its trajectory better
t1 - time spent accelerating/decelerating at the start and end of a move
t2 - time spent accelerating/decelerating the acceleration (ie. the time spent when setting the jerk; the time derivative of acceleration to a constant value, it’s zero otherwise)
cycleTime - the update period of the controller (a value of 20 runs the controller at 50Hz). You can’t set this very high on encoders unless you gear them to be more accurate (ie. chain them so they rotate faster than your drive), else you won’t be able to measure velocity.
positionCycles - how many loops to skip before updating position feedback (start this around 3-4)
The setPositionController/setVelocityController functions are self explanatory to those who understand PID: you set the tuning constants and inner/outer integration bands.
There are three control functions currently that can be used: setPosition, setVelocity and setPWMOutput:
void setPosition (int port, int position);
void setVelocity (int port, int velocity);
void setPWMOutput (int port, int output);
While the motionPlanner task is running, you can’t control the motors with motor[port], it will override that setting because it’s constantly looping to update the motors. If you set a position you can clear it by calling setPWMOutput (port, 0); which will stop the motor(s).
You can set motor ports to follow other ones using setMotionSlave (int motorPort, int masterPort).
There’s a bunch of other features that I’m planning to add still like correction to ensure a robot drives straight and specific integration with my gyro lib, maybe auto tuning the feedforward/feedback controllers, waypoints/2D trajectories, and an easier to use API.
Once I document this proper, I’ll make a tuning guide for it as well.
Let me know if there are any features that would be useful. Feedback is always appreciated as well.
@technik3k I’ve been thinking about how to improve on the smart motor library concept and make it more approachable to teams. I might have some free time to work on either testing or getting the constants that I need to implement the PTC estimation functionality that the smart motor lib has. Maybe instead of stopping/slowing down the motors, indicator lights or a speaker output would be more useful to some drivers. I’ll give it some thought.
You tune Ka after you set Kv. To get a good baseline Kv, you do 127/maxVelocity (encoder ticks per second for example), then you try out the curving. I like to use the logger in ROBOTC for testing. Then you adjust Ka until it follows the velocity set point really well. Ka just adds a bit/subtracts a bit from the motor output while accelerating/decelerating.
Jerk gets computed automatically per loop based on your t1, t2 values and max velocity. This does third orders-curves so it will set a constant jerk value in order to ramp acceleration up/down.
For really short moves where you can’t use an s curve with your predefined t1 and t2, a second order triangular profile will be used, which could have issues with jerk. Things like 90 degree turns will either need to be slow or deal with this.
Though PID control is nice that it is best to move the robot full power until it reaches the threshold. Therefore, it is faster to move the robot because it does not move with a low power when the error is small.
@JamesIsAmazing You would be correct in situations when suboptimal conditions that cause things like wheel slip do not exist. You generate these motion curves to minimize loss of traction and to improve repeatability.
My motion control library combines velocity feedforward with PID feedback. Your feedback output from a position PID controller is fed into a hybrid feedforward/feedback velocity controller (error is feedforward, integral/derivative terms are feedback if you want to use them). The position set point ramps up as you move so your error is always small (see the datalog graph above). This allows you to have really aggressive gains (In testing I’m finding that you set Kp between 1.0 to 10.0 usually, but it would be something like 0.5 or less if just using PID feedback and no velocity feedforward).
So, pardon my ignorance (I’m bad at programming and trying to get better), but it sounds like I can use the setPosition(port, destination) to tell any motor to go to any distance as long as it has a sensor and profile for it. In regards to a competition-template, would the motor/sensor profiling occur in the pre-auton? If I do that for a bunch of motors, could I then use them anywhere in the usercontrol or autonomous sections? What would I have to change if I tried to implement it in a competition code?
This looks like a really awesome and useful thing for controlling a robot…it blows my mind how complex it all is.
@Royal_xD yeah it’s a really heavy problem for ROBOTC, I struggled to make it function properly while still being as configurable as it is due to ROBOTC’s limitations.
The profiling is done in a background task in real time, so you can use it for auton/competition with no changes. When you call createMotionProfile() it will automatically start the required tasks and be ready to go.
I’ve retooled the configuration functions quite a bit in order to make them more user friendly. For example, let’s say you wanted to set a profile for a four motor turbo drive (ports 2 and 9 on left, ports 3 and 8 on right) using encoders (dgtl1/2 for left, dgtl3/4 for right). Here’s the initial setup for that configuration:
createMotionProfile(port2);
createMotionProfile(port3);
profileSetSensor (port2, dgtl1);
profileSetSensor (port3, dgtl3);
//set max velocity in ticks (sensor units) per second
profileSetMaxVelocity (port2, 1728); //120rpm * 360 ticks per rev / 60 seconds = 1728 ticks per second
profileSetMaxVelocity (port3, 1728);
//sets motors to follow other motors/profiles. Reversed flag swaps direction of this motor
//<port>, <master to follow>, <reversed>
profileSetMaster (port9, port2, false);
profileSetMaster (port8, port3, false);
Ports 8 and 9 would be set to follow ports 3 and 2 respectively.
createMotionProfile will also autodetect encoders defined in the ROBOTC motors and sensors setup, but not the type/velocity. So you can skip profileSetSensor if you want for encoders configured this way, but you still would need to set the maxVelocity parameter.
To tune the control gains and features, there are a handful of functions:
//would cap the speed during a move to 1200 ticks per second, defaults to maxVelocity above
//moves are computed with using this speed as the max, and will take longer if this value is lowered
//calling profileSetMaxVelocity resets this speed limit
profileSetSpeedLimit (port2, 1200);
//a gain that makes the motor try and follow the acceleration cuve more aggresively
//higher values mean that more power will be added when speeding up/slowing down to help "stick"
//to the speed curve
profileSetAccelerationGain (port2, 0.15);
//time in milliseconds to ramp up to max speed for a move
//think of this like a "slew rate", or a 0-max speed time
//defaults to 1000, or 1 second from 0 to max speed
profileSetAccelerationTime (port2, 1000);
//sets the rate (in samples per second, Hz) in which the controllers try and sample and set motor output
//defaults to 50Hz
profileSetSampleRate (port2, 50);
//the position PID controller needs to run slower than the velocity PID controller
//this function sets the number of velocity cycles that run for every position update
//defaults to 4, should be 3 or higher usually.
profileSetPositionSampleTime (port2, 4);
//sets the amount of acceleration "smoothing" that is done. A value of 0 makes the speed curve
//a sharp trapezoid, a value of 1 makes it a very smooth "S" shape
//defaults to 0.5, can range from 0.0 to 1.0
profileSetJerkRatio (port2, 0.5);
//basically calls SensorValue [sensorPort] = 0;
//may have strange results depending on the type of sensor being used
//when in doubt, just reset the sensor manually if necessary
profileResetPosition (port2);
//sets position PID gains and integral cutoffs
//the integral cutoffs define a range in which the position error is summed and used for the I component.
//loops with an error value outside the defined range leave the errorSum at the previous value
//<motorPort>, <kP>, <kI>, <kD>, <innerICutoff>, <outerICutoff>
//these are the default position PID settings
profileSetPositionController (port2, 3.0, 0.0, 0.0, 30, 150);
//sets velocity PID gains and cutoffs
//these are the default velocity PID settings, except for kP.
//kP is set to (127.0 / maxVelocity) whenever profileSetMaxVelocity is called, but can be overridden here.
profileSetVelocityController (port2, 0.0, 0.0, 0.0, 50, 500);
You can plot the output curves in ROBOTC’s datalogger like my above post by calling the function below. You can only log one motor at a time due to there only being so many datalog series.
while (true) {
profileLog(port2); //port2, or whatever other motor you want
delay (100); //can be any delay you want
}
There’s an advanced feature that allows users to use a pointer to a variable (int or float) in place of a sensor port, in case you’re doing some sensor processing. Once you set this, you can update the value of the variable you used and the controller will detect the changes automatically.
To use the motion profiler to execute moves or control the speed of motors, you call the following functions:
//calculates and executes a move from the current position to the desired position
//two consecutive calls to profileGoTo with the same position value do not stack, it's absolute positioning
//reset the sensor value to 0 in between moves to get relative position control
//<motorPort>, <position>
profileGoTo (port2, 3000);
//sets the target velocity of a motor.
//The motor will try to run at that velocity until another command is executed
profileSetVelocity (port2, 1500);
//sets the raw output of the motor. Equivalent to motor[port2] = value;
//You need to use this function to set the motor value of a profiled motor, else it will be overridden
//This value is linearized using a TrueSpeed lookup table (63 is ~50% speed, 32 is ~25% etc...)
profileSetMotorOutput (port2, 127);
If using the red quadrature encoders (the external ones), be aware that ROBOTC only uses a 16 bit signed integer to store the value of the encoder, so it will overflow after 32,767 ticks. If you want to use a red encoder at higher resolution (by using a gear reduction), you can assign an encoder to a motor in the motors and sensors setup, and then assign the value of getMotorEncoder(nMotor) to a variable and use profileSetSensorPtr() with that value in order to get a 32 bit value (getMotorEncoder() goes all the way to 2,147,483,647). Pro tip: the integrated encoder functions work on red encoders if they’re configured in motors/sensors setup (getMotorEncoder, resetMotorEncoder, etc…).
