Zwei programme zusammenfügen

**carlitoco** · 23.01.2008, 16:45

Also ich habe jetzt Fahrprogramm und möchte dieses mit einem programm zusammen fügen, welches die Servosteuerung übernimmt.

Wichtig ist, das beide programme "ununterbrochen" und "Paralel" ablaufen, da das servo programm für einen IR abstandsmesser gebaut ist.

Hier der fahr code

Code:

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

// Includes:



#include "RP6RobotBaseLib.h" 	



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

// Behaviour command type:



#define IDLE  0



// The behaviour command data type:

typedef struct {

	uint8_t  speed_left;  // left speed (is used for rotation and 

						  // move distance commands - if these commands are 

						  // active, speed_right is ignored!)

	uint8_t  speed_right; // right speed

	unsigned dir:2;       // direction (FWD, BWD, LEFT, RIGHT)

	unsigned move:1;      // move flag

	unsigned rotate:1;    // rotate flag

	uint16_t move_value;  // move value is used for distance and angle values

	uint8_t  state;       // state of the behaviour

} behaviour_command_t;



behaviour_command_t STOP = {0, 0, FWD, false, false, 0, IDLE};



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

// Cruise Behaviour:



#define CRUISE_SPEED_FWD    100 // Default speed when no obstacles are detected!



#define MOVE_FORWARDS 1

behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD, 

								false, false, 0, MOVE_FORWARDS};



/**

 * We don't have anything to do here - this behaviour has only

 * a constant value for moving forwards - s. above!

 * Of course you can change this and add some random or timed movements 

 * in here...

 */

void behaviour_cruise(void)

{

}



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

// Escape Behaviour:



#define ESCAPE_SPEED_BWD    100

#define ESCAPE_SPEED_ROTATE 65



#define ESCAPE_FRONT		1

#define ESCAPE_FRONT_WAIT 	2

#define ESCAPE_LEFT  		3

#define ESCAPE_LEFT_WAIT	4

#define ESCAPE_RIGHT	    5

#define ESCAPE_RIGHT_WAIT 	6

#define ESCAPE_WAIT_END		7

behaviour_command_t escape = {0, 0, FWD, false, false, 0, IDLE}; 



/**

 * This is the Escape behaviour for the Bumpers.

 */

void behaviour_escape(void)

{

	static uint8_t bump_count = 0;

	

	switch(escape.state)

	{

		case IDLE: 

		break;

		case ESCAPE_FRONT:

			escape.speed_left = ESCAPE_SPEED_BWD;

			escape.dir = BWD;

			escape.move = true;

			if(bump_count > 3)

				escape.move_value = 220;

			else

				escape.move_value = 160;

			escape.state = ESCAPE_FRONT_WAIT;

			bump_count+=2;

		break;

		case ESCAPE_FRONT_WAIT:			

			if(!escape.move) // Wait for movement to be completed

			{	

				escape.speed_left = ESCAPE_SPEED_ROTATE;

				if(bump_count > 3)

				{

					escape.move_value = 100;

					escape.dir = RIGHT;

					bump_count = 0;

				}

				else 

				{

					escape.dir = LEFT;

					escape.move_value = 70;

				}

				escape.rotate = true;

				escape.state = ESCAPE_WAIT_END;

			}

		break;

		case ESCAPE_LEFT:

			escape.speed_left = ESCAPE_SPEED_BWD;

			escape.dir 	= BWD;

			escape.move = true;

			if(bump_count > 3)

				escape.move_value = 190;

			else

				escape.move_value = 150;

			escape.state = ESCAPE_LEFT_WAIT;

			bump_count++;

		break;

		case ESCAPE_LEFT_WAIT:

			if(!escape.move) // Wait for movement to be completed

			{

				escape.speed_left = ESCAPE_SPEED_ROTATE;

				escape.dir = RIGHT;

				escape.rotate = true;

				if(bump_count > 3)

				{

					escape.move_value = 110;

					bump_count = 0;

				}

				else

					escape.move_value = 80;

				escape.state = ESCAPE_WAIT_END;

			}

		break;

		case ESCAPE_RIGHT:	

			escape.speed_left = ESCAPE_SPEED_BWD;

			escape.dir 	= BWD;

			escape.move = true;

			if(bump_count > 3)

				escape.move_value = 190;

			else

				escape.move_value = 150;

			escape.state = ESCAPE_RIGHT_WAIT;

			bump_count++;

		break;

		case ESCAPE_RIGHT_WAIT:

			if(!escape.move) // Wait for movement to be completed

			{ 

				escape.speed_left = ESCAPE_SPEED_ROTATE;		

				escape.dir = LEFT;

				escape.rotate = true;

				if(bump_count > 3)

				{

					escape.move_value = 110;

					bump_count = 0;

				}

				else

					escape.move_value = 80;

				escape.state = ESCAPE_WAIT_END;

			}

		break;

		case ESCAPE_WAIT_END:

			if(!(escape.move || escape.rotate)) // Wait for movement/rotation to be completed

				escape.state = IDLE;

		break;

	}

}



/**

 * Bumpers Event handler

 */

