Man kann ASURO bei Allem - also recht modular - damit sogar geregelt (hier einfacher P-Regler) fahren lassen:
Code:
int leftSpeed=0;
int rightSpeed=0;
long motorTime=0;
int delta=10, Kp=20; // entspricht Kp=0.2

void drive(int leftSpeed_, int rightSpeed_) {
	leftSpeed=leftSpeed_;
	rightSpeed=rightSpeed_;
}

int regeln(int pwm, int e) {
	int y= (Kp * e)/100;
	y= (y>10)? 10 : ((y<-10) ? -10 : y);
	pwm+=y;
	return (pwm>127)? 127 : ((pwm<-127) ? -127 : pwm);
}

int motor_Awake(int idx) {
	return Gettime()>motorTime;
}

int motor_Control(int sensor) {
	static int lpwm=0;
	static int rpwm=0;
	int l_ticks, r_ticks;
	int Ta=Gettime()-motorTime;

	l_ticks=encoder[LEFT];
	r_ticks=encoder[RIGHT];
	EncoderReset();
	motorTime=Gettime()+delta;
	
	lpwm=regeln(lpwm, leftSpeed -800L*L_DIR*l_ticks/Ta);
	rpwm=regeln(rpwm, rightSpeed-800L*L_DIR*r_ticks/Ta);
		
	SetMotorPower(lpwm, rpwm);
	return 0x1;
}

und dann später

FunctionPointer actionList[] ={
	/* sense	  ,	action */
	motor_Awake,	motor_Control,
	...
	cruise_Forever,	cruise_Action,
};