kombination von fahren(move) und servobew. (getStopwatch)

[inka]

hi allerseits,

mit folgendem code versuche ich das anfahren von servopositionen mit fahren/drehen des RP6 zu kombinieren, mit mäßigem erfolg

 // Uncommented Version of greifarm an m32.c
// written by georg
// ------------------------------------------------------------------------------------------

#include "RP6ControlServoLib.h" //servolib von Dirk
#include "RP6I2CmasterTWI.h"	// I2C Master Library
#include "RP6Control_I2CMasterLib.h" //RP6 Control I2C Master library

int pos[] = { 5, 15, 25, 35, 45, 55, 65, 75, 85, 95, 105, 115, 125};

/*****************************************************************************/

/**
 * Timed Watchdog display only - the other heartbeat function
 * does not work in this example as we use blocked moving functions here.
 */
void watchDogRequest(void)
{
	static uint8_t heartbeat2 = false;
	if(heartbeat2)
	{
		clearPosLCD(0, 14, 1);
		heartbeat2 = false;
	}
	else
	{
		setCursorPosLCD(0, 14);
		writeStringLCD_P("#"); 
		heartbeat2 = true;
	}
}

/*****************************************************************************/
// I2C Requests: 

/**
 * The I2C_requestedDataReady Event Handler
 */
void I2C_requestedDataReady(uint8_t dataRequestID)
{
	checkRP6Status(dataRequestID);
}

/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 */
void I2C_transmissionError(uint8_t errorState)
{
	writeString_P("\nI2C ERROR - TWI STATE: 0x");
	writeInteger(errorState, HEX);
	writeChar('\n');
}

/*****************************************************************************/


/********************funktionen für servo**********************/
void servo_right_top(void)
{
servo2_position = pos[1];
servo4_position = pos[12];
}

void servo_right_bottom(void)
{
servo2_position = pos[12];
servo4_position = pos[0];
}

void servo_center_top(void)
{
servo2_position = pos[5];
servo4_position = pos[12];
}

void servo_center_bottom(void)
{
servo2_position = pos[5];
servo4_position = pos[3];
}

void servo_left_top(void)
{
servo2_position = pos[12];
servo4_position = pos[12];
}

void servo_left_bottom(void)
{
servo2_position = pos[1];
servo4_position = pos[1];
}

void servo_open(void)
{
servo6_position = pos[8];
}

void servo_close(void)
{
servo6_position = pos[4];
}

int main(void)
{ 
   initRP6Control();

   initLCD();

   showScreenLCD("################", "################");
   mSleep(1500);
   showScreenLCD("<<RP6  Control>>", "<<LC - DISPLAY>>");
   mSleep(2500);
   showScreenLCD(" greifarm an m32 ", "  Version 1.02  ");
   mSleep(2500);
   clearLCD();

   
   initSERVO(SERVO2 | SERVO4 | SERVO6);

   servo2_position = pos[5];
   servo4_position = pos[5];
   servo6_position = pos[5];   
   mSleep(2000);
   
   
   startStopwatch1();
   startStopwatch2();

	// ---------------------------------------
	WDT_setRequestHandler(watchDogRequest); 
	
	// ---------------------------------------
	// Init TWI Interface:
	I2CTWI_initMaster(100);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	sound(180,80,25);
	sound(220,80,25);

	setLEDs(0b1111);

	showScreenLCD("################", "################");
	mSleep(500);
	showScreenLCD("I2C-Master", "Movement...");
	mSleep(1000);
	setLEDs(0b0000);
	
	// ---------------------------------------
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);

   
   while(true)
   {
	   if (getStopwatch2() > 1000)
	   {
       servo_right_top();
	   servo_open();
	   
	   }
	   if (getStopwatch2() > 2000)
	   {
	   servo_close();
//	   move(60, FWD, DIST_MM(100), true);
//     setStopwatch2(0);
//     startStopwatch2();
	   }
	   if (getStopwatch2() > 3000)
	   {
	   servo_left_bottom();	   
//	   rotate(50, LEFT, 180, true);	   
	   }
	   if (getStopwatch2() > 4000)
	   {
		   servo_left_top();
	    
	   }
	   if (getStopwatch2() > 5000)
	   {
		   servo_right_bottom();
//		   move(60, FWD, DIST_MM(100), true);
		   
	   }
	   if (getStopwatch2() > 6000)
	   {
		   servo_center_top();
		   
	   }
	   if (getStopwatch2() > 7000)		   
	   {
	       servo_open();
		   servo_center_bottom();
           setStopwatch2(0);
           setStopwatch1(0);
	   }

	   task_SERVO();
	   mSleep(3);
//	   move(60, FWD, DIST_MM(100), true);
   }
   return 0;
}

die auskomentierten befehlszeilen in der whileschleife zeigen meine versuche, ich habe beim move true und false versucht, auch verschiedene (längere) zeitabstände bei den If getStopwatch(2) abfragen, wie auch eine zweite If getStopwatch(1) schleife innerhalb der abfrage (2) einzubauen, ohne den erwünschten erfolg...