void bumpersStateChanged(void)

{

	if(bumper_left && bumper_right) // Both Bumpers were hit

	{

		escape.state = ESCAPE_FRONT;

	}

	else if(bumper_left)  			// Left Bumper was hit

	{

		if(escape.state != ESCAPE_FRONT_WAIT) 

			escape.state = ESCAPE_LEFT;

	}

	else if(bumper_right) 			// Right Bumper was hit

	{

		if(escape.state != ESCAPE_FRONT_WAIT)

			escape.state = ESCAPE_RIGHT;

	}

}



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

// The new Avoid Behaviour:



// Some speed values for different movements:

#define AVOID_SPEED_L_ARC_LEFT  30

#define AVOID_SPEED_L_ARC_RIGHT 90

#define AVOID_SPEED_R_ARC_LEFT  90

#define AVOID_SPEED_R_ARC_RIGHT 30

#define AVOID_SPEED_ROTATE 	60



// States for the Avoid FSM:

#define AVOID_OBSTACLE_RIGHT 		1

#define AVOID_OBSTACLE_LEFT 		2

#define AVOID_OBSTACLE_MIDDLE	    3

#define AVOID_OBSTACLE_MIDDLE_WAIT 	4

#define AVOID_END 					5

behaviour_command_t avoid = {0, 0, FWD, false, false, 0, IDLE};



/**

 * The new avoid behaviour. It uses the two ACS channels to avoid

 * collisions with obstacles. It drives arcs or rotates depending

 * on the sensor states and also behaves different after some

 * detecting cycles to avoid lock up situations. 

 */

void behaviour_avoid(void)

{

	static uint8_t last_obstacle = LEFT;

	static uint8_t obstacle_counter = 0;

	switch(avoid.state)

	{

		case IDLE: 

		// This is different to the escape Behaviour where

		// we used the Event Handler to detect sensor changes...

		// Here we do this within the states!

			if(obstacle_right && obstacle_left) // left AND right sensor have detected something...

				avoid.state = AVOID_OBSTACLE_MIDDLE;

			else if(obstacle_left)  // Left "sensor" has detected something

				avoid.state = AVOID_OBSTACLE_LEFT;

			else if(obstacle_right) // Right "sensor" has detected something

				avoid.state = AVOID_OBSTACLE_RIGHT;

		break;

		case AVOID_OBSTACLE_MIDDLE:

			avoid.dir = last_obstacle;

			avoid.speed_left = AVOID_SPEED_ROTATE;

			avoid.speed_right = AVOID_SPEED_ROTATE;

			if(!(obstacle_left || obstacle_right))

			{

				if(obstacle_counter > 3)

				{

					obstacle_counter = 0;

					setStopwatch4(0);

				}

				else

					setStopwatch4(400);

				startStopwatch4();

				avoid.state = AVOID_END;

			}

		break;

		case AVOID_OBSTACLE_RIGHT:

			avoid.dir = FWD;

			avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;

			avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;

			if(obstacle_right && obstacle_left)

				avoid.state = AVOID_OBSTACLE_MIDDLE;

			if(!obstacle_right)

			{

				setStopwatch4(500);

				startStopwatch4();

				avoid.state = AVOID_END;

			}

			last_obstacle = RIGHT;

			obstacle_counter++;

		break;

		case AVOID_OBSTACLE_LEFT:

			avoid.dir = FWD;

			avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;

			avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;

			if(obstacle_right && obstacle_left)

				avoid.state = AVOID_OBSTACLE_MIDDLE;

			if(!obstacle_left)

			{

				setStopwatch4(500); 

				startStopwatch4();

				avoid.state = AVOID_END;

			}

			last_obstacle = LEFT;

			obstacle_counter++;

		break;

		case AVOID_END:

			if(getStopwatch4() > 1000) // We used timing based movement here!

			{

				stopStopwatch4();

				setStopwatch4(0);

				avoid.state = IDLE;

			}

		break;

	}

}



/**

 * ACS Event Handler - ONLY used for LED display! 

 * This does not affect the behaviour at all! 

 * The sensor checks are completely done in the Avoid behaviour

 * statemachine.

 */

void acsStateChanged(void)

{

	if(obstacle_left && obstacle_right)

		statusLEDs.byte = 0b100100;

	else

		statusLEDs.byte = 0b000000;

	statusLEDs.LED2 = obstacle_left;

	statusLEDs.LED3 = (!obstacle_left);

	statusLEDs.LED5 = obstacle_right;

	statusLEDs.LED6 = (!obstacle_right);

	updateStatusLEDs();
	}



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

// Behaviour control:



/**

 * This function processes the movement commands that the behaviours generate. 

 * Depending on the values in the behaviour_command_t struct, it sets motor

 * speed, moves a given distance or rotates.

 */

void moveCommand(behaviour_command_t * cmd)

