# Value of degree to calculate range?

Hi everyone. I’m not sure if what I’m doing to get the degree for the tilt_servo to find the range of the bot to the rack is correct.
I wrote this in tracking.c at the very bottom:

``````unsigned float Find_Distance(void)
{
float range;
radian = ((((angle I will measure in degrees that the tilt servo has to offer from down to up) / 256) * pi) / 180) * pwm07(which is my tilt_servo pwm);
range = (116 - (distance I have yet to measure between the floor and my camera because we haven't placed it yet)) / tan(radian);
return(range);
}
``````

I read on another thread that the angle has to be in radians for the Tan() function to take in. Also, does it matter in which source file I place this function? SO, my real questions are, Does anything about what I wrote make sense? And how does this compare to the:
pan angle = ((current pan PWM) - (pan center PWM)) * degrees/pan PWM step ?(other than I’m looking for tilt rather than pan). Do I have to subtract my pwm07 by something like you did in this one? Sorry if I’m being confusing, but I’m trying to understand all of this at once. (My team is made up of 5 people and I’m the only electrician and the only programmer and I don’t have much experience as a junior in high school.)

i dunno where you learned that you need radians… i use degrees. if you look in tracking.H, you’ll see the upper and lower servo limits are (i think) 160 and 94. It also says that that’s approximately +25 deg and -25 deg. so degrees per servo step is 25/33. then, you need to find the number of servo steps either up or down. i did something like:

``````
unsigned float Find_Distance(void)
{
double angle;
double range;
angle = (TILT_SERVO-127)*.757575;
range = HEIGHT_DIFF */*that's 116- camera height*/* /atan(angle);
return(range)
}

``````

make sure to double check, but this should do it…

I wrote like Kevin’s suggestion:

``````float range_to_target(void)
{
float range;
range = HIGHT_OF_CAMERA - HIGHT_OF_TARGET/tan(TILT_ANGLE);//
return(range);
}
``````

We are looking for the Tilt_AngleMy tracking.H file says that the tilt min is 94 and the tilt max is 194. So, the center of that would then be 144, right? (It also says that Tilt_Center_Pwm_Default is 144). So, shouldn’t the equation then be:

unsigned float Find_Distance(void)
{
float angle;
float range;
angle = (TILT_SERVO-144)*.757575;
range = HEIGHT_DIFF /that’s 116- camera height/ /tan(angle);
return(range)
}

also, where did you get 25/33 from? where’d the 33 come from? Also, what does this mean?

``````#define TILT_SEARCH_STEP_SIZE_DEFAULT 50
``````

wouldn’t that mean that each step is 50, not 25? (And also its tan(angle), right? not atan(angle)…

I think it would be more efficient to just create a lookup table which maps PWM values to Range Values, you can do all the pre-calculations in Excel or something.

just create a static const array, it will be stored in read only memory so you don’t have to worry too much about the size of the array

``````
static const int lookup = { 1,2,3....4};

``````

where 1,2,3,4 are your range values

now say you only have a pwm range from 90-150 (61 different values)
lookup[60] would be the range value for pwm 150, so for the look up function you could do something like this:

``````
int getRange(int pwm)
{
if(pwm<90) {println("ERROR : unexpected lookup value!"); return -1;}
return lookup[pwm-90];
}

``````

I’d advise against using a double or a float for storing range in an array
because you really don’t need
that much precision (if the range is in inches) and it would just be a waste of memory

I dont understand whats wrong with what I did. I’m not using an arraylist or anything of that sort. I just want to know, for the pwm value at which the tiltservo is at NOW, what is its angle? thereafter, what is the range from the bot to the rack?

the <math.h> header has a tan() function in it. And in the standard header file the function accepts radians as an input. I have it coded this way, and it works reliably for me. The best thing you can do is to set the camera to its centers and then recenter the servos so you know for sure where the center values are.

There’s nothing wrong with what you did. Salik was just suggesting another way of doing it – one which has a couple of very good features, by the way. It lets you turn a tilt servo value directly into a range value just by reading from an array, without doing any time-consuming calculations in the program. It lets you compensate for any nonlinearities in the system, such as vertical parallax from the motion of the camera lens or a changing centroid bias when the size of the target images changes, by giving you a way to manually tweak values for each angle. And it lets you fill in the array without doing any trigonometry at all if you want to, by placing the robot on the field and actually measuring the distance associated with each tilt servo value.

How might I go about doing that then. I see now the advantages of it, but I am a first year programmer and I actually just learned what an array is last week in school. Any hints on how to set that up in the code would be extremely helpful and appreciated!

First off, the way you learn arrays in java probably is at a much higher level (you probably learn about Vectors/ArrayLists and other top level containers)

For robots we are dealing with simple arrays,
So like I mentioned above all you need to do is simply calculate your PWM->Tilt Conversion table (in Excel or manually)

If you want to do it manually you could printf the TILT_ANGLE , then measure how far your robot is from the light.
Collect a few key data points (20-30) then interpolate between them.

Then just create an array which has each range value in it.

Salik’s first post has examples you can follow.

First to clarify: Math functions in the C library work with radians, not degrees.

Here is excerpt from MPLAB C18 libraries manual:

tan
Function: Compute the tangent.
Include: math.h
Prototype: float tan( float x );
Remarks: Computes the tangent of x (in radians). A domain error occurs if the
argument is infinite or NaN. Both cases return NaN.
Return Value: The tangent of x.
File Name: tan.c

Attached is spread sheet for calculating a look-up table. Copy and paste the data from the Range Text column to create your array. See rangelu.c for example of completed array.

When you need to use the array declare as extern in that file.

extern const rom unsigned short targetRange];

If you use Kevin’s changes to increase resolution on the tilt axis. The number of table entries will need to increase and the PWM TO DEGREES factor will need to be changed.

TargetRange.xls (37 KB)
rangelu.c (857 Bytes)

TargetRange.xls (37 KB)
rangelu.c (857 Bytes)

So this is what I have so far. My pwmTILT is pwm07. My default center Tilt is 144 because the min is 94 and the max is 194. Also, the tilt_serach_step default is 50, so it makes 3 stops for tilt. Here is my code now, does this make sense?

``````const rom unsigned short targetRange] =
{
USHRT_MAX,  // zero index is undefined
5519,
2759,
1839,
1102,
918,
787,
688,
611,
549,
499,
457,
421,
390,
364,
340,
320,
302,
285,
270,
257,
234,
223,
214,
205,
197,
189,
182,
176,
169,
163,
158,
153,
148,
143,
139,
134,
130,
127,
123,
119,
116,
110,
107,
104,
101,
99,
96,
94,
91,
89,
87,
85,
82,
80,
78,
76,
75,
73,
71,
69,
68,
66,
64,
63,
61,
60,
58,
57,
55,
54,
53,
51,
50,
49,
48,
46,
45,
44,
43,
42,
40,
39,
38,
37,
36,
35,
34,
33,
32,
31,
30,
29,
28,
27,
26,
25,
24,
23,
22,
21,
20,
19,
18,
18,
17,
16,
15,
14,
13,
12,
11,
11,
10,
9,
8,
7,
6,
5,
5,
4,
3,
2,
1,
0
};

unsigned int Find_Distance(void)
{
return(targetRange[pwm07-67]);
}
``````

I subtracted the pwm07 by 67 because there is a 67 difference between the max_pwm (194) and the number of items in the array, so that means if the pwm is going at 194, then return the 127th item which is 0. So, does that make sense? Did I miss something or make wrong calculations?

(ALSO, my camera does not function properly. It turns on, everything is dandy, but it does not track the light well. It has only locked onto the light once out of many tries. The camera will often face quite to the left of the actual light and then it will go up and down in short steps very fast. Any suggestions?)

I actually just realized that I have to change the numbers in the array because I have to recalculate the height of my camera, so the numbers will be a little different. Lets say I’m working with this, though. What I’m worried about is this:

``````unsigned int Find_Distance(void)
{
return(targetRange[pwm07-67]);
}
``````

Is it right to subtract 67 from the pwm value? What should I be doing in order to translate the one index number out of 127 pwm indexes, or 90 degrees, into the range that I should have? Thanks

ahh no ones responding and im still confused!

Sorry to be a bother, but we’re stuck here. Thanks

So you should have
EXACTLY as many array values as the number of different PWM values your camera would possibly output.
I would put some bound checking too… because you will crash the program if you give it a bad index value!

You said that your tilt center is 144, and that tilt_max is 194. I’m assuming the robot won’t get tall enough tilt the camera down past the center. Is this correct?

Thus your array should contain 51
entries. 194-144 + 1 =51 … add 1 since the set is inclusive on both sides

So we want a value of 144 to point to array[0] and 194 to point to array[50] so all you need to do is subtract 144 from the PWM value

I’m not sure how you got 67 entries? can you explain.

OHHH I get it now. I got 67 from this: it takes …wait a minute…i have no idea where I got 67 form now that I think about it. When I did it, it made sense. But now that I know how to do it (and it makes A LOT of sense now) I can’t think of how to do it incorrectly in order to figure out where I got 67 from. If I think of it I’ll certainly let you know.
Here’s what I have now:

this segment of code is in User_Routines.c at the top before the functions (at the variable declarations):

``````const rom unsigned int targetRange] =
{
USHRT_MAX,  // zero index is undefined
53, //0
51,
50,
49,
47,
46,
45,
44,
42,
41,
40, //10
39,
38,
37,
35,
34,
33,
32,
31,
30,
29, //20
28,
27,
26,
25,
24,
23,
22,
21,
20,
19, //30
18,
17,
16,
15,
14,
13,
12,
11,
10,
10, //40
9,
8,
7,
6,
5,
4,
3,
2,
1,
0,
};
``````

