Servo bewegt sich nicht

[smusmut]

Hallo Dirk,

Entschuldige das ich nicht gleich zurück geschrieben habe: mein Laptop war in Reperatur. Großen dank für deine Mühe!
Dass heißt es soll jetzt so sein:

// written by David Smutny
// RP6BaseServoLib was written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6BaseServoLib.h"
#include "RP6RobotBaseLib.h" 



#define RC_YOUR_OWN

#ifdef RC_YOUR_OWN
	#define RC5_KEY_LEFT 				20	
	#define RC5_KEY_RIGHT 				22		
	#define RC5_KEY_FORWARDS 			18  	
	#define RC5_KEY_BACKWARDS 			24	
	#define RC5_KEY_STOP 				32		
	#define RC5_KEY_CURVE_LEFT 			17	
	#define RC5_KEY_CURVE_RIGHT 		19	
	#define RC5_KEY_CURVE_BACK_LEFT 	23	
	#define RC5_KEY_CURVE_BACK_RIGHT 	25	
	#define RC5_KEY_LEFT_MOTOR_FWD 		26
	#define RC5_KEY_LEFT_MOTOR_BWD 		41
	#define RC5_KEY_RIGHT_MOTOR_FWD 	42
	#define RC5_KEY_RIGHT_MOTOR_BWD 	49
	#define RC5_KEY_SERVO_L             37
	#define RC5_KEY_SERVO_R				36
#endif


#define MAX_SPEED_MOVE 200
#define MAX_SPEED_TURN 100 

#define MAX_SPEED_CURVE 120 
#define MAX_SPEED_CURVE2 40 
#define ACCELERATE_CURVE 10
#define ACCELERATE_CURVE2 4
#define DECELERATE_CURVE 4
#define DECELERATE_CURVE2 2

#define MAX_SPEED_1_MOTOR 120 

#define ACCELERATE_VALUE 8
#define DECELERATE_VALUE 4

uint8_t max_speed_left;
uint8_t max_speed_right;
uint8_t acl_left;
uint8_t acl_right;
uint8_t decl_left;
uint8_t decl_right;
uint16_t pos = 100;

void setDefaultSpeedParameters(void)
{
	max_speed_left = MAX_SPEED_MOVE;
	max_speed_right = max_speed_left;
	acl_left = ACCELERATE_VALUE;
	acl_right = ACCELERATE_VALUE;
	decl_left = DECELERATE_VALUE;
	decl_right = DECELERATE_VALUE;
	uint16_t tmp = (getDesSpeedLeft() + getDesSpeedRight())/2;
	moveAtSpeed(tmp , tmp);
}


