Schöne Aufgabe. Ich mache das so.
Code:
#include "asuro.h"

typedef struct { int soll, ist, pwm; } TSTATE;
TSTATE left={0,0,0}, right={0,0,0};

int regelnP(TSTATE* x, int delta) {
	x->pwm+=(delta*x->soll - 1000*x->ist)/1000;
	x->pwm= (x->pwm>127) ? 127 : ((x->pwm<-127) ? -127 : x->pwm);
	return x->pwm;
}

int main(void) {
	Init();	
	EncoderInit();
	EncoderStart();	
	
	left.soll=40; right.soll=60;
	long lastTime=0;
	while(1) {	
		left.ist=encoder[LEFT];
		right.ist=encoder[RIGHT];
		int delta=Gettime()-lastTime;
		if(left.ist<10 && right.ist<10 && delta<50) continue;
		
		EncoderSet(0, 0);	
		SetMotorPower(regelnP(&left, delta), regelnP(&right, delta));
		lastTime+=delta;
	}
}