Then I have this at the very bottom of user_routines.c:

``````unsigned int Find_Distance(void)
{
return(targetRange[pwm07-144]);
}
``````

Finally, also in User_Routines.c in Process_Data_From_Master_uP() I have this:

``````if(Get_Camera_State() == 1)
{
if(((Get_Tracking_State() == CAMERA_ON_TARGET) || (Get_Tracking_State() == TARGET_IN_VIEW)) && (Find_Distance() > 20))
{
pwm01 = 150; //Desired Forward Velocity
pwm02 = 150;
Switch3_LED = 1;
}
else if(((Get_Tracking_State() == CAMERA_ON_TARGET) || (Get_Tracking_State() == TARGET_IN_VIEW)) && (Find_Distance() <= 20))
{
pwm01 = 127;
pwm02 = 127;
I_Want_To_Search = 0; //Stops calling Servo_Track().
Switch3_LED = 0;
}
else
{
Switch3_LED = 0;
}
}
``````

When I tried this with the robot, it didn’t work. The pwm values did not change when the camera was locked onto the green light.
Any suggestions? And is the previous code for calculating the range sufficient?

Can you try using a different variable …are you sure pwm07 isn’t being overwritten to some other garbage value?
Try println what range outputs.

Is this always true?:
Get_Camera_State() == 1

