RP6 Servo Ansteuerung

[RP6-Besitzer]

Hallo,
habe jetzt einen Servo an meinen RP6 gebaut und mich ans programmieren gemacht
Hab mich eingelesen und alles grundsätzlich verstanden, hätte aber ein Problem:
Mein Programm geht immer vom aktuellen Winkel aus, d.h. wenn ich das Programm starte dreht er sich 30 grad nach links und 30 grad nach rechts. Verdreh ich den Servo manuell und starte das Programm dreht er sich wieder 30 grad nach links und 30 grad nach rechts.

Mein Programm:

#include "RP6RobotBaseLib.h"

int main(void)
{
initRobotBase();
DDRA |= 1;
while(1)
{
PORTA |= 1;
sleep(5);
PORTA &= ~1;
mSleep(500);
PORTA |= 1;
sleep(30);
PORTA &= ~1;
mSleep(5000);
}


return(0);
}

Eigentlich will ich, dass er sich auf einen bestimmten Winkel einstellt, eine halbe Sekunde wartet und sich dann auf einen anderen Winkel dreht, 5 Sekunden wartet und von vorne...
...nur leider führt er das Programm immer von der aktuellen Postition aus
...und durch ändern der sleep-Zahlen dreht sich der Servo um keinen anderen Winkel, als vorher

was mache ich falsch?
Danke für eure Antworten

[radbruch]

Hallo

So vielleicht:

#include "RP6RobotBaseLib.h"

uint8_t i; //Stellzeit für das Servo

int main(void)
{
	initRobotBase();
	DDRA |= 1;
	while(1)
	{
		setLEDs(1);
		for(i=0; i<50; i++) // 50 mal Servosignal für Position 1 senden
		{
			PORTA |= 1;
			sleep(5); // Impulslänge Position 1 (0,5ms)
			PORTA &= ~1;
			sleep(200-5); // Impulspause
		}

		setLEDs(2);
		mSleep(500); // Pause 1

		setLEDs(4);
		for(i=0; i<50; i++) // und 50 mal Servosignal für Position 2 senden
		{
			PORTA |= 1;
			sleep(30); // Impulslänge Position 2 (3ms?)
			PORTA &= ~1;
			sleep(200-30); // Impulspause
		}

		setLEDs(8);
		mSleep(2000); // Pause 2
	}
	return(0);
}

(ungetestet)

Das Servosignal muss solange wiederholt werden, bis das Servo seine Position nicht mehr ändert. Da wir keine Positionsrückmeldung erhalten, kann das nur über eine Zeitsteuerung realisiert werden. Günstig ist hierbei, dass der Impuls alle 20ms erzeugt werden sollte, 50 Impulse dauern dann ca. eine Sekunde.

Mein Programm geht immer vom aktuellen Winkel aus...

Das Servo fährt immer absolut die zur Impulsdauer gehörende Position an. Wenn du relativ von der jeweiligen Startposition schwenken willst, kannst du das ohne zusätzliche Positionsrückmeldung vergessen.

Die Belegung von ADC0 passt nicht zu den Servosteckern. Das hast du aber sicher beachtet

Gruß

mic

P.S. Bitte Code-Tags verwenden.

[RP6-Besitzer]

Danke Radbruch
Hast mir sehr weitergeholfen, jetzt funktionierts
Ich weiß, das die Belegung von ADC0 nicht zu den Servosteckern passt, hab bei einem Servo die Konstakte umgesteckt. Welche Belegung wäre besser?
Die von XBUS1 oder XBUS2?
Schöne Videos auf YouTube übrigens

[radbruch]

Hallo

Schön, dass es gleich auf Anhieb funktioniert.

Mit "Welche Belegung wäre besser? " meinst vermutlich den USRBUS. Der XBUS ist schon belegt. Ich habe den USRBUS1 verwendet, weil der näher am Mega32 sitzt und die Signalleitungen dadurch kürzer und störungunempfindlicher sind.

Nicht alle meine veröffentlichten Videos sind "schön" ;)

Gruß

mic

[RP6-Besitzer]

Entschuldigung, aber wie erstelle ich solche Code Tags?

[TobiKa]

Klick auf "Antwort erstellen", dann siehst du die dazugehörigen Button.
Man kann übrigens auch ausversehen doppelt abgeschickte Posts selbst löschen!

[RP6-Besitzer]

