/* ////////////////////////////////////////////////////////////////////////////////////// ** File: quadrato.c ** Versione: 0.1 del 30 Agosto 2003 ** Autore: Guido Ottaviani-->g.ottaviani@mediaprogetti.it<-- ** Descrizione:Gestione Robot Dino, prova algoritmi di movimento ** il bot cammina lungo i lati di un quadrato di un metro, ** eseguendo delle svolte di 90° esatti, ** serve alla verifica della corrispondenza valori impostati-manovra eseguita ** ** LA DESCRIZIONE PROSEGUE IN FONDO AL FILE ///////////////////////////////////////////////////////////////////////////////////////*/ #include #include /*-------------------------------------------------------------------------------------- header necessario per la compatibilità con il bootloader di Rick Farmer */ #asm psect powerup, class=CODE, delta=2 global powerup, start powerup: org 0x3 goto start #endasm /* The BootLoader requires a GOTO at address 0x0003 rather than 0x0000 #pragma resetVector 0x0003 --------------------------------------------------------------------------------------*/ /* Definizioni globali++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ bit EyeR @ ((unsigned)&PORTA*8+2); // A2 IR detect destro I analog bit EyeL @ ((unsigned)&PORTA*8+3); // A3 IR detect sinistro I analog bit EncoderRdir@((unsigned)&PORTB*8+1); // B1 Encoder SX,direzione I bit EncoderLdir@((unsigned)&PORTB*8+2); // B2 Encoder DX,direzione I bit EncoderRpulse@((unsigned)&PORTB*8+4);// B4 Encoder SX,ticks I int on change bit EncoderLpulse@((unsigned)&PORTB*8+5);// B5 Encoder DX,ticks I int on change bit XAxis @ ((unsigned)&PORTB*8+6); // B6 Accellerometro asseX I int on change bit YAxis @ ((unsigned)&PORTB*8+7); // B7 Accellerometro asseY I int on change // C1 PWMmotR O PWM // C2 PWMmotL O PWM // C3 SCL // C4 SDA bit Buzzer @ ((unsigned)&PORTC*8+5); // C5 Buzzer O bit MotEnable @ ((unsigned)&PORTD*8+0); // D0 Enable H-bridge O bit BumperDx @ ((unsigned)&PORTD*8+2); // D2 Bumper destro I bit BumperSx @ ((unsigned)&PORTD*8+3); // D3 Bumper sinistro I bit LineR @ ((unsigned)&PORTE*8+0); // E0 LineFollower DX I analog bit LineC @ ((unsigned)&PORTE*8+1); // E1 LineFollower center I analog bit LineL @ ((unsigned)&PORTE*8+2); // E2 LineFollower SX I analog bit EncoderRprev; // stato precedente encoder bit EncoderLprev; // stato precedente encoder bit PidTick = 0; // timer routine PID, impostato dalla ISR bit Space2RunFlag = 0; // Abilita la routine di controllo dello spazio da percorrere bit FlagBumpers = 0; // Abilita la routine controllo collisioni bit PrevBumper = 0; // Memorizza su quale lato è avvenuta la collisione precedente bit FlagStop = 0; // Flag per stop motori bank1 unsigned long Tempo; // clock ticks per ora non usato // le variabili long seguenti contengono impulsi dell'encoder bank1 long EncoderRcount = 0; // impulsi encoder destro bank1 long EncoderLcount = 0; // impulsi encoder sinistro bank1 long EncoderRcountPrev =0; // misura precedente per il computo della velocità bank1 long EncoderLcountPrev =0; // spazio percorso = valore attuale - valore precedente bank1 long Space2RunR = 0; // spazio da percorrere bank1 long Space2RunL = 0; // bank1 long Space2RunRstart = 0; // valore di partenza per il calcolo della strada da percorrere bank1 long Space2RunLstart = 0; // bank1 int SpeedR = 0; // velocità attuale ruota destra bank1 int SpeedL = 0; // velocità attuale ruota sinistra bank1 int DesSpeedR = 0; // velocità desiderata ruota destra bank1 int DesSpeedL = 0; // velocità desiderata ruota sinistra bank1 int IntR = 0; // accumulatore errore integrale del PID (motore destro) bank1 int IntL = 0; // accumulatore errore integrale del PID (motore sinistro) bank1 int ErroreRprev = 0; // errore precedente, serve per il calcolo dell'errore differenziale del PID (motore destro) bank1 int ErroreLprev = 0; // errore precedente, serve per il calcolo dell'errore differenziale del PID (motore sinistro) bank1 int TimerStop; // timer fino a 30 Sec per routine Stop motori bank1 int TimerBumpers; // timer per routine Bumpers bank1 char DeltaT; // tempo trascorso, per il computo della velocità bank1 char DeadCorner; // conta le collisioni sullo stesso lato per uscire da un angolo morto char PathSeq [11]; // contiene la sequenza dei movimenti da fare (n-1 + EOL) char PathSeqPointer; // puntatore dell'array sequenza /* diametro ruota = 58mm -> circonferenza = 182,212mm l'encoder è su uno degli assi del gruppo riduttore e gira 20 volte più della ruota questo ha 40 finestrelle che danno due interrupt ognuna (fronte di salita e di discesa) quindi 40 x 2 x 20 = 1600 tick per giro -> risoluzione 0,1138827336926 mm */ const int stepR = 115; // in millesimi di millimetro const int stepL = 114; // i valori leggermente diversi correggono le piccole differenze tra le due ruote const int constSpeed = 180; // velocità normale, da verificare se il PID ha ancora sufficiente controllo const manSpeedFwd = 100; // velocità delle ruote durante le manovre const manSpeedRew = -100; // tenendola bassa la precisione è maggiore /* Fine definizioni+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* Prototipi funzioni *****************************************************************/ void settings(void); void MotSpeed (void); void Turn(long Gradi); void Walk(long Space); void Space2Run (void); void Stop(void); void Path (void); void Bumpers(void); void interrupt IntServiceRoutine (void); /* Fine prototipi funzioni ***********************************************************/ /* MAIN =============================================================================*/ main (void) { di(); settings(); // imposta porte e registri ei(); MotEnable=1; Buzzer=0; DesSpeedR = 0; // fermo DesSpeedL = 0; while (1) // main loop { if (PidTick) { MotSpeed(); //-------------------------------------------------- /* questa routine è eseguita solo su abilitazione della ISR, quest'ultima calcola la velocità ogni x mSec e solo dopo averla calcolata ne permette la correzione. Il calcolo PID è quindi eseguito più o meno con la stessa periodicità */ } if (Space2RunFlag) { Space2Run(); //------------------------------------------------ /* qualche routine ha abilitato il controllo dello spazio percorso, ha impostato la velocità (DesSpeedX), lo spazio da percorrere (Space2RunX) e la posizione di partenza (Space2RunXstart). Questa routine sarà eseguita continuamente fino al raggiungimento della posizione desiderata, dopodichè fermerà i motori e disabiliterà il flag */ } if (Space2RunFlag == 0 && PathSeq[PathSeqPointer] != 0 && FlagStop == 0) { Path(); //----------------------------------------------------- /* se Space2RunFlag = 1, è ancora in esecuzione una routine di movimento se PathSeq[PathSeqPointer] = 0 -> fine sequenza (EOL) se FlagStop = 1 -> la routine Stop ha fermato i motori La routine che deve usare Path -Imposta DesSpeedX = 0; // ferma motori -imposta PathSeq con passi da fare in ordine (fine seq = 0) -imposta PathSeqPointer = 0; // inizializza sequenza passi -imposta Space2RunFlag = 0; // reset di qualsiasi routine di movimento Se deve avanzare a velocità costante non c'è bisogno di chiamare Path(), si imposta semplicemente la velocità */ } Bumpers(); // controlla collisioni ------------------------------ if (FlagStop) // se qualcuno ha abilitato lo stop i motori si fermano per X mSec { Stop(); //---------------------------------------------------- } } } // main /*====================================================================================*/ /* Funzioni ***************************************************************************/ /*Turn ******************************************************************************** routine per girare a destra o a sinistra di un angolo desiderato la rotazione è intorno al centro del bot senza avanzamento quindi le ruote girano una in senso opposto all'altra, alla stessa velocità. Qui calcola e imposta i valori per girare dell'angolo desiderato. L'interasse del bot è di 140mm, la circonferenza lungo la quale girano le ruote è quindi di PI * 140 = 439,823mm. Per ruotare di 180° ogni ruota deve fare metà circonferenza: 219.911micron (millesimi di millimetro), dividendo per 180 sappiamo che un grado corrisponde a 1.222micron di spostamento di ogni ruota ((D * PI)/2/180). Si considera: 0° la direzione di avanzamento da +1 a +180° la rotazione in senso orario da -1 a -180° la rotazione in senso antiorario operazioni: imposta: Space2RunXstart calcola e imposta: Space2RunX imposta velocità: DesSpeedX imposta: Space2RunFlag a 1 */ void Turn(long Gradi) { long const unGrado = 1220; // arco di circonferenza corrispondente ad un grado, in micron const char corrGradi = 10; // è presente un errore sistematico assoluto che quindi influisce // in una percentuale maggiore sui valori di angolo piccoli Space2RunRstart = EncoderRcount; // punto di partenza Space2RunLstart = EncoderLcount; if (Gradi > 0) { Gradi = Gradi - corrGradi; DesSpeedR = manSpeedRew; // rotazione oraria, motore destro gira indietro DesSpeedL = manSpeedFwd; // ruota sinistra gira in avanti Space2RunR = unGrado * Gradi / stepR;// spazio (in impulsi encoder) da percorrere Space2RunL = unGrado * Gradi / stepL;// (micron/gradi)*(gradi)/(micron/impulsi)=impulsi } else { Gradi = Gradi + corrGradi; DesSpeedR = manSpeedFwd; // rotazione antioraria, motore destro gira avanti DesSpeedL = manSpeedRew; // ruota sinistra gira indietro Space2RunR = unGrado * Gradi / stepR; Space2RunL = unGrado * Gradi / stepL; } Space2RunFlag = 1; // inizia il controllo dello spazio da percorrere } // Turn /**************************************************************************************/ /*Walk ******************************************************************************** Calcola e inizializza le variabili per Space2Run, per camminare dello spazio desiderato operazioni: imposta: Space2RunXstart Space2RunX è impostata dalla routine chiamante imposta velocità: DesSpeedX imposta: Space2RunFlag a 1 */ void Walk(long Space) { Space2RunRstart = EncoderRcount; // punto di partenza Space2RunLstart = EncoderLcount; Space2RunR = Space / stepR; // da micron a conteggio encoder Space2RunL = Space / stepL; if (Space > 0) // avanti { DesSpeedR = manSpeedFwd; // durante le manovre va più piano DesSpeedL = manSpeedFwd; } else // indietro { DesSpeedR = manSpeedRew; // durante le manovre va più piano DesSpeedL = manSpeedRew; } Space2RunFlag = 1; // inizia il controllo dello spazio da percorrere } // Walk /**************************************************************************************/ /*Space2Run *************************************************************************** controlla continuamente lo spazio percorso per pilotare i motori fino al raggiungimento della posizione desiderata operazioni: controlla se spazio percorso >= spazio desiderato: if yes, ferma motore corrispondente se entrambi motori fermi: Space2RunFlag a 0 (si autodisabilita) */ void Space2Run (void) { /* qualche routine ha abilitato il controllo dello spazio percorso, ha impostato la velocità (DesSpeedX), lo spazio da percorrere (Space2RunX) e la posizione di partenza (Space2RunXstart). Questa routine sarà eseguita continuamente fino al raggiungimento della posizione desiderata, dopodichè fermerà i motori e disabiliterà il flag */ if (abs(EncoderRcount - Space2RunRstart) >= abs(Space2RunR)) { DesSpeedR = 0; } if (abs(EncoderLcount - Space2RunLstart) >= abs(Space2RunL)) { DesSpeedL = 0; } if ((DesSpeedR == 0) && (DesSpeedL == 0)) { // raggiunto il punto desiderato si ferma per 250 mSec TimerStop = 250; // carica il contatore FlagStop = 1; // abilita routine Stop Space2RunFlag = 0; // si autodisabilita } } // Space2Run /**************************************************************************************/ /*Stop ******************************************************************************** Ferma i motori per X millisecondi (fino a 30 Sec) */ void Stop(void) { DesSpeedR = 0; // Motori fermi DesSpeedL = 0; if (TimerStop <= 0) // il clock decrementa il contatore, quindi il valore caricato // inizialmente in TimerStop è = alla pausa in mSec { FlagStop = 0; // disabilita la routine Stop, riabilita la routine Path } } // Stop /**************************************************************************************/ /*Path ******************************************************************************** viene chiamata solo se (Space2RunFlag==0 & PathSeq[PathSeqPointer] != 0) se Space2RunFlag = 1, sono ancora in esecuzione routine movimento e quindi non può passare allo step successivo se PathSeq[PathSeqPointer]= 0 la sequenza è terminata e rilascia il controllo routine che deve usare Path -Imposta DesSpeedX = 0; // ferma motori -imposta PathSeq con passi da fare in ordine (fine seq = 0) -imposta PathSeqPointer = 0; // inizializza sequenza passi -imposta Space2RunFlag = 0; // reset di qualsiasi routine di movimento Se deve avanzare a velocità costante si imposta semplicemente la velocità */ void Path (void) { switch (PathSeq[PathSeqPointer]) { case 1: Walk(1000000); // 1.000.000 micron = 1 metro avanti break; case 2: Walk(-150000); // -150.000 micron = 15 cm indietro break; case 10: Turn(30); // gira n gradi a Dx break; case 11: Turn(-30);// gira n gradi a Sx break; case 12: Turn(45); break; case 13: Turn(-45); break; case 14: Turn(90); break; case 15: Turn(-90); break; case 16: Turn(135); break; case 17: Turn(-135); break; case 18: Turn(180); break; case 20: TimerStop = 10000; // pausa per 10 Sec (solo per demo quadrato) FlagStop = 1; // abilita routine Stop motori break; default: DesSpeedR=constSpeed; // a default cammina a velocità costante DesSpeedL=constSpeed; // a default cammina a velocità costante break; } PathSeqPointer++; // predispone al prossimo passo } // Path /**************************************************************************************/ /*Bumpers **************************************************************************** Controllo dei sensori di collisione */ void Bumpers(void) { const char maxDeadCorner = 5; // limite di urti alternativi if (FlagBumpers) // routine disattiva { /* disabilita i sensori per 1 Sec (debouncer), il timer è incrementato dall'interrupt ogni mSec */ if (TimerBumpers >= 1000) { FlagBumpers=0; // riabilita la routine } } else { if (BumperDx == 0) // controlla bumper destro { if (PrevBumper) // l'urto precedente era stato sul bumper sinistro ? { DeadCorner++; // se entra in un angolo morto gli urti si susseguono // alternativamente sui due bumpers } else { DeadCorner = 0; // reset della situazione di allarme } PrevBumper = 0; // Destro = 0, Sinistro = 1 FlagBumpers = 1; // disattiva bumpers TimerBumpers = 0; // per 1 Sec Space2RunFlag = 0; // reset di qualsiasi routine di movimento if (DeadCorner <= maxDeadCorner) // dopo n volte cambia drasticamente percorso { // cammina sui lati di un quadrato di 1M girando a DX usato per la calibrazione PathSeq [0] = 20; // fermo 10 Sec PathSeq [1] = 1; // avanti 1M PathSeq [2] = 14; // gira 90°a DX PathSeq [3] = 1; // avanti 1M PathSeq [4] = 14; // gira 90°a DX PathSeq [5] = 1; // avanti 1M PathSeq [6] = 14; // gira 90°a DX PathSeq [7] = 1; // avanti 1M PathSeq [8] = 14; // gira 90°a DX PathSeq [9] = 0; // fine sequenza PathSeqPointer = 0; // inizializza sequenza } else { // inverte la marcia PathSeq [0] = 2; // indietro 15 cm PathSeq [1] = 18; // gira 180° PathSeq [2] = 255; // cammina avanti a velocità costante PathSeq [3] = 0; // fine sequenza PathSeqPointer = 0;// inizializza sequenza DeadCorner = 0; // ricomincia il giro } } if (BumperSx == 0 && !FlagBumpers) // bumper sinistro ma non bumper destro { if (!PrevBumper) // l'urto precedente era stato sul bumper destro ? { DeadCorner++; // se entra in un angolo morto gli urti si susseguono // alternativamente sui due bumpers } else { DeadCorner = 0; // reset della situazione di allarme } PrevBumper = 1; // Destro = 0, Sinistro = 1 FlagBumpers = 1; // disattiva bumpers TimerBumpers = 0; // per 1Sec Space2RunFlag = 0; // reset di qualsiasi routine di movimento if (DeadCorner <= maxDeadCorner) // dopo n volte cambia drasticamente percorso { // cammina sui lati di un quadrato di 1M girando a SX, usato per la calibrazione PathSeq [0] = 20; // fermo 10 Sec PathSeq [1] = 1; // avanti 1M PathSeq [2] = 15; // gira 90°a SX PathSeq [3] = 1; // avanti 1M PathSeq [4] = 15; // gira 90°a SX PathSeq [5] = 1; // avanti 1M PathSeq [6] = 15; // gira 90°a SX PathSeq [7] = 1; // avanti 1M PathSeq [8] = 15; // gira 90°a SX PathSeq [9] = 0; // fine sequenza PathSeqPointer = 0; // inizializza sequenza } else { // inverte la marcia PathSeq [0] = 2; // indietro 15 cm PathSeq [1] = 18; // gira 180° PathSeq [2] = 255; // cammina avanti a velocità costante PathSeq [3] = 0; // fine sequenza PathSeqPointer = 0;// inizializza sequenza DeadCorner = 0; // ricomincia il giro } } } } // Bumpers /**************************************************************************************/ /*MotSpeed **************************************************************************** Controllo velocità motori PID (Proportional + Integral + Derivative) il ponte ad H è usato in modalità "Locked Anti Phase" (LAP): PWM = 128 -> motore fermo PWM = 255 -> velocità massima FWD (circa 200 mm/sec) PWM = 0 -> velocità massima REW (circa -200 mm/sec) CCPR2L collegato al motore R CCPR1L collegato al motore L */ void MotSpeed (void) { /* const divisore =((200-0)/(255-128)); rapporto tra range velocità e range PWM = 1,57 viene moltiplicato per 100, il risultato finale è poi diviso per 100, per mantenere la precisone di due decimali nei calcoli intermedi senza usare la virgola mobile */ const divisore = 157; const biasFwd = 128; // (MinPWMFwd-(MinSpeed/divisore)), in pratica sono lo 0 del PWM FWD e REW, in questo caso const biasRew = 128; // (MinPWMRew+(Minspeed/divisore)), non c'è dead-band, la regolazione è continua da 0 a 255 const intUpperLimit = 2000; //limite superiore della correzione integrale const intLowerLimit = -2000; //limite inferiore della correzione integrale const devUpperLimit = 2000; //limite superiore della correzione derivativa const devLowerLimit = -2000; //limite inferiore della correzione derivativa const speedUpperLimit = 200; //limite superiore velocità const speedLowerLimit = -200;//limite inferiore velocità const kp = 5; // costante errore proporzionale (fattore P) moltiplicato 10 const ki = 5; // costante errore integrale (fattore I) moltiplicato 10 const kd = 5; // costante errore derivativo (fattore D) moltiplicato 10 int PWMR; // risultato del calcolo del PID, ancora è in mm/sec int PWML; int ErroreR = 0; //errore attuale int ErroreL = 0; int DevR = 0; //componente derivativa attuale int DevL = 0; //componente derivativa attuale /* questa routine è eseguita solo su abilitazione della ISR, quest'ultima calcola la velocità ogni x mSec e solo dopo averla calcolata ne permette la correzione il calcolo PID è quindi eseguito più o meno con la stessa periodicità */ PidTick = 0; // resetta il flag per il prossimo giro ErroreR = (DesSpeedR - SpeedR); //errore attuale = velocità desiderata - velocità attuale ErroreL = (DesSpeedL - SpeedL); // /* Fattore I esegue la sommatoria algebrica dell'errore, cioè lo integra. L'integrale dell'errore di velocità è lo spazio perso, o guadagnato, rispetto a quello voluto. In questo modo anche se una ruota rallenta a causa di un ostacolo perdendo terreno rispetto all'altra questo viene riguadagnato (entro certi limiti) */ IntR =IntR + (ErroreR * ki); if (IntR > intUpperLimit) // la quantità assoluta di correzione è limitata per non saturare i contatori { IntR = intUpperLimit; } if (IntR < intLowerLimit) { IntR = intLowerLimit; } IntL = IntL + (ErroreL * ki); if (IntL > intUpperLimit) { IntL = intUpperLimit; } if (IntL < intLowerLimit) { IntL = intLowerLimit; } /* Fattore D calcola la variazione dell'errore rispetto alla misura precedente, essendo l'intervallo di tempo di misura costante, corrisponde con la derivata dell'errore di velocità e quindi con l'accellerazione */ DevR = kd * (ErroreR - ErroreRprev); ErroreRprev = ErroreR; if (DevR > devUpperLimit) // la quantità assoluta di correzione è limitata per non saturare i contatori { DevR = devUpperLimit; } if (DevR < devLowerLimit) { DevR = devLowerLimit; } DevL = kd * (ErroreL - ErroreLprev); ErroreLprev = ErroreL; if (DevL > devUpperLimit) { DevL = devUpperLimit; } if (DevL < devLowerLimit) { DevL = devLowerLimit; } /* Il risultato è diviso 10 per sfruttare il decimale senza usare variabili float, le costanti erano infatti moltiplicate per 10, in questo modo si usa la precisione del decimale nei calcoli intermedi pur avendo un intero come risultato finale */ PWMR = (ErroreR * kp + IntR + DevR) / 10; // P + I + D PWML = (ErroreL * kp + IntL + DevL) / 10; // /* converte la velocità da mm/sec (signed int moltiplicato 100) a valore da impostare nei registri CCPRxL (char) PWM da -200 a 200 -> CCPRxL da 0 a 255 imposta solo gli 8 bit alti del PWM (CCPRxL), i due bassi sono fissi a zero */ if (PWMR > 0) // Motore Destro in avanti { if (PWMR > speedUpperLimit) // anche qui si limita per non sforare le variabili { PWMR = speedUpperLimit; } CCPR2L = (char)(PWMR * 100 / divisore) + biasFwd; } else // indietro { if (PWMR < speedLowerLimit) { PWMR = speedLowerLimit; } CCPR2L = (char)biasRew + (PWMR * 100 / divisore); } if (PWML > 0) // Motore Sinistro in avanti { if (PWML > speedUpperLimit) { PWML = speedUpperLimit; } CCPR1L = (char)(PWML * 100 / divisore) + biasFwd; } else // indietro { if (PWML < speedLowerLimit) { PWML = speedLowerLimit; } CCPR1L = (char) biasRew + ((PWML * 100 / divisore)); } } // MotSpeed /**************************************************************************************/ /*IntServiceRoutine********************************************************************/ void interrupt IntServiceRoutine (void) { // Analizza per primo il TMR0, per mantenere il più costante possibile il clock if (T0IF) // l'interrupt è stato generato dall'overflow del timer 0 ? { Tempo ++; // è passato un'altro milliSecondo per ora non usato #asm // fine tuning nop #endasm TMR0 -= 156; TimerBumpers ++; // timer fino a 255 mSec per routine bumpers TimerStop --; // timer fino a 30 Sec per routine Stop Motori /*interrupt ogni 1mSec periodo = CLKOUT * prescaler * TMR0 1mSec ~= 1/(20.000.000/4) * 32 * 156 */ if (DeltaT >= 50) //ogni 50 mSec campiona la velocità { // velocità = spazio/Tempo. Dividendo millesimi di mm con millisecondi il risultato è in mm/Sec SpeedR = ((EncoderRcount - EncoderRcountPrev) * stepR) / DeltaT; EncoderRcountPrev = EncoderRcount; // reset contatore SpeedL = ((EncoderLcount - EncoderLcountPrev) * stepL) / DeltaT; // ripete per l'altra ruota EncoderLcountPrev = EncoderLcount; DeltaT=0; PidTick = 1; // eseguito il calcolo della velocità può essere eseguito il calcolo del PID } else { DeltaT++; } T0IF = 0; // reset flag di interrupt } if (RBIF) //cambiamento di stato di una porta B da 4 a 7 { if (EncoderRpulse != EncoderRprev) // impulso dall'encoder? (sia "rising edge" che "falling edge") { /* encoder in quadratura, se al cambiamento di stato i due segnali sono in fase la direzione è = FWD, altrimenti è REW */ if (EncoderRpulse == EncoderRdir) { EncoderRcount ++; // allora incrementa contatore } else { EncoderRcount --; // altrimenti decrementa contatore } EncoderRprev=EncoderRpulse; // memorizza stato attuale } if (EncoderLpulse != EncoderLprev) // ripete per altro encoder { if (EncoderLpulse == EncoderLdir) { EncoderLcount ++; // allora incrementa contatore } else { EncoderLcount --; // altrimenti decrementa contatore } EncoderLprev=EncoderLpulse; } if (XAxis) //accellerometro, per ora don't care { } if (YAxis) { } RBIF = 0; //reset del flag di interrupt } } // IntServiceRoutine /**************************************************************************************/ /*Settings***************************************************************************** setting di porte e registri */ void settings(void) { TRISA = 0b11111111; // tutti input (anche quelli non usati) ADCON1 = 0b10000000; //tutte le A e le E sono porte analogiche TRISB = 0b11111111; // tutti input OPTION = 0b01000100; /* pull-ups enabled don't care Timer on internal clock don't care prescaler assigned to TIMER0 prescaler 1:32 */ TRISC = 0b10011001; /* RX TX Buzzer SDA SCL PWMR PWML don't care */ CCP1CON = 0x3c;// CCP1 in PWM mode CCP2CON = 0x3c;// CCP2 in PWM mode T2CON = 0X06; // prescaler = 16 PR2 = 0xFF; // PWM freq = 1,22KHz /* SSPSTAT SSPCON i2c */ /* USART già settato da boot loader */ TRISD = 0b11111110; //solo bit 0 come output TRISE = 0b00000111; /* don't care don't care don't care PORTD in I/O mode don't care all PORTE as input */ TMR0 -= 156; /*interrupt ogni 1mSec periodo = CLKOUT * prescaler * TMR0 1mSec =~ 1/(20.000.000/4) * 32 * 156 */ INTCON = 0b10101000; /* 7 Global interrupt enabled 6 Peripheral interrupt disabled 5 TMR0 int enabled 4 RB0/INT interrupt disabled 3 RB int on change enabled 2 1 0 */ } // settings /**************************************************************************************/ /* Fine funzioni **********************************************************************/ /* //////////////////////////////////////////////////////////////////////////////////// /* Continua descrizione: ** ** Processore PIC 16F877, controller board + sensor board Mark III ** ** è previsto l'utilizzo dei seguenti sensori: ** 2 sensori di prossimità IR sharp GP2Y0A02YK 2 I (EyeR, EyeL) A2-A3 ** 3 IR photoreflector Fairchild QRB1134 tipo 3 I (LineR, C, L) E0-E1-E2 ** 2 sensori d'urto a microswitch (bumper) 2 I (BumperR, L) D2-D3 ** 2 shaft encoder in quadratura 4 I (EncoderRdir,L) B1-B2 ** (EncoderRpulse,L) B3-B4 ** Accellerometro 2 I (XAxis, YAxis) B6-B7 ** ?? Sensore di luce 1 I (????????) ?? ** ?? Sensore di suono 1 I (????????) ?? ** ?? Sensore di gas 1 I (????????) ?? ** ** e dei seguenti attuatori: ** H-bridge motor driver 3 O (PWMmotR, L) C1-C2 ** (MotEnable) D0 ** Buzzer 1 O C5 ** delle porte Rx-Tx 2 I/O ** ** oltre ai seguenti dispositivi I2c (2 I/O SCL-SDA) ** Ultrasonic range finder Devantech SRF08 Adr = ** 256Kbit I2C EEprom Adr = ** I2C 8bit I/O expander PCF8574 Adr = ** Analog input expander Adr = ** Digital compass Devantech CMPS03 Adr = ** ** ** A0 ------------------ ** A1 ------------------ ** A2 EyeR I analog ** A3 EyeL I analog ** A4 ------------------ ** A5 ------------------ ** B0 ------------------ ** B1 EncoderRdir I ** B2 EncoderLdir I ** B3 ------------------ ** B4 EncoderRpulse I ** B5 EncoderLpulse I ** B6 XAxis I ** B7 YAxis I ** C0 ------------------ ** C1 PWMmotR O PWM ** C2 PWMmotL O PWM ** C3 SCL I ** C4 SDA I ** C5 Buzzer O ** C6 TX O ** C7 RX I ** D0 MotEnable O ** D1 ------------------ ** D2 BumperR I ** D3 BumperL I ** D4 ------------------ ** D5 ------------------ ** D6 ------------------ ** D7 ------------------ ** E0 LineR I analog ** E1 LineC I analog ** E2 LineL I analog ---------------------------------------------------------------------------------------- La configurazione preimpostata dal PIC Loader è la seguente: Brown-out reset ON Code protection OFF Flash WRT Enable ON Power UP timer ON Watch dog timer OFF Oscillator HS Debug OFF EE Memory code prot OFF Low voltage prog OFF PORTC,TX impegnati PORTC,RX 8 bit asynch mode TMR0 Prescaler 256,internal clock, 13,56 ticks/second ---------------------------------------------------------------------------------------- Descrizione delle metodologie generali di programmazione Il programma lavora in una versione semplificata di "Real Time Operating System", sono state seguite quindi le seguenti regole per lo sviluppo di un "Sistema Operativo Multitasking Cooperativo": -Ogni routine collabora con tutte le altre, ed in particolare con il main, per il buon funzionamento del sistema. -Ogni routine occupa il sistema per il minor tempo possibile -Non ci sono loop lunghi o ritardi a SW -Il main si preoccupa di schedulare i vari task, le routine si chiamano raramente tra loro -Lo scambio di informazioni tra task avviene tramite flag e variabili globali -Le temporizzazioni sono realizzate usando come "Real Time Clock" l'interrupt overflow del TIMER0 che è il vero "hearth-beat" del sistema -L'esecuzione dei vari task è condizionata dai relativi semafori, il main o un altro task possono disabilitare una certa funzione ---------------------------------------------------------------------------------------- Descrizione delle procedure di movimento: Il cuore di tutto è il timer (TMR0) che ogni 1mSec genera un interrupt. Nella ISR vengono incrementati i timer (a 8, 16, 24 bit secondo la durata necessaria) che servono per le temporizzazioni delle varie routine. Anche il computo dello spazio percorso tramite encoder ottici in quadratura, funziona ad interrupt,ognuno dei due encoder genera il proprio interrupt che, analizzato e confrontato con il flag di direzione, incrementa o decrementa le variabili di conteggio dello spazio. Nella stessa ISR viene calcolata, ogni 50mSec, la velocità di avanzamento attuale, dividendo lo spazio percorso per il tempo. Il main è semplicemente uno scheduler che lancia le diverse routine con le giuste priorità. Una di queste è la MotSpeed che lavora in background e, ogni 50 mSec (dopo la misura della velocità), imposta il PWM (calcolato tramite un algoritmo PID) per fare in modo che la velocità attuale corrisponda con quella desiderata, . A questo punto abbiamo un sistema a loop chiuso nel quale sappiamo la velocità, lo spazio percorso ed il tempo trascorso. Anche la routine Path lavora in background e, se abilitata, controlla la sequenza di operazioni da effetuare per seguire il percorso impostato da qualche altra routine che decide il comportamento del bot. La routine che decida di eseguire un percorso diverso dal normale, deve: - impostare il flag di attivazione della routine Path - scrivere nell'array PathSeq la sequenza di codici della manovra da effettuare (0=fine sequenza) - azzerare il puntatore della sequenza Path comincerà allora a scandire l'array chiamando le routine in funzione del codice impostato (cammina X cm, ruota Y gradi, cammina W mm/sec). La routine chiamata (Walk, Turn) calcola lo spazio che deve percorrere ogni ruota prima di ripassare il comando a Path. A questo punto si è attivata Space2Run (schedulata da main) che si disattiva, ripassando il controllo a Path, solo quando le ruote si sono mosse dello spazio desiderato. Path controlla la sequenza successiva ripetendo le operazioni o restituendo il controllo quando il codice è = 0. Il Main è sempre attivo e, visto che le operazioni di movimento sono molto lente rispetto ai tempi di esecuzione del PIC, per la maggior parte del tempo non farà niente. L'evento che occupa di più il PIC è l'interrupt degli encoder: alla massima velocità ci sono circa 1600 impulsi per ogni encoder ogni secondo, nella condizione peggiore quindi 1 / 3200 = 1 impulso ogni 312,5 uSec che corrispondono comunque a 1562 istruzioni. ////////////////////////////////////////////////////////////////////////////////////// */