[usbcd36]

I struggled with using sensors to drive straight earlier this year, so I understand what you're going through. What you probably want is PD control, since integral control isn't really necessary for velocity.

Since I am not educated in the ways of easyC, I will have to explain this in English and pseudocode instead.

PD control is not straightforward when it comes to design, but it is easy to see how it works once it is implemented. The first thing to do is convert units. Get your robot up on blocks and find the maximum counts your sensors will read per slow loop (check forwards and backwards, both wheels/sides). This maximum count will be the max speed of the robot (ticks per slow loop is the unit).

MaxTicks = max number of encoder ticks per slow loop


Now convert the joystick values to be in encoder ticks per slow loop…

p1_y is the joystick value for this example

Vdes = desired velocity in encoder ticks per slow loop

Vdes = (p1_y - 127) * MaxTicks / 127

Subtracting 127 changes the neutral point to zero. Multiplying by MaxTicks/127 converts units.

Now that you have the desired speed for the wheels in units you can use, PD control can be applied.

There are two parts to velocity PD control: Proportional and Derivative.

Proportional control increases the speed based on the difference between desired and actual speed.

Ticks = amount of encoder ticks recorded since last slow loop

Error = Vdes - C

Derivative control dampens the overshoot caused by proportional control (it takes more time to accelerate the robot than it does to build up a large error).

ErrorPrev = previous value of Error

DeltaError = Error - ErrorPrev

(set ErrorPrev = Error after calculating DeltaError)


Now that you have the components of the PD routine, you can assemble them.

pwm01 = the motor whose speed you are setting

Kp = Proportional constant of proportionality
Kd = Derivative constant of proportionality

The constants above should be tweaked until everything works, and DEFINITELY put your robot up on blocks until it stops doing weird things. Don't be surprised if you hear some nasty sounds from your gearboxes when you inadvertently use a bad set of values, just disable the robot and try again. I got away with using Kp = 4 and Kd = 1, but your values probably will be different.

pwm01 = pwm01 + Error / Kp + DeltaError / Kd


Finally, make sure you don't let the value for the motor get outside of the range you want it to have, since this can cause undesired problems.

Another thing to watch out for: I don't know how picky EasyC is about variable types, but MPLAB/C is VERY picky, and I left out all of the typecasting necessary to make this work.