RP6 Probleme mit M32 Erweiterunsplatine
Hy Leute,
ich habe schon einige Programme mit dem RP6 geschrieben, wie Hindernisse ausweichen, einer Lichtquelle folgen. Neuerlich habe eich mir die M32 Erweiterungsplatine bestellt und nun versuche ich wieder ein Programm zuschreiben um Hindernissen auszuweichen, doch wenn er mit den Pumpern kollidiert fährt er 30cm zurück und bleibt dann stehen und rührt dich nicht mehr. Ich bedanke mich schon im vorhinein für die Hilfe.
Mit freundlichen Grüßen fulltime
Hier ist der Sourcecode:
/************************************************** ***************************/
// Includes:
#include "RP6ControlLib.h" // The RP6 Control Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // I2C Master Library
/************************************************** ***************************/
/************************************************** ***************************/
// Include our new "RP6 Control I2C Master library":
#include "RP6Control_I2CMasterLib.h"
/************************************************** ***************************/
/**
* Timed Watchdog display only - the other heartbeat function
* does not work in this example as we use blocked moving functions here.
*/
void watchDogRequest(void)
{
static uint8_t heartbeat2 = false;
if(heartbeat2)
{
clearPosLCD(0, 14, 1);
heartbeat2 = false;
}
else
{
setCursorPosLCD(0, 14);
writeStringLCD_P("#");
heartbeat2 = true;
}
}
/************************************************** ***************************/
// I2C Requests:
/**
* The I2C_requestedDataReady Event Handler
*/
void I2C_requestedDataReady(uint8_t dataRequestID)
{
checkRP6Status(dataRequestID);
}
/************************************************** ***************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR - TWI STATE: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/*********************************
********Eigene Funktionen*********
**********************************/
void bumpersStateChanged(void)
{
if(bumper_left)
{
move(60,BWD,DIST_MM(300),BLOCKING);
showScreenLCD("Kollision","Links");
rotate(60,RIGHT,90,BLOCKING);
mSleep(2000);
}
if(bumper_right)
{
move(60,BWD,DIST_MM(300),BLOCKING);
showScreenLCD("Kollision","Rechts");
rotate(60,LEFT,90,BLOCKING);
mSleep(2000);
}
clearLCD();
moveAtSpeed(60,60);
}
void acsStateChanged(void)
{
if(obstacle_left) moveAtSpeed(60,30);
if(obstacle_right) moveAtSpeed(30,60);
if(!obstacle_left || !obstacle_right) moveAtSpeed(60,60);
return;
}
/************************************************** ***************************/
// Main function - The program starts here:
int16_t main(void)
{
initRP6Control();
initLCD();
// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
// ---------------------------------------
// Init TWI Interface:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);
sound(180,80,25);
sound(220,80,25);
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
moveAtSpeed(60,60);
// ---------------------------------------
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);
while(true)
{
task_checkINT0();
task_I2CTWI();
}
return 0;
}
