/*
* writes data from a given buffer to a file on the sd card
*/
int WriteToFile(int fileID, char* Buffer, unsigned int len, int type)
{
UINT s2;
FRESULT res;
int oldFileID=curFileID; //save current file id
//Ausgabe de Zugriffs auf PIN 16
PDR03_P6 = 0;
//check if sd card is initialized
if(!sdcInitDone)
return -1;
res = 1;
//set current file per file ID
SetCurrentFileID(fileID, type);
res = f_open(&ffile1,(char*)sdcFilename, FA_OPEN_ALWAYS | FA_WRITE);
if (res == FR_OK){
res = f_write(&ffile1,(char*) Buffer,len,&s2);
if (res != FR_OK){
Seg_Hex(0x0D);
}
}
else{
Seg_Hex(0x0C);
}
//OK , INVALID_OBJECT, NOT_READY, FR_RW_ERROR
f_close(&ffile1);
//set filename config back to original value
SetCurrentFileID(oldFileID, type);
PDR03_P6 = 1;
return res;
}
/*
* initializes sd card and reads config file
*/
void initSDCard()
{
int retVal=0;
int timeout = 0;
DWORD P1;
// if(sdcInitDone) //init procedure already started
// return; // Thread wird sowieso beendet
curFileID=-99;
//Set output Pins for LEDs
DDR03_D7 = 1; //Read (Pin 16)
DDR03_D6 = 1; //Write (Pin 15)
PDR03_P7 = 1;
PDR03_P6 = 1;
mmc_initPorts();
// Seg_Hex(0xC);
//wait for card to be inserted
while(mmc_getCardStatus() > 0) //it is 0 if we have a card
{
//return after 10 sec
if(timeout > 20)
return;
#ifdef SDCARD_DEBUG
printf("waiting for card.. \r\n");
#endif
os_wait(500);
timeout++;
}
// Seg_Hex(0xF);
os_wait(200);
//initialize card
retVal=mmc_init();
os_wait(200);
//mount FAT file system
P1=0;
f_mount((BYTE)P1, &fatfs[P1]);
if(curFileID==-99) //has not been set
SetCurrentFileID(1,1);
sdcInitDone=1;
//read configuration file from sd card
readConfigFromFile();
Seg_Hex(0x00);
}
/**
* Hilfsmethode zum Fahren Etappen fester Länge
*/
void driveDistance(void){
if (DriveLength > 0){
if (wheel_turns <= 0){
wheel_turns = (DriveLength)/HALL_RESOLUTION;
realdistance = 0;
//Zwei Wechsel der Sensorwerte = 1 Radumdrehung = 21,04cm
if ((DriveLength*10) % (int16_t)(HALL_RESOLUTION*10) > ((DriveLength*10)/2)) wheel_turns++; //Faktor 10 weil kein Mod mit double möglich
set_distanceReached(FALSE);
}
if (Drive_Speed > 3 && wheel_turns > 2) wheel_turns = wheel_turns - 2; //weil bremsen so lange dauert
if ( init == FALSE){
driveTime = os_getTime();
init = TRUE;
}
if (distance - wheel_turns < 0){
Seg_Hex(0x07);
}
if (distance - wheel_turns >= 0){
//Werte zurücksetzen
wheel_turns = 0;
init = FALSE;
drive(0,0,0);
Drive_SetMotor(0);
realdistance = 0;
DriveLength = 0;
set_distanceReached(TRUE);
Seg_Hex(0x08);
}
}
}
/*
* reads data from a file into a given buffer
*/
int ReadFromFile(int fileID, char* Buffer, unsigned int start, unsigned int len, int type)
{
FRESULT res;
UINT ByteRead =0;
int oldFileID=curFileID; //save current file id
//Ausgabe de Zugriffs auf PIN 15
PDR03_P7 = 0;
//check if sd card is initialized
if(!sdcInitDone)
return -1;
//set current file per file ID
SetCurrentFileID(fileID, type);
res = f_open(&ffile1,(char*)sdcFilename, FA_READ);
if (res != FR_OK){
}
else{
if(start>0)
{
f_lseek(&ffile1, start);
}
res = f_read(&ffile1, Buffer, (UINT)len, &ByteRead);
if (res != FR_OK) Seg_Hex(0x0F);
}
f_close(&ffile1);
//set filename config back to original value
SetCurrentFileID(oldFileID, type);
PDR03_P7 = 1;
return res;
}
/**
* Interrupt um die Radumdrehungen zu zählen
*/
__interrupt void irq_hall(void){
hall = PDR01_P5;
PDR01_P0 = hall;
newturn = TRUE;
realdistance++;
totalDistance++;
#ifdef CARTOGRAPHY_SCENARIO
if (realdistance > wheel_turns && wheel_turns != 0 && turn_Active == FALSE){
distance = wheel_turns;
Drive_SetMotor(0);
Seg_Hex(0x0F);
}
#endif
#ifdef SLAM_SCENARIO
if (turn_Active == FALSE)
Drive_SetMotor(0);
#endif
ELVR0_LA7 = hall;
EIRR0_ER7 = 0; // auf 0 setzen um auslösen des nächsten Interrupts zu ermöglichen
}
/**
* SensorRange = einschränkung der Reichweite. Verhindert unnötig lange Berechnung
* Return: Distance
*/
uint16_t driven_before(int8_t type, uint16_t SensorRange) {
double diffR;
#ifdef CARTOGRAPHY_SCENARIO
gps_reducedData_t *own;
uint16_t angle, diffL;
int16_t xCollision = 0, yCollision = 0, ownX, ownY;
int32_t newX, newY;
//Aktuelle Orientierung beachten!!! Orientierung in Abhängigkeit der Fahrzeugorientierung und dem aktuell betrachteten Sensor
own = get_ownCoords();
switch(type) {
case RIGHT_SENSOR:
angle = AngleMinus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2; //siehe Anforderungen in wallFollow für align = 2
break;
case LEFT_SENSOR:
angle = AnglePlus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;//siehe Anforderungen in wallFollow für align = 2
break;
case FRONT_SENSOR:
angle = own->angle;
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;
break;
default: angle = own->angle; break;
}
SensorRange = SensorRange / 5;
ownX = CoordinatesToMap(own->x);
ownY = CoordinatesToMap(own->y);
if (Sema != NULL) {
while ( xSemaphoreTake( Sema, ( portTickType ) 10 ) != pdTRUE ) {
os_wait(10);
}
newX = (SensorRange*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX;
ownX = (2*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX; //Ein stück aus dem Urpsrung rausgehen
newY = (SensorRange*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
ownY = (2*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
//gibt es ein bereits befahrenes Gebiet/Hindernis das zwischen Sensorwert und Auto liegt?
diffL = calc_reachability(&xCollision, &yCollision, ownX, ownY, (int16_t)newX, (int16_t)newY);
xSemaphoreGive(Sema);
} else return 600; //600 = max. Sensorreichweite
if (diffL == 1) {
diffR = sqrt(abs16(ownX - xCollision)*abs16(ownX - xCollision) + abs16(ownY - yCollision)*abs16(ownY - yCollision));
diffR = diffR*5;
if (diffR < 100) {
Seg_Hex(0x00);
}
else Seg_Hex(0x01);
}
else {
diffR = 600; // 600 für nichts gefunden
}
if (diffR > 600) diffR = 600;
#endif
return (uint16_t) diffR;
}