void receiveRC5Data(RC5data_t rc5data)
{
	writeString_P("Toggle Bit:");
	writeChar(rc5data.toggle_bit + '0');
	writeString_P(" | Device Address:");
	writeInteger(rc5data.device, DEC);
	writeString_P(" | Key Code:");
	writeInteger(rc5data.key_code, DEC);
	writeChar('\n');
	
#ifndef DO_NOT_MOVE  
	
	uint8_t movement_command = false; 

	switch(rc5data.key_code)
	{
		case RC5_KEY_LEFT: 	
			writeString_P("LEFT\n");
			setDefaultSpeedParameters();
			max_speed_left = MAX_SPEED_TURN;
			max_speed_right = max_speed_left;
			changeDirection(LEFT);
			setLEDs(0b100000);
			movement_command = true;
		break;
		case RC5_KEY_RIGHT:
			writeString_P("RIGHT\n");
			setDefaultSpeedParameters();
			max_speed_left = MAX_SPEED_TURN;
			max_speed_right = max_speed_left;
			changeDirection(RIGHT);
			setLEDs(0b000100);
			movement_command = true;
		break;
		case RC5_KEY_FORWARDS:
			writeString_P("FORWARDS\n");
			setDefaultSpeedParameters();
			changeDirection(FWD);
			setLEDs(0b100100);
			movement_command = true;
		break;
		case RC5_KEY_BACKWARDS:
			writeString_P("BACKWARDS\n");
			setDefaultSpeedParameters();
			changeDirection(BWD);
			setLEDs(0b001001);
			movement_command = true;
		break;
		case RC5_KEY_STOP:
			writeString_P("STOP\n");
			max_speed_left = 0;
			max_speed_right = max_speed_left;
			moveAtSpeed(0,0);
			setLEDs(0b011011);
			movement_command = true;
		break;
		case RC5_KEY_CURVE_LEFT:
			writeString_P("CURVE LEFT FWD\n");
			max_speed_left = MAX_SPEED_CURVE2;
			max_speed_right = MAX_SPEED_CURVE;
			acl_left = ACCELERATE_CURVE2;
			acl_right = ACCELERATE_CURVE;
			decl_left = DECELERATE_CURVE2;
			decl_right = DECELERATE_CURVE;
			changeDirection(FWD);
			setLEDs(0b110100);
			movement_command = true;
		break;
		case RC5_KEY_CURVE_RIGHT:
			writeString_P("CURVE RIGHT FWD\n");
			max_speed_left = MAX_SPEED_CURVE;
			max_speed_right = MAX_SPEED_CURVE2;
			acl_left = ACCELERATE_CURVE;
			acl_right = ACCELERATE_CURVE2;
			decl_left = DECELERATE_CURVE;
			decl_right = DECELERATE_CURVE2;
			changeDirection(FWD);
			setLEDs(0b100110);
			movement_command = true;
		break;
		case RC5_KEY_CURVE_BACK_LEFT:
			writeString_P("CURVE LEFT BWD\n");
			max_speed_left = MAX_SPEED_CURVE2;
			max_speed_right = MAX_SPEED_CURVE;
			acl_left = ACCELERATE_CURVE2;
			acl_right = ACCELERATE_CURVE;
			decl_left = DECELERATE_CURVE2;
			decl_right = DECELERATE_CURVE;
			changeDirection(BWD);
			setLEDs(0b011001);
			movement_command = true;
		break;
		case RC5_KEY_CURVE_BACK_RIGHT:
			writeString_P("CURVE RIGHT BWD\n");
			max_speed_left = MAX_SPEED_CURVE;
			max_speed_right = MAX_SPEED_CURVE2;
			acl_left = ACCELERATE_CURVE;
			acl_right = ACCELERATE_CURVE2;
			decl_left = DECELERATE_CURVE;
			decl_right = DECELERATE_CURVE2;
			changeDirection(BWD);
			setLEDs(0b001011);
			movement_command = true;
		break;
		case RC5_KEY_LEFT_MOTOR_FWD:
			writeString_P("MOTOR LEFT FWD\n");
			max_speed_left = 0;
			max_speed_right = MAX_SPEED_1_MOTOR;
			acl_left = 4;
			acl_right = 4;
			decl_left = 4;
			decl_right = 4;
			changeDirection(FWD);
			setLEDs(0b110000);
			movement_command = true;
		break;
		case RC5_KEY_LEFT_MOTOR_BWD:
			writeString_P("MOTOR LEFT BWD\n");
			max_speed_left = 0;
			max_speed_right = MAX_SPEED_1_MOTOR;
			acl_left = 4;
			acl_right = 4;
			decl_left = 4;
			decl_right = 4;
			changeDirection(BWD);
			setLEDs(0b101000);
			movement_command = true;
		break;
		case RC5_KEY_RIGHT_MOTOR_FWD:
			writeString_P("MOTOR RIGHT FWD\n");
			max_speed_left = MAX_SPEED_1_MOTOR;
			max_speed_right = 0;
			acl_left = 4;
			acl_right = 4;
			decl_left = 4;
			decl_right = 4;
			changeDirection(FWD);
			setLEDs(0b000110);
			movement_command = true;
		break;
		case RC5_KEY_RIGHT_MOTOR_BWD:
			writeString_P("MOTOR RIGHT BWD\n");
			max_speed_left = MAX_SPEED_1_MOTOR;
			max_speed_right = 0;
			acl_left = 4;
			acl_right = 4;
			decl_left = 4;
			decl_right = 4;
			changeDirection(BWD);
			setLEDs(0b000101);
			movement_command = true;
		break;
        case RC5_KEY_SERVO_L:
            startSERVO();
            setStopwatch6(0);
			startStopwatch6();
			writeString_P("Servo nach links\n");
			if (pos >= 10) {
            pos -= 10;
			}
			else {
			pos = 0;
			}
			servo1_position = pos;
			writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
      break;
      case RC5_KEY_SERVO_R:
			startSERVO();
			setStopwatch6(0);
			startStopwatch6();
			writeString_P("Servo nach rechts\n");
			if (pos <= (RIGHT_TOUCH - 10)) {
            pos += 10;
			}
			else {
			pos = RIGHT_TOUCH;
			}
			servo1_position = pos;
			writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
      break; 
	}
	
	if(movement_command) 
	{
		if(getDesSpeedLeft() < max_speed_left)
		{							
			moveAtSpeed(getDesSpeedLeft()+acl_left, getDesSpeedRight());
			if(getDesSpeedLeft() < 10)
				moveAtSpeed(10, getDesSpeedRight());
		}
		if(getDesSpeedRight() < max_speed_right)
		{
			moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()+acl_right);
			if(getDesSpeedRight() < 10)
				moveAtSpeed(getDesSpeedLeft(), 10);
		}
		setStopwatch4(0);
		startStopwatch4();
	}