auch wird die "zero_position" nicht mehr angefahren...

Ich möchte nur, dass der roboter ein stück fährt, dann ein, zwei servo bewegungen, dann evtl. roboter drehen, wieder servos, ...

was muss ich anders machen? Die hauptschleife in mehrere schleifen aufteilen? Oder gibts da noch ein anderen weg?...

[Dirk]

@inka:

Die hauptschleife in mehrere schleifen aufteilen?

Nein, die Hauptschleife bleibt EINE Hauptschleife.

Ich hatte dir ja hier: https://www.roboternetz.de/phpBB2/viewtopic.php?t=45223 .. mal im Prinzip hingeschrieben (17.8.), wie es gehen kann.

Du hast zwar jetzt schon einzelne Funktionen, aber nur für Servo-POSITIONEN und nicht für Servo-Bewegungen.

Bevor du das Fahren da rein mixt, mach erst einmal je eine Funktion für jede geplante Bewegung fertig.
Also z.B.: Greifarm_runter() oder Schaufel_zu().
Diese Funktionen stimmst du so ab, dass sie perfekt funktionieren (also richtige Geschwindigkeit der Servoarme, Positionen werden erreicht ...).

Dann kannst du im Hauptprogramm Bedingungen formulieren, wann die Servo-Bewegung gemacht werden soll. Wenn das nach einer Roboter-Bewegung sein soll: if (isMovementComplete() ) Schaufel_zu();

Gruß Dirk

[inka]

hallo Dirk,

danke erstmal für deine antwort. Ich dachte ich hätte alles so gemacht, wie du es in dem anderen thread vorgeschlagen hast...

als ich es jetzt noch einmal gelesen habe, ist mir klargeworden, dass die if-abfrage der stopwatches bestandteil der bewegungsfunktion sein muss und nicht der hauptschleife.

und das werde ich jetzt versuchen...

[inka]

hi Dirk,

also so, wie ich es mir nun gedacht habe geht es offensichtlich auch nicht

die zwei funktionen sehen so aus:

void servo_right_top(void)
{
	 startStopwatch2();
	 setStopwatch2(0);
	 if (getStopwatch2() > 1000)
		   {
		 	servo2_position = pos[1];
		 	servo4_position = pos[12];
		 	stopStopwatch2();
		   }
}
---------------------------------
void servo_left_bottom(void)
{
	 startStopwatch2();
	 setStopwatch2(0);
	 if (getStopwatch2() > 2000)
		   {
		 	servo2_position = pos[1];
		 	servo4_position = pos[1];
		 	stopStopwatch2();
		   }
}

der aufruf der beiden funktionen in der hauptschleife:

   while(true)
   {
	   servo_right_top();
	   move(60, FWD, DIST_MM(300), BLOCKING);
	   servo_left_bottom();	   

	   task_SERVO();
	   mSleep(3);
   }
   return 0;

die movefunktion zwischen den beiden servofunktionen soll eigentlich nur als eine zusätzliche verzögerung dienen...

der roboter führt die move funktion aus, die Servos zucken nur auf der stelle. Ich vermute mal, weil die if-bedingung nie erfüllt wird. Ich habe versucht (in der funktion) den startzustand der stopwatches auf einen höheren wert als der, der abgefragt wird einzustellen, bringt aber auch nichts...

was habe ich denn noch nicht verstanden?

[SlyD]

> was habe ich denn noch nicht verstanden?

So einiges
Sorry habe keine Zeit da näher drauf einzugehen daher nur kurz:


startStopwatch2();
setStopwatch2(0);
if (getStopwatch2() > 2000)

In Deutsch:
Starte Stopuhr und Setze Stopuhr auf 0
paar kurze Befehle im MICROsekunden Bereich später
abfrage: ist Stopwatch2 > 2000*0.1 Millisekunde = 200 Millisekunden ???

NEIN, die ist dann noch nichtmal über 0 hinaus gekommen...

Danach wird die Funktion sofort verlassen und der blockierende bewegen Befehl ausgeführt.
Bei der zweiten Funktion ist es dasselbe.

Dann wird noch die Servoeinstellung aufgerufen und 3 Millisekunden gewartet - daher zucken die Servos eben kurz.


Klar warum das so nicht funktionieren kann?

So eine Stopwatch MUSS in einer Schleife abgefragt werden und darf natürlich nicht währenddessen ständig gestartet und gesetzt werden.
(das darfst Du nur dann machen wenn eine bestimmte Zeit erreicht wurde oder ein bestimmtes ereignis eingetreten ist)

Blockierende Funktionen wie "move...BLOCKING" darfst Du da natürlich auch nicht verwenden sondern musst mit isMovementcomplete abfragen ob die Bewegung abgeschlossen ist und DANN eine andere Aktion auslösen.

--> Für komplexe Bewegungsabläufe nimmt man State Machines (Zustandsautomaten). Das ist kurz in der RP6 Anleitung beschrieben und such auch mal hier im RP6 Forum danach hab ich schonmal was zu geschrieben.


MfG,
SlyD