Program for variable speed
 

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

Re: Program for variable speed
 

This is using integer arithmetic. The division by 254 gives an intermediate result of zero, so the final answer is always 127.

You need to either a) force floating point arithmetic by dividing by 254.0, or b) scale the intermediate calculations up by multiplying by a constant (e.g. 256) first and dividing by the same constant just before adding the 127.

Re: Program for variable speed
 

and for what its worth, sticking with integer division is almost always the best choice... floating point division on this processor is surprisingly slow, having no fpu.

Re: Program for variable speed
 

The multiplication by p1_wheel should happen first by order of operations (left-to-right). You could try adding parenthesis around the (p1_wheel)*(p1_y - 127) part and that may fix it. My other thought is that you may be doing 8-bit arithmetic and not 16-bit arithmetic. You might want to move p1_y and p1_wheel into int's first so that it's guaranteed to be 16-bit arithmetic and not 8-bit.

Matt

Re: Program for variable speed
 

So would this be the same if I just added parenthesis and changed the formula to this:

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

or is this what you were trying to say?

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

Thanks, I shall try these out!

Re: Program for variable speed
 

That was one of our thoughts about the problem....that it was doing the calculation and somehow losing the number. We didn't know how to solve this problem though. Thanks for the suggestion. We'll try creating new 16-bit variables and try this again.