Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"
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');
}
void Bumpers(void)
{
if (bumper_left || bumper_right)
{
moveAtSpeed(0,0);
if (bumper_left)
{
changeDirection(BWD);
moveAtSpeed(150,150);
}
else if (bumper_right)
{
move(60,BWD,1000,1);
changeDirection(FWD);
moveAtSpeed(150,150);
}
else if (bumper_left && bumper_right)
{
move(60,BWD,500,1);
rotate(60,LEFT,90,1);
changeDirection(FWD);
moveAtSpeed(60,60);
}
}
}
void ACS (void)
{
if(obstacle_left)
{
setCursorPosLCD(1, 12);
writeStringLCD_P("LEFT");
rotate(50, RIGHT, 180, 1);
}
else
{
clearPosLCD(1, 12, 4);
}
if(obstacle_right)
{
setCursorPosLCD(1, 0);
writeStringLCD_P("RIGHT");
moveAtSpeed(90,40);
}
else
{
clearPosLCD(1, 0, 5);
}
if(obstacle_left && obstacle_right)
{
setLEDs(0b0110);
setCursorPosLCD(1, 7);
writeStringLCD_P("MID");
moveAtSpeed(0,0);
changeDirection(FWD);
moveAtSpeed(50,50);
}
else
{
setLEDs(0b0000);
clearPosLCD(1, 7, 3);
}
if(obstacle_left)
{
setLEDs(0b0001);
}
if(obstacle_right)
{
setLEDs(0b1000);
}
if(obstacle_left && obstacle_right)
{
sound(140,10,0);
}
else
{
if(obstacle_left)
sound(100,5,0);
if(obstacle_right)
sound(120,5,0);
}
}
int main(void)
{
initRP6Control();
initLCD();
ACS_setStateChangedHandler(ACS);
BUMPERS_setStateChangedHandler(Bumpers);
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
sound(180,80,25);
sound(220,80,25);
setLEDs(0b1111);
showScreenLCD("ATTENTION", "");
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);
startStopwatch1();
changeDirection(FWD);
moveAtSpeed(80,80);
while(true)
{
task_I2CTWI();
task_LCDHeartbeat();
}
return 0;
}
wenn ich moveAtSpeed(x,x) schreibe, dann fährt er auch, aber wenn ich rotate benutze, dann macht er das zwar auch, aber danach passiert nichts mehr
Lesezeichen