Program for variable speed

[cabbagekid2]

For our previous robots, we've been implementing variable speed for the drive motors by using the wheel on the player one joystick. Since we took a break from FIRST last year, we did not have a chance to convert our pbasic code for the variable speed to C. When we tried, the "robot" (platform with wheels) did not follow the code at all. What happened was when p1_y became greater than 147, p1_y, pwm13, pwm 15, and p1_x all became 127 no matter where the joystick was positioned. The same happened when p1_y became less than 107. Is the algorithm to calculated the new p1_y too complex?

Could someone look at our interpretation of the code and help us find our problem? Any help would be much appreciated, thanks!


Previous PBasic Code:
minfwd con c127+5
minbak con c127-5
c2k con 2000
c2kc254 con c2k+c254


XYAdjust:
Gosub WAdj_y
Gosub WAdj_x
return

WAdj_y:
if p1_y => minfwd then yForward
if p1_y <= minbak then yReverse

p1_y=c127 'yNeutral
return

yForward:
p1_y = (p1_wheel)*(p1_y-c127)/c254+c127 max c254
return

yReverse:
p1_y = c127-((p1_wheel)*(c127-p1_y)/c254)
return

WAdj_x:
if p1_x => minfwd then xForward
if p1_x <= minbak then xReverse

p1_x=c127 'yNeutral
return

xForward:
p1_x = (p1_wheel)*(p1_x-c127)/c254+c127 max c254
return

xReverse:
p1_x = c127-((p1_wheel)*(c127-p1_x)/c254)
return

PWM15 = (c2k + p1_y - p1_x + 127) Min c2k Max c2kc254 - c2k
PWM13 = (c2k + p1_y + p1_x - 127) Min c2k Max c2kc254 - c2k


C Code placed in user_routines.c:
unsigned char direction = 0;
unsigned char p1_top_last = 0;

if (p1_y >= 147)
p1_y = ((p1_wheel)*(p1_y - 127)/254) + 127;
else if (p1_y <= 107)
p1_y = 127 - ((p1_wheel)*(127 - p1_y)/254);
else p1_y = 127;

if (p1_x >= 147)
p1_x = ((p1_wheel)*(p1_x - 127)/254) + 127;
else if (p1_x <= 107)
p1_x = 127 - ((p1_wheel)*(127 - p1_x)/254);
else p1_x = 127;

pwm13 =Limit_Mix(2000 + p1_y + p1_x - 127);
pwm15 = Limit_Mix(2000 + p1_y - p1_x + 127);