Code:
// 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"
/*****************************************************************************/
#define I2C_RP6_BASE_ADR 10
//#define adc2 ;
/*
* 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:
void I2C_requestedDataReady(uint8_t dataRequestID)
{
checkRP6Status(dataRequestID);
}
/*****************************************************************************/
void bumpersStateChanged(void)
{
// Bumper status changed
writeString_P("Bumper betätigt: ");
if(bumper_right && bumper_left)
{
writeString_P("Beide!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}
else if(bumper_left)
{
writeString_P("Links!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}
else if(bumper_right)
{
writeString_P("Rechts!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}
else
{
writeString_P("Frei!");
}
writeChar('\n');
}
int z; //Globale Variable
void scan (int ds)
{
writeString_P("\nDS\n");
writeInteger(ds, DEC );
int max;
do
{
task_checkINT0();
task_I2CTWI();
sound(20,50,1);//Tonhöhe,Zeit,Pause
int16_t adc2r = readADC(ADC_2);
int16_t adc3r = readADC(ADC_3);
max=adc2r-adc3r;
writeString_P("\nrechts SCAN\n");
writeInteger(adc2r, DEC );
writeString_P("\nlinks SCAN\n");
writeInteger(adc3r, DEC );
moveAtSpeed(50,0); //Drehe
setLEDs(0b1111);
}
while(!(max<(ds-15))); //Bis neu gemessener Wert stark von dem Übergebenen Anfangswert abweicht
moveAtSpeed(0,0);
uint8_t i;
uint8_t j;
for(i = 1; i < 4; i++) //Sirene--> Abeichung zum Startwert erkannt
{
for(j = 230; j > 46; j-=2)
{
setBeeperPitch(j);
mSleep(5);
}
for(; j < 230; j+=2)
{
setBeeperPitch(j);
mSleep(5);
}
// Beeper aus:
setBeeperPitch(0);
}
rotate(40,LEFT,10,BLOCKING); //Ein kleines Stück zurückdrehen
mSleep(2000);
changeDirection(FWD);
}
void Snake (int dif)
{
task_checkINT0();
task_I2CTWI();
sound(20,40,0);
sound(215,60,0);
int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;
int a,b;
a=(sSnake-sSnake2); //Neuer Wert wird genommen
b=0;
if (a>dif+12)
{
writeString_P("\na ist grosser dif\n");
writeInteger(a, DEC );
setLEDs(0b1100);
moveAtSpeed(50,0); //rechts drehen
b=1;
}
else if(a<dif-12)
{
writeString_P("\na ist kleiner dif\n");
writeInteger(a, DEC );
setLEDs(0b0011);
moveAtSpeed(0,50); //links drehen
b=1;
}
else if (b==0)
{
writeString_P("\na ist gleich dif\n");
writeInteger(a, DEC );
setLEDs(0b0110);
moveAtSpeed(40,40); //gerade fahren Richtung Feuer
z=a;
}
else if((sSnake>420||sSnake2>420))
{
writeString_P("\nFeuer erreicht Löschen\n");
writeString_P("\nFeuer erreicht links\n");
writeInteger(sSnake, DEC );
writeString_P("\nFeuer erreicht rechts\n");
writeInteger(sSnake2, DEC );
moveAtSpeed(0,0);
DDRC |= IO_PC7; //PC7 als Ausgang Lüfter 5-Volt
PORTC |= IO_PC7; // Lüfter an
mSleep(500); // 0,5s
PORTC &= ~IO_PC7; // Lüfter aus
move(60,BWD, DIST_MM(200), BLOCKING);
rotate(40,RIGHT,65,BLOCKING);
mSleep(4000);
changeDirection(FWD);
adc2 = readADC(ADC_2);
sSnake = adc2;
adc3 = readADC(ADC_3);
sSnake2 = adc3;
a=(sSnake-sSnake2);
z=a;
scan(z);
}
}
// 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');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6Control();
initLCD();
writeString_P("\n\nRP6 CONTROL M32 I2C Master Example Program!\n");
writeString_P("\nMoving...\n");
DDRA &= ~ADC3;
DDRA &= ~ADC2;
// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
BUMPERS_setStateChangedHandler(bumpersStateChanged);
// ---------------------------------------
// Init TWI Interface:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
sound(180,80,25);
sound(220,80,25);
setLEDs(0b1001);
// ---------------------------------------
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);
int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
writeString_P("\nadc2 MAIN\n");
writeInteger(sSnake, DEC );
int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;
writeString_P("\nadc3 MAIN\n");
writeInteger(sSnake2, DEC );
int dd;
dd=(sSnake-sSnake2); //Startwert
scan(dd);
//Nach dem ersten Scannen geht es hier weiter
setLEDs(0b0001);
sSnake=readADC(ADC_2);
writeString_P("\nneu rechts ADC2\n");
writeInteger(sSnake, DEC );
sSnake2=readADC(ADC_3);
writeString_P("\nneu links ADC3\n");
writeInteger(sSnake2, DEC );
dd=(sSnake-sSnake2); //Feuer gefunden Neuer Wert
writeString_P("\ndd zu dif\n");
writeInteger(dd, DEC );
z=dd;
while (true)
{
task_checkINT0();
task_I2CTWI();
Snake(dd);
dd=z;
}
return 0;
}
Danke
Zitieren
Lesezeichen