[Chris Hibner]

I would suggest shifting based upon the torque, not the speed. The problem is that torque is more difficult to measure than speed, but you can do a decent job using current sensors.

The algorithm would look something like:

if (torque > DownShiftThresh)
    Gear--;
else if (torque < UpShiftThresh)
    Gear++;

if (Gear > 4)
    Gear = 4;
if (Gear < 1)
    Gear = 1;

Shift(Gear);