I am almost certain it is a problem with one of those things, because your table looks perfect.

Table should have as many entries as you have pwm values to go from 0 to 90 degrees. Change the spread sheet to reflect your configuration. Camera height, degrees per pwm step, number of steps required to go from 0 to 90.

Then subtract your minimum pwm value from the current pwm value to get your index into the table.

Here is an example with some additional “bullet proofing”:

#define MIN_TILT_PWM 144

unsigned int Find_Distance(void)
{
unsigned int returnValue;
// using int here because value could go negative
int distanceIndex = (int)pwm07 - (int)MIN_TILT_PWM;

`````` // check to make sure we have a value that is in range

// are we too small
if( distanceIndex &lt; 0 )
{
returnValue = USHRT_MAX;
}
// are we too big
// note: sizeof an array divided by size of the first element gives size of the array
// array indexing is zero based so a 50 element array is accessed using index values from 0 to 49
else if( distanceIndex &gt;= sizeof(targetRange)/sizeof(targetRange[0]) )
{
returnValue = 0;
}
// we are just right
else
{
// go to the table to get our range value
returnValue = targetRange distanceIndex ];
}

return returnValue;
``````

}

Try a simpler version first. Maybe something that just drives forward if the light is in view and stops the motors otherwise. That will help you debug.

What’s the purpose of USHRT_MAX? Don’t fight with the C arrays, just start from zero. Your code expects a value for targetRange[0] when pwm07 is at its minimum of 144.

Jason