RP6 fernsteuern

[I♥ROBOTIC]

ich hab jetzt and die stelle IRCOMM_OFF() eingetragen und nun ließ sich das Programm auch kompilieren und mein RP6 fährt auch, allerding nicht ganz so wie gewünscht:

1. wen man gas gibt (egal ob vor- oder rückwärts) er fährt hin und wieder mal rückwärts, dann aber mal doch wieder vorwärts.....
2. ich muss nach jeder drehung oder fahrt 2 oder 3 sekunden warten, weil der rp6 sonst die vorherige bewegung fortführt (also wenn ich nach rechts drehe, und dan plötzlich rückwärts gasgebe dreht er weiter...

ich hab mir das ehrlich gesagt leichter vorgestellt -.-

hier mein Programm bisher:

#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"    
#include "RP6Control_I2CMasterLib.h"

uint8_t frequenz=0;
uint8_t furz=0;

void I2C_requestedDataReady(uint8_t dataRequestID) 
{ 
   checkRP6Status(dataRequestID); 
} 

void I2C_transmissionError(uint8_t errorState) 
{ 
   writeString_P("\nI2C ERROR - TWI STATE: 0x"); 
   writeInteger(errorState, HEX); 
   writeChar('\n'); 
   
   if(getStopwatch1() > 1)
   {
	if(frequenz == 255)
	{
		furz=1;
	}
	
	else if(frequenz == 0)
	{
		furz=0;
	}
   
	if(furz){frequenz--;}
	else{frequenz++;}
   
   setBeeperPitch(frequenz);
   setStopwatch1(0);
   }
}

volatile uint16_t speed=0;
volatile uint16_t dir=0;
volatile uint16_t speedcount=0;
volatile uint16_t dircount=0;

ISR(TIMER1_COMPA_vect)
{
	if (PINA & ADC5)
	{
		speedcount++;
	}
	else 
	{
		if (speedcount > 0)
		{
			speed=speedcount;
		}
		speedcount=0;
	}
	
	
	if (PINA & ADC3)
	{
		dircount++;
	}
	else 
	{
		if (dircount > 0)
		{
			dir=dircount;
		}
		dircount=0;
	}
}

void initRC(void)
{
	TCCR1A = (0 << WGM11) | (0 << WGM10) | (0 << COM1A0) | (0 << COM1A1);	// CTC-Mode4, ohne OCR-Pin
	TCCR1B = (0 << WGM13) | (1 << WGM12) | (0 << CS12)  | (1 << CS11) | (0 << CS10); // CTC und prescaler /8
	TIMSK |= (1 << OCIE1A); 										// Interrupt ein
	OCR1A  = 20; // 100kHz?
	
	DDRA &= ~ADC5;
	PORTA &= ~ADC5; 
	DDRA &= ~ADC3;
	PORTA &= ~ADC3; 
}
	
	

int main(void)
{
	initRP6Control();
	initLCD();
	initRC();
	
	startStopwatch1();
	
	mSleep(1000);
	
	I2CTWI_initMaster(100);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady); 
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	uint16_t speed1, dir1;
	
while(1)
{
	cli(); // Interrupts verbieten
	speed1=speed;
	dir1=dir;
	sei(); // Interrupts wieder zulassen
	
	rc_pwr=0;
	rc_dir=0;

	if (dir1<140) { rc_dir=150; changeDirection(LEFT); writeInteger(dir1, DEC); writeString("\t\t");}
	if (dir1>170) { rc_dir=150; changeDirection(RIGHT); writeInteger(dir1, DEC); writeString("\t\t");}
	if (speed1<150) { rc_pwr=speed1+50; changeDirection(BWD); writeInteger(speed1, DEC); writeString("\n");}
	if (speed1>160) { rc_pwr=speed1-50; changeDirection(FWD); writeInteger(speed1, DEC); writeString("\n");}
	rc_pwr*=3;
   if (rc_pwr)
   {
		if (dir1<150) setMotorPower(rc_pwr-rc_dir,rc_pwr+rc_dir);
		else if (dir1>160) setMotorPower(rc_pwr+rc_dir,rc_pwr-rc_dir);
		else setMotorPower(rc_pwr,rc_pwr);
   }
	 else
	{
	   setMotorPower(rc_dir, rc_dir);
	}
	
	task_checkINT0(); 
	task_I2CTWI();
}
	return 0;
}