[Mr. Lim]

The lookup tables suggested earlier are definitely the way to go.

If you're looking for a quick and dirty method to test things out, you can also try:

temp_p1_long = (signed long)p1_y - 127;
pwm01 = (unsigned char)(((temp_p1_long * temp_p1_long * temp_p1_long)  >> 14) + 127);

temp_p2_long = (signed long)p2_y - 127;
pwm02 = (unsigned char)(((temp_p2_long * temp_p2_long * temp_p2_long)  >> 14) + 127);

It's a cubic transfer function that we've used since 2004. If it works for you, you're eventually better off calculating the values from the function, and using a look-up table for competition. The long integer operations are a bit wasteful, but it's four quick lines of code for testing!