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)
}