Freescale Race Challenge 2011
Tento rok som sa prvýkrát zúčastnill Freescale Race Challenge. O tejto súťaži som sa dozvedel z plagátov polepených na Ústave rádiotechniky. Cieľom súťaže bolo naprogramovať autíčko jazdiace po autodráhe tak, aby v čo najkratšom čase prešlo určený počet koliečok. Do súťaže sa mohli zapojiť ako jednotlivci, tak aj 3-členné týmy. Každé prihlásené družstvo dostalo autíčko a osadenú dosku so súčiastkami, medzi ktorými bol Freescale mikrokontrolér s jadrom ColdFire V1, akceleromeder (merajúci zrýchlenie vo všetkých 3 osiach) a výkonový mostík na ovládanie motora.
Našou základnou stratégiou bolo vyhodnocovanie dát zo senzorov v reálnom čase. K senzorom sme doplnili ešte meranie otáčok, pomocou 2 infračervených diód. Ako odrazovú plôšku sme použili kúsok alobalu na pneumatike auta. Takýto spôsob merania otáčok nebol moc citlivý, ale pre vyhodnotenie blížiacej sa zákruty dostačujúci.
Po skonštruovaní hardware auta sme sa pustili do tvorby riadiaceho software. Nepracovali sme na mapovaní trati, čo sa neskôr ukázalo ako najvhodnejší spôsob a najlepšia stratégia pri prechádzaní trate, ale sústredili sme sa na vyhodnotení dát zo senzorov v reálnom čase.
Empiricky som z grafov nameraných hodnôt stanovil hranicu otáčok a rýchlosti auta v zákrute, aby som vedel, kedy mám prestať zrýchľovať a nevyletel z trate.
Našťastie sa podarilo spraviť analýzu dát predtým ako sa mi deň pred súťažou pokazil slot na SD kartu a stratil som možnosť načítavania dát z karty. Všetko však nakoniec dopadlo dobre a zobral som si ponaučenie, že vždy treba mať poruke náhradné diely.
Tu je náš biely tátoš v plnej kráse. Silný motor a dobrý vodič sú zaručená cesta k úspechu!
A tu vidieť vnútornosti. Špičkové prevedenie profesionálneho družstva.
Následuje zdrojový kód, ktorý som krvopotne látal posledné dni pred súťažou dokopy a vďaka ktorému sme skončili na 8.mieste. Dúfajme, že ďalší rok to bude lepšie.
/****************************************************************************** * * Freescale Semiconductor Inc. * (c) Copyright 2009 Freescale Semiconductor, Inc. * ALL RIGHTS RESERVED. * ******************************************************************************* * * File main.c * * Author Milan Brejl * * Version 1.0 * * Date 26-Nov-2009 * * Brief Quick start with the Self-Driven Slot Car development * *******************************************************************************/ #include/* for EnableInterrupts macro */ #include "derivative.h" /* include peripheral declarations */ #include "ramvector.h" /* redirects interrupt vectors into RAM */ #include "slotcar.h" /* slot car HW related macros and routines */ #include "ff.h" /* access to FAT file system on SD card */ /****************************************************************************** * Constants and macros ******************************************************************************/ #define LOG_BUFFER_SIZE 32 #define ACCXFILT_THRESHOLD_LEFT 35000 /* depends on the particular device */ #define ACCXFILT_THRESHOLD_RIGHT 29000 /* depends on the particular device */ #define ACCYFILT_THRESHOLD 37000 /* depends on the particular device */ /****************************************************************************** * Global variables ******************************************************************************/ /* accelerometer */ unsigned short accX, accY, accZ; /* 12-bit unsigned samples */ long accXFilt, accYFilt; /* 16-bit unsigned filtered values */ unsigned short daccX, daccY; /* motor */ unsigned short motorVoltage; /* range 0 to 6000 */ unsigned short motorCurrent; /* 12-bit unsigned sample */ /* track voltage */ unsigned short trackVoltage; /* 12-bit unsigned sample */ /* global time */ unsigned long timeCounter; /* global free-running 1/2ms counter */ unsigned long casVypadnutia; unsigned short svietim = 0; unsigned short boloKrizenie = 0; unsigned short pocetKrizeni = 0; /* pocet otacok */ unsigned long otacky = 0; unsigned long rychlost = 0; struct{ unsigned short krok; unsigned long poslednyKrok; unsigned short zrychlenie; long accX; unsigned short maxOtacky; } jazdaZakrutou; struct{ unsigned short krok; unsigned long poslednyKrok; unsigned short zrychlenie; unsigned short maxOtacky; long accX; } jazdaRovinkou; /* logBuffer */ struct { unsigned long timeCounter; unsigned short accXFilt; unsigned short accYFilt; unsigned short accZ; unsigned short trackVoltage; unsigned short motorCurrent; unsigned long rychlost; unsigned long motorVoltage; } logBuffer[LOG_BUFFER_SIZE]; byte pocetZaznamov = 0; byte poslednyZaznam = 0; unsigned long celkomZaznamov = 0; /* SD card */ FATFS fileSystem; /* FAT driver File System Object */ FIL file; /* log file File Object */ /****************************************************************************** * Functions ******************************************************************************/ void zapni(void){ SET_LED_HL_ON; SET_LED_HR_ON; SET_LED_BR_ON; SET_LED_BL_ON; svietim = 1; } void zapniPredne(void){ SET_LED_HL_ON; SET_LED_HR_ON; SET_LED_BR_OFF; SET_LED_BL_OFF; svietim = 1; } void zapniZadne(void){ SET_LED_HL_OFF; SET_LED_HR_OFF; SET_LED_BR_ON; SET_LED_BL_ON; svietim = 1; } void zhasni(void){ SET_LED_HL_OFF; SET_LED_HR_OFF; SET_LED_BR_OFF; SET_LED_BL_OFF; svietim = 0; } void prepniSvetla(){ if (svietim == 0){ zapni(); }else{ zhasni(); } } /* FilterHalfBand8Lynn function */ /* * The FilterHalfBand8Lynn function filters the input 12-bit sample * using an 8-level multirate half-band system based on Lynn filters. * Every 128th call the function returns a nonzero value which is a * sample of the filtered and decimated output signal. */ unsigned int FilterHalfBand8Lynn(unsigned int input) { static unsigned char step; static unsigned int a0, a1, a2; static unsigned int b, b0, b1, b2; static unsigned int c, c0, c1, c2; static unsigned int d, d0, d1, d2; static unsigned int e, e0, e1, e2; static unsigned int f, f0, f1, f2; static unsigned int g, g0, g1, g2; static unsigned int h0, h1, h2; unsigned char step0; unsigned int output = 0; step0 = step; step++; switch (step0 ^ step) { case 1: a2 = input>>1; b = a0 + a1 + a2; /* 1st level filter output, 13-bit range */ a0 = a2; break; case 3: a1 = input; b2 = b>>1; c = b0 + b1 + b2; /* 2nd level filter output, 14-bit range */ b0 = b2; break; case 7: a1 = input; b1 = b; c2 = c>>1; d = c0 + c1 + c2; /* 3rd level filter output, 15-bit range */ c0 = c2; break; case 15: a1 = input; b1 = b; c1 = c; d2 = d>>1; e = d0 + d1 + d2; /* 4th level filter output, 16-bit range */ d0 = d2; break; case 31: a1 = input; b1 = b; c1 = c; d1 = d; e2 = e>>2; f = e0 + e1 + e2; /* 5th level filter output, 16-bit range */ e0 = e2; break; case 63: a1 = input; b1 = b; c1 = c; d1 = d; e1 = e>>1; f2 = f>>2; g = f0 + f1 + f2; /* 6th level filter output, 16-bit range */ f0 = f2; break; case 127: a1 = input; b1 = b; c1 = c; d1 = d; e1 = e>>1; f1 = f>>1; g2 = g>>2; h2 = g0 + g1 + g2; /* 7th level filter output, 16-bit range */ g0 = g2; break; case 255: a1 = input; b1 = b; c1 = c; d1 = d; e1 = e>>1; f1 = f>>1; g1 = g>>1; output = (h0>>2) + (h1>>1) + (h2>>2); /* 8th level filter output, 16-bit range */ h0 = h1; h1 = h2; break; } return(output); } /****************************************************************************** * Interrupt Handlers ******************************************************************************/ /* Real Time Counter Interrupt (10ms period) */ interrupt VectorNumber_Vrtc void Vrtc_isr(void) { /* Update iodisk_sd internal timers */ disk_timerproc(); logBuffer[pocetZaznamov].timeCounter = timeCounter; logBuffer[pocetZaznamov].accXFilt = accXFilt; logBuffer[pocetZaznamov].accYFilt = accYFilt; logBuffer[pocetZaznamov].accZ = accZ; logBuffer[pocetZaznamov].trackVoltage = trackVoltage; logBuffer[pocetZaznamov].motorCurrent = motorCurrent; logBuffer[pocetZaznamov].rychlost = rychlost; logBuffer[pocetZaznamov].motorVoltage = motorVoltage; pocetZaznamov++; if(pocetZaznamov == LOG_BUFFER_SIZE) pocetZaznamov = 0; /* Zapise aktualny pocet otacok do rychlosti a vynuluje pocitadlo */ if (timeCounter % 100 == 0){ rychlost = otacky; otacky = 0; } celkomZaznamov++; /* Clear the interrupt flag */ RTCSC_RTIF = 1; } /* Timer/PWM Interrupt (1/2ms interrupt period) */ interrupt VectorNumber_Vtpm2ovf void Vtpm2ovf_isr(void) { /* clear interrupt flag */ TPM2SC_TOF = 0; /* update PWM duty-cycle according to the global variable motorVoltage */ SET_MOTOR_VOLTAGE(motorVoltage); /* Start sequence of analog inputs sampling */ START_CONV(MOTOR_CURRENT); /* Update the global 1/2ms timeCounter */ timeCounter++; /* Aby som zmeral presny cas vypadnutia naatia potrebujem jemne hodinky */ if (GET_INT1 == 0 && GET_INT2 == 0){ /* Ak som niekde mimo drahy pocitam si polmilisekundy ako dlho som vypadol */ casVypadnutia++; }else{ /* Ak som na drahe, tak som doteraz nikdy nevypadol */ if (GET_INT1 == 1 || GET_INT2 == 1){ casVypadnutia = 0; } } } /* ADC Conversion Complete Interrupt (sequence of 5 interrupts every 1/2ms)*/ interrupt VectorNumber_Vadc void Vadc_isr(void) { unsigned int output; /* * Read the new sample using READ_ADC_SAMPLE macro. * The reading also clears the interrupt flag. */ switch(ADCSC1_ADCH) { case MOTOR_CURRENT: motorCurrent = READ_ADC_SAMPLE; START_CONV(ACC_X); break; case ACC_X: accX = READ_ADC_SAMPLE; START_CONV(ACC_Y); /* 8-Level Half Band filter */ /* if ((output = FilterHalfBand8Lynn(accX)) != 0) { accXFilt = output; } */ output = 1; accXFilt = accXFilt - (accXFilt>>4) + accX; break; case ACC_Y: accY = READ_ADC_SAMPLE; START_CONV(ACC_Z); /* EWMA filter */ accYFilt = accYFilt - (accYFilt>>4) + accY; break; case ACC_Z: accZ = READ_ADC_SAMPLE; if(GET_INT1) { INT1_TO_ADC_CONNECT; START_CONV(TRACK_VOLTAGE_1); } else if(GET_INT2) { INT2_TO_ADC_CONNECT; START_CONV(TRACK_VOLTAGE_2); } break; case TRACK_VOLTAGE_1: case TRACK_VOLTAGE_2: INTx_TO_ADC_DISCONNECT; trackVoltage = READ_ADC_SAMPLE; break; } } /* Keyboard Interrupt */ interrupt VectorNumber_Vkeyboard void Vkeyboard_isr(void) { otacky++; KBI1SC_KBACK=1; } unsigned short stavNapatia(void){ /* Ak som na prekrizeny, tak pre uC je to dlhy vypadok napatia u nas dlhsi ako 20ms */ if (casVypadnutia/2 > 20){ /* meriam totiz polmilisekundy */ return 0; }else{ return 1; } } void loguj(void){ /* Logujem len ked mam napatia, inak by som odpiekol SD-kartu */ if (casVypadnutia == 0){ if (pocetZaznamov != poslednyZaznam){ /* teraz nezapisujem hodnoty ale sleduje ci auto spravne rozoznava zakrutu a rovinku f_printf(&file,"%d %d %d %d %d %d %d %d %d\n", logBuffer[pocetZaznamov].timeCounter, logBuffer[pocetZaznamov].accXFilt, logBuffer[pocetZaznamov].accYFilt, logBuffer[pocetZaznamov].accZ, logBuffer[pocetZaznamov].trackVoltage, logBuffer[pocetZaznamov].motorCurrent, logBuffer[pocetZaznamov].rychlost, logBuffer[pocetZaznamov].motorVoltage, daccX); */ poslednyZaznam++; if (poslednyZaznam == LOG_BUFFER_SIZE) poslednyZaznam = 0; }else{ f_sync(&file); } } } void logujAktualnyStav(void){ /* Logujem len ked mam napatia, inak by som odpiekol SD-kartu */ if (casVypadnutia == 0){ f_printf( &file, "cas: %d\nmotorVoltage: %d\nrychlost: %d\naccX: %d\naccY: %d\ndaccX: %d\n\n", motorVoltage, rychlost, accXFilt, accYFilt, daccX ); } } /****************************************************************************** * Main ******************************************************************************/ void main(void) { /* stredna hodnota z akcelerometra, urcena empiricky */ unsigned short accFilterStred = 30700; word val, i = 0; byte idx; char fileName[] = "00000000.CSV"; /* This needs to be here when running in bootloader framework */ RedirectInterruptVectorsToRAM(); /******************* Device and Board Initialization ***********************/ SlotCarInit(); /* enable interrupts */ EnableInterrupts; /* create new file on SD card for data logging */ f_mount(0, &fileSystem); do { idx = 8; val = i++; do { fileName[--idx] = (char)(val % 10 + '0'); val /= 10; } while (idx && val); } while(FR_EXIST == f_open(&file, fileName, FA_CREATE_NEW | FA_WRITE)); f_printf(&file, "%s\n", "#timeCounter;accXFilt;accYFilt;accZ;trackVoltage;motorCurrent;otacky;motorVoltage"); /* enable motor */ motorVoltage = 1600; MOTOR_ENABLE; zapni(); /* - tieto dve struktury urcuju nastavenie jazdy v zakrute a na rovinke - nastavujem maxOtacky, ktore sa nesmu prekrocit - krok, urcuje ako casto sa ma regulacia vykonat - zrychlenie urcuje o kolko sa inkrementuje motorVoltage (pri spomalovani sa tiez pouziva ale je prenasobeene empirickou hodnotou, vid nizsie) - poslednyKrok je pomocna premenna ktora si pamata v akom case sa vykonala posledna regulacia auta */ jazdaZakrutou.maxOtacky = 30; //30 jazdaZakrutou.krok = 30; jazdaZakrutou.zrychlenie = 50; jazdaZakrutou.poslednyKrok = 0; jazdaRovinkou.maxOtacky = 30; //40 jazdaRovinkou.krok = 30; jazdaRovinkou.zrychlenie = 40; jazdaRovinkou.poslednyKrok = 0; jazdaRovinkou.accX = 0; /*************************** Background Loop *******************************/ while(1) { loguj(); /* Testujem ako dlho mi vypadlo napatie */ if (stavNapatia() == 0){ /* Ak som na krizeni tak si to zapamatam */ boloKrizenie = 1; }else{ /* - daccX je premenna ktora sleduje relativnu zmenu zrychlenia vzhladom na strednu hodnotu - strednu hodnotu udava rucne na zaciatku main() */ if (accXFilt > accFilterStred){ daccX = accXFilt - accFilterStred; }else{ daccX = accFilterStred - accXFilt; } /* tymto preskocim pociatocne nezrovnalosti a zacnem snimat az auto dava nejake rozumne hodnoty */ if (timeCounter > 2000){ /* pomocou daccX rozlisujem ci idem po rovinke alebo som v zakrute */ if (daccX > 1000){ /* - jazdaZakrutov.krok udava ako casto procesor vchadza do regulacie auta v zakrutach - ak som do zakruty siel z rovinky, tak poslednyKrok je nulovy a teda to vojde do tejto podmienky */ if (timeCounter - jazdaZakrutou.poslednyKrok > jazdaZakrutou.krok){ /* ak je zrychlenie mensie ako predosle, tak mozem pridat kym nedosiahnem hranicne otacky */ if (daccX < jazdaZakrutou.accX && rychlost < jazdaZakrutou.maxOtacky){ f_printf(&file, " zrychlujem\n"); logujAktualnyStav(); motorVoltage += jazdaZakrutou.zrychlenie; jazdaZakrutou.krok += 5; /* nastavim svetla nech vidim ze zrychlujem */ zhasni(); zapniPredne(); }else{ /* pri dosiahnuti hranicneho zrychlenia a otacok zacnem spomalovat */ if (rychlost > jazdaZakrutou.maxOtacky){ f_printf(&file, " spomalujem\n"); logujAktualnyStav(); /* spomalujem len do urcitej hodnoty aby to zas nezastavilo uplne na nule */ if (motorVoltage > 1400){ /* rychlost uberam rychlejsie, ako pridavam plyn, empiricky dosateny nasobitel */ motorVoltage -= jazdaZakrutou.zrychlenie; /* zjemnim krok, lebo brzdit treba rychlejsie */ if (jazdaZakrutou.krok > 5) jazdaZakrutou.krok -= 5; } /* nastavim si svetla */ zhasni(); zapniZadne(); } } f_printf(&file, "idem do zakruty\n"); logujAktualnyStav(); /* zapamatam si cas kedy som spravil posledny krok, a vynulujem pre rovinku udaje, lebo teraz som v zakrute*/ jazdaRovinkou.poslednyKrok = 0; jazdaRovinkou.accX = 0; jazdaRovinkou.krok = 30; jazdaZakrutou.poslednyKrok = timeCounter; jazdaZakrutou.accX = daccX; } }else{ /* - ak som na rovinke tak si testujem ci ubehol dostatocny cas medzi regulacnymi krokmi - pri vchadzani zo zakruty na rovinku je poslednyKrok nulovy takze to vojde do tejto podmienky */ if (timeCounter - jazdaRovinkou.poslednyKrok > jazdaRovinkou.krok){ /* - ak som nedosiahol hranicne otacky a napatie na motore, tak mozem zrychlovat - hodnota hranicneho napatia urcena empiricky */ if (rychlost < jazdaRovinkou.maxOtacky && motorVoltage < 2000){ /* - zistujem ci sa nahodou neblizi zakruta a to tak, ze si pamatam poslednu hodnotu daccX a ak je nova hodnota vacsia, tak to znamena, ze vchadzam na zakrutu - ak je vsak stara hodnota vacsia, tak mozem kludne pridavat, som stale na rovine - jazdaRovinkou.accX je ulozena predosla hodnota daccX a ak chcem to porovnat s nulou, tak porovnam ci nie je mensie ako napr. 200, je to relativne */ if (jazdaRovinkou.accX < 200){ f_printf(&file, " zrychlujem\n"); logujAktualnyStav(); motorVoltage += jazdaRovinkou.zrychlenie; /* kedze som na rovine, resetnem krok pre zakrutu na nejaku hodnotu */ jazdaZakrutou.krok = 30; /* nastavim svetla */ zhasni(); zapniPredne(); }else{ /* ak som uz dlhsie na rovinke a zrychlenie daccX sa zvacsilo od poslednej hodnoty, takze sa asi zacina zakruta a mam vacsie otacky ako mam mat, tak spomalim */ if (jazdaRovinkou.accX < daccX && rychlost > jazdaRovinkou.maxOtacky){ f_printf(&file, " spomalujem\n"); logujAktualnyStav(); /* spomalujem len do desiatich otacok, aby nahodou auto v zakrute nezastalo */ if (motorVoltage > 1600){ motorVoltage -= 2*jazdaRovinkou.zrychlenie; jazdaRovinkou.krok -= 5; } /* nastavim svetla */ zhasni(); zapniPredne(); } } /* ulozim novu hodnoty daccX a pokracujem dalej */ jazdaRovinkou.accX = daccX; }else{ /* mam velke otacky alebo rychlost alebo oboje tak sa ide spomalovat */ f_printf(&file, " spomalujem\n"); logujAktualnyStav(); /* poistka, aby som auto nespomalil na rovine na nulu, empiricka hodnota */ if (motorVoltage > 1800) motorVoltage -= 2*jazdaRovinkou.zrychlenie; /* nastavenie svetiel */ zhasni(); zapniZadne(); } f_printf(&file, "%d idem po rovinke\n"); logujAktualnyStav(); /* som na rovine, tak vynulujem zakrutu a zapamatam si cas posledneho kroku */ jazdaZakrutou.poslednyKrok = 0; jazdaRovinkou.poslednyKrok = timeCounter; } } } /* Pocet prekrizeni zvysujem az po prekrizeni */ if (boloKrizenie == 1){ /* Zapisem si nove koliecko a vojdem do neho s vacsou rychlostou. Kazde neparne krizenie je nove koliecko. */ if (++pocetKrizeni%2 == 1){ f_printf(&file, "\n\n#Koliecko: %d\n", pocetKrizeni/2); // prepniSvetla(); } } boloKrizenie = 0; } } motorVoltage = 0; SET_MOTOR_VOLTAGE(motorVoltage); /* never gets here to close the file and dismount SD card - don't care, because: */ f_close(&file); f_mount(0, NULL); /* zastavim auto */ } /*****************************************************************************/