Why not measure the RPMs of the motor and up/downshift as necessary?
IE:
Code:
// Globals
long RPM;
#define MINRPM 10
#define MAXRPM 100
unsigned char Gear = 1;
#define MAXGEAR 4
#define MINGEAR 1
//...
// in some sub...
if (RPM < MINRPM) Gear--;
if (RPM > MAXRPM) Gear++;
if (Gear < MINGEAR) Gear = MINGEAR;
if (Gear > MAXGEAR) Gear = MAXGEAR;
// change gears, if necesary
[edit]This is all asuming the code can shift well and measure rpms acurately enough[/edit]