Hello, I am a VEX IQ team and I am starting to use Odometry. Here is my code for the odometry - Github for the odometry, I also use the PID Control system to move towards the targets. In the code you can see that I set two positions I want to go to. First I want to go to -2, 4. When I do this the robot immediately turns right. If I reposition the robot to the left, the robot appears to go to 2, 4. Then I tell the robot to go to 0, 2. When this happens the robot starts spinning uncontrollably, any ideas?
To debug it’s best to split down your code into smaller parts and test them individually. You can try this by first disabling odometry and making sure the PID works, then disable PID and make sure odom works, then testing with odometry and PID turning but not PID driving, etc.
I can’t tell exactly what is causing the problem from your description. Could you maybe send a video of it driving or give more information like what is printing on the brain? From looking through your code I did notice a few things though, but I don’t think any of them are causing the problems you’re describing.
These lines of code are assuming theta is in degrees but on the line above this you convert it to radians:
if (theta > 180 || theta < -180) {
theta = 0;
robotReverse = true;
}
Your odometry code shouldn’t need to check if you are going in reverse anyways, my team’s odometry (and I think most other teams) works without that.
Also, you apply ki to the integral over and over again every tick and it keeps stacking every time the loop is ran, meaning if ki is less then 1 it will get very small very fast and if ki is > 1 it will get very big very fast. You should instead multiply the integral by ki whenever you calculate the output of the PID, not when you add the current error to the integral. This isn’t causing the problem though because ki is 0 for both driving and turning.
The exit condition of the loop in the autonomous function also seems like it could cause problems, because you check if the x position or the y position is right, meaning the robot could be lined up on only one axis but very far off on the other one and the loop will end.
Lastly, you have two different versions of odometry, one in the autonomous function and one in it’s own function. It makes the code a lot easier to work on and debug if you just have one version of odometry. I would make the one in it’s own function not stop during autonomous and remove the odometry from the autonomous function.
Thank you for all of the help. I started to change the code yesterday and I also saw the thing about theta not being in degrees. I changed that. Also it does need to check if it is going in reverse because if you look at the next lines of code
if (robotReverse == false) {
change_in_x = dist_moved * cos(theta);
change_in_y = dist_moved * sin(theta);
}
else {
change_in_x = dist_moved * sin(theta);
change_in_y = dist_moved * cos(theta);
}
you will see that the math is different if the robot is reversed. This is because triangles cannot be greater than 180 degrees. If that was so then the triangle would go to the other side (the reason why we change from sin to cos and cos to sin). I do also need to change the math because this would change the x to y and the y to x. What I need to do is find the difference. So if theta = 190 in degrees then it would subtract theta by 180 or 10. Then I subtract 180 by 10 and get 170. Then I would multiply that by -1. Also in these lines
if (target_angle_odom != target_angle) {
angle_difference = target_angle_odom - theta;
heading_error = angle_difference * (180 / pi);
}
else {
angle_difference = (target_angle * (pi / 180)) - theta;
heading_error = angle_difference * (180 / pi);
}
The target_angle_odom is in radians. I would need to convert this to degrees to make this work. Also I am going to fix the exit loop. Also I will fix the ki problem. I will continue to try and find the problem, and send a video when I can. However, I am still working on the robot and I am going away on a trip soon. The updated and fixed code should be uploaded to github soon.
Thank you for the detailed response to my problem!
The math actually doesn’t need to change when you are going in reverse. You are right that triangles can’t have angles greater then 180 degrees, but it is still fine to make the angle go above 180 because we aren’t just solving for a triangle normally. We are instead using a unit circle, which is a circle with a radius of 1, and the trig functions allow us to turn any angle into an x and y position on the circle (image below)
When the robot is at any angle from 0 to 90 degrees the trig functions both give positive values, meaning it is going up and to the right. Then when the robot is going between 90 and 180 the x position becomes negative, meaning it is going up and to the left. This is what we want, because then the odometry still works when the robot is going at any angle.
Here’s a interactive desmos graph which should hopefully help explain it if my explanation didn’t make sense:
With my math I am not using a unit circle. I am just using basic trig functions sin(theta) = opposite/hypotenuse or hypotenuse * sin(theta) = opposite or y. I could try to convert my functions into using a unit circle or I believe it is called converting them into arcs. However from my knowledge this assumes that each line I am taking is curved. I only know basic trig because I am still in middle school and this is all new to me sorry if I am being stupid. Also if you could, could you give me an example of how I can convert my functions into using the math you are describing?
Also with the ki error, you are saying that I should multiply the output of the PID by a ki or multiply integral_heading + heading_error by a ki. Because currently that is what I am doing, I am multiplying (integral_heading + heading_error) * ki, or equal to the same thing as integral_output * ki. Also if i multiplied it by the pid output I would have the same problem since I calculate the pid output and integral output at the same time. There is only some lag for doing the calculations, probably a couple milliseconds or even nanoseconds. Could you provide some clarification?
Also in the last paragraph where you talk about testing the odometry and PID separate I don’t know how I would do that because I want to test moving the robot towards the values given from odometry. To that I need a controller, and my controller of choice is PID. Also only the autonomous function is actually running when I started the code because the if (!isAutonomousRunning) { } inside of the updatePosition function prevents the code from running when the autonomous function is running, so it wouldn’t help to consolidate this into 1 function. Also I would continue to calculate where the robot is on the field even if the autonomous function is not running. This is so that if I decide that I want to run the autonomous function after it is stopped and something hits the robot or we use the controller on the robot to move it while the autonomous function is not running then I would like the robot to still move towards the correct position
Ok, sorry for all of the messages, but I started to read even more on odometry in the vex forum, and it seams like I am calculating the y values wrong. Here is a desmos graph explaining the math I believed I was using - Odometry | Desmos. I found it from this post - Odometry Code? - #21 by iseau395. In this desmos graph to find y you need to do sin (theta + pi/2) * difference between left and right wheel position. However now I am confused because what I thought was that I had the length of the hypotenuse and to find the length of the opposite and adjacent sides of the triangle, or the x and y position I just use cosine and sine like I explained 3 posts above.
Yeah the math for the unit circle and for triangles is actually two different visualizations of the exact same thing. You don’t need to make any changes to your math besides removing the part where it checks if the robot’s rotation is > 180 degrees and swaps the trig functions, I just brought that up to help show that it still works with angles more then 180 degrees. Odometry can be done with arcs but that’s a lot more complicated, and honestly not that much more accurate (my team has done both). Sorry if I confused you
Right now the code is almost right, it’s like this right now:
integral_heading = (integral_heading + heading_error) * ki
output = proportional_heading + integral_heading + derivative_heading
Let’s walk through the code to see what it’s doing right now. For the sake of the example I’m going to make integral_heading start at 0, ki start at 100, and heading_error start at 90. On the first loop of the code integral_heading is set to (0 + 90) * 100, or 9,000. The next loop of the code moved 10 degrees closer to the target, so error is now 80. When this line is ran again it plugs in the output from the last run back in and calculates(9000 + 80) * 100, or 908,000. This number is way bigger then it should be (it should be 17,000) because the value we plugged in for integral_heading is the version which had already been multiplied by ki last tick. Instead you should calculate the integral like this:
integral_heading = integral_heading + heading_error
output = proportional_heading + integral_heading * ki + derivative_heading
This makes sure that ki isn’t multiplied multiple times
TLDR: every tick you re-multiply by ki, meaning it’s stacking the multiplications. integral_heading should be set to a value which isn’t multiplied by ki and instead multiply it by ki later
You can test odometry separately by disabling the PID then printing the x and y positions odometry thinks it’s at on the brain screen, then just drive the robot around with the controller and make sure the position that’s printed is consistent with what you think should be printed. For example, you can put a tape measure on the floor and drive the robot 12 inches forward, then see if your robot’s position moved 12 inches.
Yeah but the update position function is in a second thread, meaning the code can run at the same time as the autonomous function if you want it to. This allows you to run odometry in a seperate function at the same time as autonomous so you don’t need to have to seperate versions of it. I would research multithreading to help with this (this thread is specifically for VRC but it still applies to IQ because the programming is very similar)
Sorry if that post confused you, I made that example based on odometry with 2 tracking wheels, which is common in VRC. This means there are two triangles as you can see on the right, one which uses the data from the forward/back tracking wheel and one which uses the data from another tracking wheel rotated 90 degrees relative to the first one. Because you don’t have any sideways data, you only need to deal with 1 triangle, so you can ignore the second one. The way you were doing it before is right.
Just in case I am confusing you with all my lengthy explanations here’s some pseudo code of basically all you need for odom:
var last_left = getLeft()
var last_right = getRight()
loop {
var left_change = last_left - getLeft()
var right_change = last_right - getRight()
last_left = getLeft()
last_right = getRight()
var distance_traveled = (left_change + right_change) / 2
var theta = getRotation() * (pi / 180)
// if 0 degrees is directly to the right and positive angles are counterclockwise, do the next 2 lines
x_position = x_position + distance_traveled * cos(theta)
y_position = y_position + distance_traveled * sin(theta)
// if 0 degrees is directly up and positive angles are clockwise, do these 2 lines instead
x_position = x_position + distance_traveled * sin(theta)
y_position = y_position + distance_traveled * cos(theta)
wait(10, msec)
}
Alright, thank you! I updated my code with the changes you wanted me to make and I will be testing it after New Years. I still have the two versions on odometry and PID, but I am only using the updatePosition function currently to test the odometry separately from PID. Here is the code if you want to take a look. Also shameless plug for our blog where I made a post on our code.
Everything works now thank you guys!
Hey birb, I am also on a Vex IQ team and would like to get started on odometry. Do you know where you learnt so much about odometry even in Vex IQ with the limited sensors. Also did your team have 2 motors specifically for odom or just used the drivetrain encoders. Do you have any resources to get started on learning and coding for odom and PID together for IQ. If there is anything useful that you used to get to know so much about odom could you please share it? I practically only know the basics and would like to deepen my knowledge.
Hey so sorry for the very late response, but if this doesn’t help you know, maybe it’ll help someone else. I’m now in highschool and have learned A LOT. For getting into odometry it is vital to know trigonometry, if you haven’t learned it in school which you probably haven’t if you are doing IQ as you are in middle school and would like to learn khan academy is a great learning tool, here is their lesson on right triangle trigonometry. It is actual pretty intuitive once you get the hang of it, all you gotta do is use SOH CAH TOA. Once you know that you can do basic euler integration (just adding up the delta_positon each timestep, delta just means change in position) to find the robot’s current position given an initial position. This part is pretty intuitive if you know right trig. The equations for this odometry (which isn’t that accurate, especially if you are doing v5) are shown in controls-engineering-in-frc. Now for arc odom there are 2 main ways to get the equations, pose exponential odom with constant acceleration (which I wouldn’t worry about as it simplifies to the exact same thing and requires an understanding of differential equations and the complex plane to some extent) and arc odom. Arc odom has a couple ways that it can be proven, law of sines, law of cosines, half the arc, etc. Khan Academy has a lesson on general trigonometry, not only right triangles which would similarly make arc odometry intuitive. Anyways for that odometry there are a ton of articles and videos that go over it:
http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf
Lmk if you have any questions!