Danke
Will jetzt das Rudermaschinenansteuerungsprogramm in mein normales "Gerade Fahren und Hindernissen ausweichen" Programm intigrieren.
Hatte auch grundsätzlich gut geklappt, nur mein ACS funktioniert dann nicht mehr.
Hier meine Programme:

#include "RP6RobotBaseLib.h"

void acsStateChanged(void)
  {
    if(obstacle_left)
      rotate(80, RIGHT, 50, true);
    if(obstacle_right)
      rotate(80, LEFT, 50, true);
    if(obstacle_right && obstacle_left)
      rotate(60, RIGHT, 100, true);
    }  
void bumpersStateChanged(void)
{
    if(bumper_left || bumper_right) 
    {
        changeDirection(BWD);
        move(100, BWD, DIST_MM(200), true);
        rotate(50, RIGHT, 120, true);
       
        
    }
}

int main(void)
{
  initRobotBase();
  setLEDs(0b111111);
  mSleep(500);
  setLEDs(0b001001);
  BUMPERS_setStateChangedHandler(bumpersStateChanged);
  ACS_setStateChangedHandler(acsStateChanged);
  powerON();
  setACSPwrLow();
   
  
  
  
  while(true)
   {
     task_RP6System();
     changeDirection(FWD);
     moveAtSpeed(80,80); 
     }
     return 0;
}

mein geradeausfahr und hindernissen ausweichen programm

#include "RP6RobotBaseLib.h"
uint8_t i;
int main(void)
{
 initRobotBase();
 DDRA |= 1;
 setLEDs(0b010101);
 mSleep(1000);
 setLEDs(0b101010);
 
 while(1)
    {
	 for (i=0; i<50; i++)
      {
	  PORTA |= 1;
      sleep(8);
	  PORTA &= ~1;
	  sleep(200-10);
	  }
	  mSleep(1000);
	  for (i=0; i<50; i++)
	  {
	  PORTA |= 1;
	  sleep(23);
	  PORTA &= ~1;
	  sleep(200-23);
	  }
	 
	 }
	 
     return(0);
}

mein Rudermaschinenansteuerungsprogramm

#include "RP6RobotBaseLib.h"

uint8_t i;

void acsStateChanged(void)
  {
    if(obstacle_left)
	  rotate(80, RIGHT, 50, true);
	if(obstacle_right)
	  rotate(80, LEFT, 50, true);
	if(obstacle_right && obstacle_left)
	  rotate(60, RIGHT, 100, true);
    }  
void bumpersStateChanged(void)
{
	if(bumper_left || bumper_right) 
	{
		changeDirection(BWD);
		move(100, BWD, DIST_MM(200), true);
		rotate(50, RIGHT, 120, true);
	   
		
	}
}

int main(void)
{
  initRobotBase();
  setLEDs(0b111111);
  mSleep(500);
  setLEDs(0b001001);
  DDRA |= 1;
  BUMPERS_setStateChangedHandler(bumpersStateChanged);
  ACS_setStateChangedHandler(acsStateChanged);
  powerON();
  setACSPwrLow();
   
  
  
  
  while(1)
   {
     task_RP6System();
	 changeDirection(FWD);
	 moveAtSpeed(100,100);
	 setLEDs(1);
     for(i=0; i<50; i++) // 50 mal Servosignal für Position 1 senden
      {
         PORTA |= 1;
         sleep(8); // Impulslänge Position 1 (0,5ms)
         PORTA &= ~1;
         sleep(200-5); // Impulspause
      }

      setLEDs(2);
      mSleep(1000); // Pause 1

      setLEDs(4);
      for(i=0; i<50; i++) // und 50 mal Servosignal für Position 2 senden
      {
         PORTA |= 1;
         sleep(23); // Impulslänge Position 2 (3ms?)
         PORTA &= ~1;
         sleep(200-30); // Impulspause
      }

      setLEDs(8);
      mSleep(1000); // Pause 2
   }
   return(0);
}

mein Versuch


Was muss ich ändern, damit das ACS auch funktioniert?
Danke


[/code]-Tags eingefügt von radbruch

[TobiKa]

Du musst die Code-Tags auch wieder schliessen..

[RP6-Besitzer]

Danke, jetzt weiß ich wie's geht
Jemand ne Lösung?

[radbruch]

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:

#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:

 #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