#endif

}


void deccelerate(void)
{
	if(getStopwatch4() > 250)
	{
		if(getDesSpeedLeft() <= 10) 
			moveAtSpeed(0, getDesSpeedRight());
		else			
			moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
		if(getDesSpeedRight() <= 10)
			moveAtSpeed(getDesSpeedLeft(), 0); 
		else		
			moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
			
		if (getDesSpeedRight() == 0 && getDesSpeedLeft() == 0)	
			stopStopwatch1(); 
		max_speed_left = getDesSpeedLeft();
		max_speed_right = getDesSpeedRight();
		setLEDs(0b000000);
		setStopwatch4(0);
	}
	
	if(getDesSpeedLeft() > max_speed_left) 
	{
		if(getDesSpeedLeft() <= 10)
			moveAtSpeed(0, getDesSpeedRight());
		else
			moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight()); 
	}
	if(getDesSpeedRight() > max_speed_right) 
	{
		if(getDesSpeedRight() <= 10)
				moveAtSpeed(getDesSpeedLeft(), 0);
		else
			moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right); 
	}
}




int main(void)
{
	initRobotBase(); 
	
	setLEDs(0b111111);
	writeChar('\n');
    writeString_P("RP6 controlled by RC5 TV Remote\n");
	writeString_P("___________________________\n");
	mSleep(500);	 
	setLEDs(0b000000); 
	powerON();
	
	IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

	writeString_P("\n * Turn Left: "); 					writeInteger(RC5_KEY_LEFT, DEC);
	writeString_P("\n * Turn Right: "); 				writeInteger(RC5_KEY_RIGHT, DEC);
	writeString_P("\n * Move Forwards: "); 				writeInteger(RC5_KEY_FORWARDS, DEC);
	writeString_P("\n * Move Backwards: "); 			writeInteger(RC5_KEY_BACKWARDS, DEC);
	writeString_P("\n * Stop: "); 						writeInteger(RC5_KEY_STOP, DEC);
	writeString_P("\n * Move curve left forwards: "); 	writeInteger(RC5_KEY_CURVE_LEFT, DEC);
	writeString_P("\n * Move curve right forwards: "); 	writeInteger(RC5_KEY_CURVE_RIGHT, DEC);
	writeString_P("\n *curve left backwards: "); 	writeInteger(RC5_KEY_CURVE_BACK_LEFT, DEC);
	writeString_P("\n * Move curve right backwards: "); writeInteger(RC5_KEY_CURVE_BACK_RIGHT, DEC);
	writeString_P("\n * Motor left forwards: "); 		writeInteger(RC5_KEY_LEFT_MOTOR_FWD, DEC);
	writeString_P("\n * Motor left backwards: "); 		writeInteger(RC5_KEY_LEFT_MOTOR_BWD, DEC);
	writeString_P("\n * Motor right forwards: "); 		writeInteger(RC5_KEY_RIGHT_MOTOR_FWD, DEC);
	writeString_P("\n * Motor right backwards: "); 		writeInteger(RC5_KEY_RIGHT_MOTOR_BWD, DEC);
	writeString_P("\n * Servo nach rechts: ");			writeInteger(RC5_KEY_SERVO_R, DEC);
	writeString_P("\n * Servo nach links: ");			writeInteger(RC5_KEY_SERVO_L, DEC);
	writeString_P("\n-----------------------\n");
	
    stopSERVO();
   while(true)
   
   {
      deccelerate();
      servo1_position = pos;
	  task_SERVO();
      if (getStopwatch6() > 300) {stopSERVO();}
      task_RP6System();
    }        
    }

Aber bei mir geht es immer noch nicht


