Odom help!

Programming Support VEXcode Pro V5 Text Tech Support
,
1

I looked at a bunch of tutorials and other stuff before attempting this, and I have no idea if this will work or not - i barley even understand what cos and sin mean in the first place. I do know that theta is the 30 degree angle in the popular 30 degree right triangle, and I know that cos is the adjacent angle of a right triangle divided by the hypotenuse of a right triangle, and how sin is the opposite angle of a right triangle divided by the hypotenuse. please help and point me into the right direction for how to continue and/or fix this odometry code!

float tc = 0.015;
float D = 0;
float prev_D = 0;
float dc = 0;
float L = 0;
float x = 0;
float y = 0;
float r_pos = 0;;
float l_pos = 0;;
void odom1stattempt(int xa, int ya, int theta){
  while (1>0){
    x = xa;
    y = ya;
    wait(tc,seconds);
    prev_D = D;
    r_pos = ((r_b.position(degrees))+(r_f.position(degrees))+(r_t.position(degrees))/3);
    l_pos = ((l_b.position(degrees))+(l_f.position(degrees))+(l_t.position(degrees))/3);
    D =  2 * ( ( ( r_pos + l_pos ) / 2 ) * ( (3.14159265 * 3.25) / 360)) * 3.25 * ( 15 / 8.8 );
    x = x + D * cos(theta);
    y = y + D * sin(theta);
    
  }
}
2

I am not sure where you got this from, as you don’t set theta to 30° anywhere in your code. Theta is normally used as a generic variable like x and y but for angles.

You should use floats or doubles for these, as int is not very precise.

You should put the wait at the end of the code, so it doesn’t pause in the middle of the loop, and move the pred_D = D; line to right before the wait.

What are these extra numbers for? It seems like your distance calculation is correct without them (assuming 15/8.8 is your gear ratio), as you already accounted for the wheel size, and I am not sure what the 2 is for.

Besides these things, your code looks like it will work as a motor encoder odom, and you are using sin and cos correctly. Also, make sure that you are running odom1stattempt in a separate task, or else the while loop will run forever and the code after odom1stattempt is called will never execute.

2 Likes
3

In addition to what Max said, make sure that your theta variable is in radians. A inertial sensor will only give degrees so you will have to convert it by multiplying by 180/pi.

explanation of radians

Radians are an amazing part of math! Do not learn to hate them because they will show up time and time again. 360 degrees = 2pi radians. Why are radians so important you may ask. This is because the special way a radian is defined. If you have an arc (a part of a circle) of radius 1 with an angle of 1 radian, the length will be 1! Basically, the length of an arc is the angle (in radians) of the arc times the radius. If we apply this example to a circle, 2pi radians * a radius of r, we get 2pir, the formula for circumference of a circle. It is for this reason (and some more) that trig functions, by convention, often use radians.

3 Likes