{

	if(cmd->move_value > 0)  // move or rotate?

	{

		if(cmd->rotate)

			rotate(cmd->speed_left, cmd->dir, cmd->move_value, false); 

		else if(cmd->move)

			move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false); 

		cmd->move_value = 0; // clear move value - the move commands are only

		                     // given once and then runs in background.

	}

	else if(!(cmd->move || cmd->rotate)) // just move at speed? 

	{

		changeDirection(cmd->dir);

		moveAtSpeed(cmd->speed_left,cmd->speed_right);

	}

	else if(isMovementComplete()) // movement complete? --> clear flags!

	{

		cmd->rotate = false;

		cmd->move = false;

	}

}



/**

 * The behaviourController task controls the subsumption architechture. 

 * It implements the priority levels of the different behaviours. 

 */

void behaviourController(void)

{

    // Call all the behaviour tasks:

	behaviour_cruise();

	behaviour_avoid();

	behaviour_escape();



    // Execute the commands depending on priority levels:

	if(escape.state != IDLE) // Highest priority - 3

		moveCommand(&escape);

	else if(avoid.state != IDLE) // Priority - 2

		moveCommand(&avoid);

	else if(cruise.state != IDLE) // Priority - 1

		moveCommand(&cruise); 

	else                     // Lowest priority - 0

		moveCommand(&STOP);  // Default command - do nothing! 

							 // In the current implementation this never 

							 // happens.

}


void akku(void)
{	
		setStopwatch1(300);
          startStopwatch1();
           
               if(adcBat  > 850)
                     setLEDs(0b001001);
               
               else if(adcBat < 851 && adcBat > 780)
               {
                     statusLEDs.LED4 = !statusLEDs.LED4;
                     statusLEDs.LED1 = !statusLEDs.LED1;
                     updateStatusLEDs();
               }
               
					else if(adcBat < 781 && adcBat > 700)
  					     setLEDs(0b000001);
					
					else if(adcBat < 701 && adcBat > 650)
					{		
							statusLEDs.LED1 = !statusLEDs.LED1;
										updateStatusLEDs();
               } 		
               else if(adcBat < 651 && adcBat > 580)
               
  					    setLEDs(0b000000);
  					else if(adcBat < 581 && adcBat > 500)
							powerOFF();
							
           setStopwatch1(0);
				
                  
            
}




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

// Main:



int main(void)

{

	initRobotBase(); 





	// Set Bumpers state changed event handler:

	BUMPERS_setStateChangedHandler(bumpersStateChanged);

	

	// Set ACS state changed event handler:

	ACS_setStateChangedHandler(acsStateChanged);

	

	powerON(); // Turn on Encoders, Current sensing, ACS and Power LED.

	setACSPwrLow(); 

	

	// Main loop

	while(true) 

	{		
		

		behaviourController();

		task_RP6System();
		akku();
		task_ACS();

	}

	return 0;

}

und hier der servo move von radbruch, aber verändert:

Code:

#include "RP6RobotBaseLib.h"

uint8_t  i, pause, servo_stellzeit, servo_delay;

void servo(uint8_t w0, uint8_t w1, uint8_t w2)
{
unsigned int count=0;
   do{
      PORTA |= E_INT1;      // E_INT1 (Pin8)
      sleep(w0);
      PORTA &= ~E_INT1;
      PORTC |= 1;            // SCL (Pin10)
      sleep(w1);
      PORTC &= ~1;
      PORTC |= 2;            // SDA (Pin12)
      sleep(w2);
      PORTC &= ~2;
      sleep(servo_delay-(w0+w1+w2));
      //sleep(127);
   } while (count++ < servo_stellzeit);
   mSleep(10*pause);
}

int main(void) {

initRobotBase();
i=0;
servo_stellzeit=15;
DDRA |= E_INT1;            // E_INT1 als Ausgang
DDRC |= 3;                  // SCL und SDA als Ausgang
// 5 - 15 - 25             // Min - Mitte - Max
servo(15,15,15);           // Grundstellung
while (1)
{
   switch (i)
   {
      case 0: i++;  servo_delay= 205;  break;

		
		
   }
   servo(10,14,15);        // Arm runter
   servo(5,13,15);
   servo(10,12,25);        // Arm hoch
   servo(15,11,10);
   servo(10,10,10);
   servo(5,9,10);
   servo(15,8,10);    
   servo(10,7,15);        // Arm runter
   servo(5,6,15);
   servo(10,5,25);        // Arm hoch
   servo(10,6,15);
   servo(10,7,15);
	servo(15,8,10);   
	servo(5,9,10);
   servo(10,10,10);
    servo(15,11,10);
   servo(10,12,25);
   servo(5,13,15);
   servo(10,14,15); 
    servo(10,15,15);
      servo(10,16,15);
      servo(10,17,15);
   servo(10,18,15);   
      servo(10,19,15);
    servo(10,20,10);
    servo(15,21,10);
   servo(10,22,25);
   servo(5,23,15);
   servo(10,24,15); 
      servo(5,23,15);
      servo(10,22,25);
       servo(15,21,10);
           servo(10,20,10);
           servo(10,19,15);  
          servo(10,18,15);  
             servo(10,17,15);
             servo(10,16,15); 
               servo(10,15,15);
}

return 0;
}

weiss nur das ich das irgendwie mit Stopwatches regeln muss... ist das richtig?

Danke !