Bei mein balance bot geht das wiederkeren ziemlich gut. Siehe mal video http://www.youtube.com/watch?v=a1EfgqLcNzc
Um den PWM-wert zu regelen nutze ich die summe von 4 factoren :
1. Winkel
2. Winkelgeschwindigkeit
3. Raddrehzahl
4. Position
Code:
void task_balance(void)
{	
	int16_t distance= (mleft_dist+mright_dist)/8;
			if (distance>100) distance=100;			//maximale correctie distance = 100*10/8=125 PWM
			if (distance<-100) distance=-100;		// komt overeen met 0.4*200= +/- 100 mm
	
	speed= mleft_speed+mright_speed-des_speed;	//max correctie = 180*10/4= 450 PWM, snelheid instellen
		if(speed>5) sollwert++;
		if(speed<-5)sollwert--;		//offset correctie in geval van grote weerstand en hellingen
	pwm=(sollwert-gyro_hoek)*P_balance/4+gyro_d*D_balance/64+distance*P_distance/8+speed*P_speed/4;//speed*P_speed/4;
	offset=e_compass/4;				//richting korrectie volgens e_compass
	if(offset>max_offset) offset=max_offset;		// begrenzing draaisnelheid
	if(offset<-max_offset)offset=-max_offset;
	pwm_L=pwm-offset;			//offset voor draaien 
	pwm_R=pwm+offset;
	
	if (pwm_L>10) pwm_L=pwm_L+100;	//correctie aanloopkoppel min PWM = 120
	if (pwm_L<-10) pwm_L=pwm_L-100;
	//if((pwm_L<=15)&(pwm_L>=-105)) pwm_L=7*pwm_L;
	if (pwm_R>10) pwm_R=pwm_R+100;	//correctie aanloopkoppel min PWM = 120
	if (pwm_R<-10) pwm_R=pwm_R-100;
	//if((pwm_R<=15)&(pwm_R>=-15)) pwm_R=7*pwm_R;
	setMotorPower(pwm_L,pwm_R);		//offset voor draaien robot PWM-L -offset, R+offset
	
	if((sollwert-gyro_hoek)>450)  {setMotorPower(0,0);program=0;}		//noodstop, robot is omgevallen
	if((sollwert-gyro_hoek)<-450) {setMotorPower(0,0); program=0;}		
}