Ich weis eh das ich mich unglaublich dumm anstelle aber...

Bitte Hilf mir noch ein letztes mal! [-o< [-o< [-o< [-o< [-o< [-o< [-o< [-o< [-o< [-o<

[Dirk]

Hallo David,

oben sieht jetzt alles gut aus.

VOR der While(true) Schleife (genauer: Vor stopSERVO()!) must du ja noch den Init Befehl stellen:
initSERVO(SERVO1);

IN der While(true) Schleife muss servo1_position = pos; raus.

Zwischen die letzten beiden "}" muss wieder return 0; rein.

Gruß Dirk

[smusmut]

Hallo Dirk,
Vielen, vielen Dank für deine Hilfe!
Ich weiß nicht wie ich das ohne dich geschafft hätte...
Danke Danke!

gruß David

[smusmut]

Hallo Dirk,
Und wie kann ich das dann machen wenn ich will das der servo immer auf der gleichen Position bleibt, auch wenn ich gerade keine Taste drücke und Last auf dem Servo ist?
Also das ich zum Bspl pos auf 200 hab und dann dagegen drücke das der Servo immer wieder gleich zurück geht?

vielen Dank schonmal,
gruß David

[Dirk]

...wie kann ich das dann machen wenn ich will das der servo immer auf der gleichen Position bleibt, auch wenn ich gerade keine Taste drücke und Last auf dem Servo ist?

1. Eigentlich sollte die mechanische Last des Servos auch nicht so hoch sein, dass es durch die Kräfte verstellt wird.
2. Das ginge aber, wenn man dauerhaft einen Servo-Impuls erzeugt und den nicht mit stopSERVO() 300ms nach einem IR-Empfang wieder stoppt. Wenn man aber einen Dauerimpuls für das Servo erzeugen will, findet im Hintergrund kein IR-Emfang mehr statt, da sich das gegenseitig behindert.
Was du machen kannst wäre, in der Hauptschleife den Servoimpuls z.B. jede Sekunde einmal einzuschalten (startSERVO) und nach 300ms wieder auszuschalten (stopSERVO). Dann bliebe noch genügend Zeit für den internen IR-Empfang.
Beispiel (einfügen in die While(true) Schleife):
if (getStopwatch6() > 1000) {startSERVO(); setStopwatch6(0);}

Probier da mal ein bißchen rum, ich habs nicht ausprobiert!

Gruß Dirk

[smusmut]

Hallo Dirk,

Ich habs ausprobiert. Aber die Abstände in denen der Servo eingestellt wird sind zu groß...
Ist es überhaupt möglich den Servo so oft anzusteuern UND IR-Empfang auszuwerten?

Ich danke dir für deine Hilfe!

gruß

David

[Dirk]

Ist es überhaupt möglich den Servo so oft anzusteuern UND IR-Empfang auszuwerten?

Wenn man einen dauernden Servo-Impuls erzeugen will, klappt das mit meiner Lib nicht. Man muss den Impuls mit stopSERVO() für eine gewisse Zeit ausschalten, damit IR-Empfang stattfindet. In deinem Beispiel kannst du die 1000 probeweise noch auf 900..500 verringern. Das bedeutet, dass für den IR-Empfang dann noch 600..200ms zur Verfügung stehen.
Erklärung: In meiner Lib wird der Servo-Impuls voll blockierend erzeugt. Damit werden alle anderen Aktivitäten alle 20ms für 1..2ms blockiert.

Was kann man machen:
1. Such mal im Forum nach anderen Lösungen für die Servoansteuerung mit der RP6 Base. Es gibt da Lösungen, bei denen die RP6-Libs geändert werden (man klinkt sich in eine Interrupt-Routine ein). Damit kann man eine nicht-blockierende Lösung erreichen. Ob damit IR-Empfang möglich ist, must du probieren.
2. Mit einer der Zusatzplatinen Control M32 oder CCPRO M128 kann man Servos mit PWM oder Timern komfortabel ansteuern.
3. Es gibt kleine Platinen zur Servoansteuerung, die mit I2C oder RS232 anzusteuern sind.

Gruß Dirk

[radbruch]

Hallo

Interruptlösungen sind schwierig weil die RP6-Lib schon alle Interrupts belegt. Ich hatte mir mal einen Ansatz (für 8 Servos an den LEDs und SCL/SDA) mit der ADC-ISR zusammengebastelt. Mit Servo_On() wird der ADC in den Dauerbetrieb (freeruning) mit ISR geschaltet (der ADC_Task könnte dabei übrigends weiterverwendet werden), mit Servo_Off() schaltet man die Servoansteuerung wieder aus und den ADC auf Lib-Einstellungen. Leider ist die Auflösung der Servoschritte nicht sehr groß, aber dafür funktioniert es mit der Lib zusammen:

// Domino Day für den RP6 mit neuem Greifarm und ADC-ISR         19.1.2008  mic

#include "RP6RobotBaseLib.h"

uint16_t s1, s2, s3, s4 ,s5, s6;

void servo_ON(void)
{
	cli();
// Freeruning, 5V-Ref, ISR enable, prescale /32 ((8MHZ/32)/12,5 sind ca. 20kHz bzw. 0,05us)
// AVCC with external capacitor at AREF pin, Ergebniss linksbündig, Kanal ADC7 (UBAT)
	ADMUX = (0<<REFS1) | (1<<REFS0)  | (1<<ADLAR) | 7;
// setzte free running triggern
	SFIOR = (0<<ADTS2) | (0<<ADTS1) | (0<<ADTS0);
// Interrupt ein, Wandler einschalten, prescaller /32
	ADCSRA = (1<<ADIE) | (1<<ADEN) | (1<<ADPS2) | (0<<ADPS1)  | (1<<ADPS0);
// Autotriggern bedeutet jetzt free running aktivieren, altes Flag löschen
	ADCSRA |= (1<<ADATE) | (1<<ADIF);
// Initialisierung starten
	ADCSRA |= (1<<ADSC);
// und noch die wohl eher unnötige Initiallesung
	while (!(ADCSRA & (1<<ADIF)));
	ADCSRA |= (1<<ADIF);

	DDRC |= 0x70;		// LED1-3 auf Ausgang und low
	PORTC &= ~0x70;
	DDRB |= 0x83;		// LED4-6 auf Ausgang und low
	PORTB &= ~0x83;
	DDRC |= 0x3;		// SCL und SDA auf Ausgang und low
	PORTC &= ~0x3;
	sei();
}
void servo_OFF(void)
{
	cli();
	// Initialize ADC: (Defaultsetup der RP6-Library)
	ADMUX = 0; //external reference
	ADCSRA = (0<<ADIE) | (0<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADIF);
	SFIOR = 0;
	sei();
	setLEDs(0); // LEDs restaurieren
}
int main(void)
{
	initRobotBase();

	s1=25;
	s2=25;
	s3=25;
	s4=25;
	s5=25;
	s6=25;
	servo_ON();
	setLEDs(63);
	mSleep(2000);
	setLEDs(0);
	while(1)
	{
		mSleep(1000);
		s1=40;
		mSleep(1000);
		s1=25;
		mSleep(1000);
		s2=35;
		mSleep(1000);
		s2=15;
	}
	return 0;
}

ISR(ADC_vect)
{
	static uint16_t count=0;
	(count>s1)?(PORTC&=~SL1):(PORTC|=SL1);
	(count>s2)?(PORTC&=~SL2):(PORTC|=SL2);
	(count>s3)?(PORTC&=~SL3):(PORTC|=SL3);
	(count>s4)?(PORTB&=~SL4):(PORTB|=SL4);
	(count>s5)?(PORTB&=~SL5):(PORTB|=SL5);
	(count>s6)?(PORTB&=~SL6):(PORTB|=SL6);

	(count>s1)?(PORTC&=~1):(PORTC|=1); // SCL
	(count>s2)?(PORTC&=~2):(PORTC|=2); // SDA

	(count<500)?count++:(count=0);
}

(Für eine bessere Auflösung könnte man den ADC-Prescaller noch verkleinern)

Gruß

mic

[smusmut]

Hallo Radbruch,

Heist das ich kann zum bspl auch die Motoren vom RP6 nicht gleichzeitig mit Dirks Lösung verwenden?

Ich werd mir das von dir mal anschauen, ich poste dann wenns funktioniert!

gruß David

[Dirk]

... ich kann zum bspl auch die Motoren vom RP6 nicht gleichzeitig mit Dirks Lösung verwenden?

Doch, das geht. Das hast du ja in deinem Remote-Programm auch selbst schon feststellen können, oder fährt der RP6 nicht?

Gruß Dirk