- LiFePO4 Speicher Test         
Seite 1 von 2 12 LetzteLetzte
Ergebnis 1 bis 10 von 20

Thema: Servo bewegt sich nicht

  1. 11.09.2009, 16:23 #1
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86

    Servo bewegt sich nicht

    Anzeige

     Powerstation Test
    Hallo,
    Mithilfe von Dirks Servo Base Bibliothek und dem TV_Remote Bspl habe ich folgendes Programm "geschrieben" :

    Code:
    // written 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 = 0;
uint8_t servorcan = 1;      //rechter Servo
uint8_t servolcan = 1;		//linker Ser

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:
			writeString_P("Servo nach links\n");
			
				pos -= 10;
				
		break;
		
		case RC5_KEY_SERVO_R:
			writeString_P("Servo nach rechts\n");
			
				pos += 10;
				
		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 * Move 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");
	
	
	startStopwatch2();
	
	
	initSERVO(SERVO1);
	
	while(true) 
	{   
	
		servo1_position = pos;
		
		
		deccelerate();
		task_RP6System();
		
		
	}
	return 0;
}
    Code:
    Wenn ich nun die entsprechende Taste drücke bewegt sich der servo aber nicht...  :-s 
Kann mir jemand helfen?

danke schonmal

gruß
David
    ZitierenZitieren
  2. 15.09.2009, 16:59 #2
    Dirk
    Dirk ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.803
    Hallo David,

    in die while(true) Schleife müßtest du noch die task_SERVO aufnehmen und sagen, wie sich das Servo bewegen soll:

    Code:
    	startStopwatch2();					// Used for the demo
	
	while(true)  
	{
// ---------------------------------------------------------------------
// The demo code for positioning servo 1:
		if (getStopwatch2() > 48) {	// Change position every ~50ms
			servo1_position = pos;		// Position of servo 1
			pos++;						// Next position to the right
			if (pos > RIGHT_TOUCH) {pos = 0;} // pos: 0..RIGHT_TOUCH
			setStopwatch2(0);
		}
// ---------------------------------------------------------------------

		task_SERVO();			      
		deccelerate();			      
		task_RP6System();			      
	}
    Gruß Dirk
    ZitierenZitieren
  3. 16.09.2009, 20:37 #3
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86
    hallo dirk,
    danke für deine antwort, ich werds gleich mal ausprobieren =)
    david
    ZitierenZitieren
  4. 08.10.2009, 17:11 #4
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86
    Hallo Dirk,

    Ich habs jetzt so verändert wie du´s gesagt hast...

    Code:
    // 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:
			writeString_P("Servo nach links\n");
			if (pos > RIGHT_TOUCH)
				pos ++;
				writeInteger(pos, DEC);
		break;
		
		case RC5_KEY_SERVO_R:
			writeString_P("Servo nach rechts\n");
			if (pos < LEFT_TOUCH)
				pos --;
				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 * Move 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");
	
	
	startStopwatch2();
	
	
	initSERVO(SERVO1);
	
	while(true) 
	
   {
      if (getStopwatch2() > 48) 
         
		 {servo1_position = pos;}
         
		 
		 setStopwatch2(0);
		
		deccelerate();
		task_SERVO();
		task_RP6System();
		
		
	}
	return 0;
}
    Der Servo will aber noch immer nicht...
    [-(

    Was ist noch falsch??
    Bei deinem Beispiel-Programm bewagt er sich...

    gruß David
    ZitierenZitieren
  5. 08.10.2009, 23:26 #5
    Dirk
    Dirk ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.803
    Hallo David,

    erstmal die Klammern anders:
    Code:
       startStopwatch2(); 
    
    
   initSERVO(SERVO1); 
    
   while(true) 
    
   { 
      if (getStopwatch2() > 48) 
          
       {servo1_position = pos; 
          
       
       setStopwatch2(0); 
       } 
 
      deccelerate(); 
      task_SERVO(); 
      task_RP6System(); 
  }       
       
   return 0; 
}
    ... und dann wird sich das Servo ja nur bewegen, wenn sich pos irgendwie ändert. Das must du machen.

    Gruß Dirk
    ZitierenZitieren
  6. 10.10.2009, 18:43 #6
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86
    Ja, genau...
    Das mach ich doch mit:

    Code:
    		
		case RC5_KEY_SERVO_L:
			writeString_P("Servo nach links\n");
			if (pos > RIGHT_TOUCH)
				pos ++;
				writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
		break;
		
		case RC5_KEY_SERVO_R:
			writeString_P("Servo nach rechts\n");
			if (pos < LEFT_TOUCH)
				pos --;
				writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
		break;
	}
    oder?
    Also leider gehts bei mir noch immer nicht...
    ZitierenZitieren
  7. 13.10.2009, 20:45 #7
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86
    Wie ich es drehe und wende... Es will nicht funktionieren =(
    ZitierenZitieren
  8. 13.10.2009, 21:03 #8
    Dirk
    Dirk ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.803
    Poste doch deinen aktuellen Code noch 'mal.

    Ich probiere ihn dann mal bei mir.

    Gruß Dirk
    ZitierenZitieren
  9. 14.10.2009, 16:04 #9
    smusmut
    smusmut ist offline
    Benutzer Stammmitglied
    Registriert seit
    26.02.2009
    Beiträge
    86
    Hallo Dirk,

    vielen Dank für deine Hilfe.
    Hier mein Aktueller Code:

    Code:
    // 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:
			writeString_P("Servo nach links\n");
			if (pos > RIGHT_TOUCH)
				pos ++;
				writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
		break;
		
		case RC5_KEY_SERVO_R:
			writeString_P("Servo nach rechts\n");
			if (pos < LEFT_TOUCH)
				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");
	
   startStopwatch2();
   
   
   initSERVO(SERVO1);
   
   while(true)
   
   {
      if (getStopwatch2() > 48)
         
       {servo1_position = pos;
         
       
       setStopwatch2(0);
       }
 
      deccelerate();
      task_SERVO();
      task_RP6System();
  }       
       
   return 0;
}
    Ich hoffe du kanst den Fehler finden!

    gruß David
    ZitierenZitieren
  10. 15.10.2009, 18:23 #10
    Dirk
    Dirk ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.803
    Hallo David,

    so, nun hab ich's:

    Ändere die RC5 Abfragen 'mal so:
    Code:
          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;
    ... und die while(1) Schleife so:
    Code:
    	stopSERVO();
   while(true) 
    
   { 
              
        
  
      deccelerate(); 
      task_SERVO(); 
      if (getStopwatch6() > 300) {stopSERVO();}
      task_RP6System(); 
  }
    Gruß Dirk
    ZitierenZitieren
Seite 1 von 2 12 LetzteLetzte
« Vorheriges Thema | Nächstes Thema »

Lesezeichen

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •  

Foren-Regeln


LiFePO4 Speicher Test