Hallo

StateChanged-Funktionen und das ACS werden vom Tasksystem aufgerufen. Deine Programmversion ruft das Tasksystem aber nur alle vier Sekunden auf. Fährt der RP6 so überhaupt? Das ist der Punkt warum sich die blockierende Servoansteuerung nicht gut für den RP6 mit seinem Tasksystem eignet.

Eine mögliche Lösung wäre vielleicht, das Tasksystem zusätzlich während der Impulspause des Servos aufzurufen und die sleep()-Pause anzupassen. Blöderweise schwankt aber die Ausführungszeit von task_RP6System() und damit auch die Länge der Impulspause des Servos. Hier muss man wohl etwas mit der Länge des sleep() spielen.

Wenn man das Tasksystem verwendet, sollte man auf gleichzeitige sleep()- und mSleep()-Befehle verzichten. Hier eine ungetestete Version deines Programmes mit einer Stopwatch:

Code:
#include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

void acsStateChanged(void)
{
	setLEDs(8);
	if(obstacle_right && obstacle_left) // beide Seiten?
		rotate(60, RIGHT, 100, true);
	else if(obstacle_left) 					// nur links
		rotate(80, RIGHT, 50, true);
	else if(obstacle_right) 				// bleibt nur noch rechts übrig
		rotate(80, LEFT, 50, true);
		
	changeDirection(FWD); 					// weiterfahren
	moveAtSpeed(100,100);
}

void bumpersStateChanged(void)
{
	setLEDs(1);
	if(bumper_left || bumper_right)
	{
		changeDirection(BWD);
		move(100, BWD, DIST_MM(200), true);
		rotate(50, RIGHT, 120, true);
	}
	changeDirection(FWD);
	moveAtSpeed(100,100);
}

int main(void)
{
	initRobotBase();
	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b001001);
	DDRA |= 1;
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	ACS_setStateChangedHandler(acsStateChanged);
	powerON();
	setACSPwrLow();
	
	changeDirection(FWD);
	moveAtSpeed(100,100);
	servoposition=0; 									// 0 ist sleep(8), 1 ist sleep(23)
	startStopwatch1(); 								// Stoppuhr starten

	while(1)
   {
		setLEDs(0b100100);
		task_RP6System();
		sleep(50); // das Tasksystem muss man nicht so häufig aufzurufen
		
		if(getStopwatch1() > 2000) 				// zwei Sekunden sind vorbei
		{
			setStopwatch1(0); 						// Stoppuhr zurücksetzen
			setLEDs(0b010010);
			for(i=0; i<50; i++) 						// ein Sekunde lang Servo bewegen
      	{
         	PORTA |= 1;
         	if(servoposition) sleep(23); else sleep(8);
         	PORTA &= ~1;
         	sleep(100); 							// Impulspause ist sleep() und Task zusammen!
         	task_RP6System(); 					// beim Servoschwenken weiterfahren
      	}
      	if(servoposition) servoposition=0; 	// andere Servoposition wählen
				else servoposition=1;
		}
   }
   return(0);
}
Eine zittrige Servoansteuerung nur mit Stopwatches (und einer Zählschleife für die Impulslängen) hatte ich hier mal versucht:

Code:
 #include "RP6RobotBaseLib.h"

#define pos1 800
#define pos2 1600

extern uint8_t acs_state;

uint16_t servo_pos, servo_timer, servo_dummy;

int main(void) {

   initRobotBase();
   DDRA |= 1;
   
   startStopwatch1();
   startStopwatch2();
   servo_pos = pos1;
   
   while (1)
   {
      if ((getStopwatch1() > 20) && (acs_state < 2)) // alle 20ms einen Servoimpuls erzeugen
      {
         setStopwatch1(0);
         servo_timer = servo_pos;
         PORTA|=1;
         while(servo_timer--) servo_dummy ^= servo_timer;
         PORTA&=~1;
      }

      if (getStopwatch2() > 2000) // alle 2000ms Servo auf andere Seite fahren
      {
         if (servo_pos == pos1) servo_pos=pos2; else servo_pos=pos1;
         setStopwatch2(0);
      }
      //task_RP6System();
      task_ADC();
      task_ACS();
      task_Bumpers();
      task_motionControl();
   }
return 0;
}
Aus https://www.roboternetz.de/phpBB2/ze...=479370#479370

Gruß

mic