Pilotare un servomotore con un potenziometro

//*************************************************************************************************// // Titolo: Controllo e pilotaggio di un servocomando tramite potenziometro // // Autore: Marcello Pinna 19/01/2014 // // Hardware: Freedom I // pic 18F4550 // quarzo 20 MHz // servocomando Futaba S3003 // potenziometro da 4,7 K // // Note importanti: il servo deve essere collegato a PORTD.RD0 del pic 18F4550 ed alimentato con // una tensione continua di 6 volts. Il potenziometro deve essere collegato // all'ingresso AN1 del microcontrollore //*************************************************************************************************// #include <p18F4550.h> #define SERVO_ON LATDbits.LATD0 = 1 #define SERVO_OFF LATDbits.LATD0 = 0 #define FACTOR 5 // fattore di conversione conteggi - mS #pragma config FOSC = HS //OSC = HS Impostato per lavorare ad alta frequenza #pragma config WDT = OFF //WDT = OFF Disabilito il watchdog timer #pragma config LVP = OFF //LVP = OFF Disabilito programmazione LVP #pragma config PBADEN = OFF //PBADEN = OFF Disabilito gli ingressi analogici // ============================= VARIABILI DI CONTROLLO ================================== unsigned int timer0; // vedi timers.h unsigned int timer1; // vedi timers.h unsigned int sommatoria = 0; // Variabile per salvare la sommatoria dei dati letti unsigned int valore_ADC = 0; // Variabile per salvare il valore della conversione unsigned int pulse = 0; // Variabile per convertire il valore della conversione in impulso per il servo (ms) char i; // Variabile utilizzata per il numero delle letture // ============================= PROTOTIPI DI FUNZIONE =================================== void WriteTimer0 ( unsigned int timer0); // Prototipo di funzione estrapolato dal timers.h (MCC18\h) void WriteTimer1 ( unsigned int timer1); // Prototipo di funzione estrapolato dal timers.h (MCC18\h) // ======================== IMPOSTAZIONI TIMER0 E TIMER1 ================================= void set_timer(void) { // ---> TIMER0 // Imposto la modalità a 16 bit T0CONbits.T08BIT = 0; // Imposto il clock interno T0CONbits.T0CS = 0; // Disabilito Prescaler T0CONbits.PSA = 1; // Abilito le interruzioni del Timer0 INTCONbits.TMR0IE = 1; // Abilito le interruzioni del Timer0 ad alta priorità INTCON2bits.TMR0IP = 1; // Abilito il Timer0 T0CONbits.TMR0ON = 1; //---> TIMER1 // Imposto la modalità a 16 bit T1CONbits.RD16 = 1; // Abilito il clock interno Timer1 (Fosc/4) T1CONbits.TMR1CS = 0; // Imposto il PreScaler Timer1 1:2 T1CONbits.T1CKPS0 = 1; T1CONbits.T1CKPS1 = 0; // Disabilito l'oscillatore del Timer1 T1CONbits.T1OSCEN = 0; // Abilito il Timer1 T1CONbits.TMR1ON = 1; // Calcolo del valore iniziale del Timer1 // Voglio generare un impulso della durata totale di 20 ms; con il prescaler impostato 1:4 // il tempo di inremento è di 0.0004 ms. Il numero dei conteggi è dato da (20/0.0004 = 50000) // Sottraendo a 65536 il numero dei conteggi così ottenuti si ha: (65536-50000 = 15536) timer1 = 15536; // Imposto la funzione WriteTimer1 con questo valore WriteTimer1 (timer1); // Abilito l'interrupt globale INTCONbits.GIEH = 1; } // =========================== ROUTINE INTERRUPT ALTA PRIORITA' ========================= // Prototipo di funzione void Hight_Int_Event (void); #pragma code Hight_vector=0x08 void Hight_interrupt (void) { // Imposta il salto per la gestione dell'interrupt _asm GOTO Hight_Int_Event _endasm } #pragma code #pragma interrupt Hight_Int_Event void Hight_Int_Event (void) { if (INTCONbits.TMR0IF) { // Spengo il servo SERVO_OFF; // Disabilito il Timer0 T0CONbits.TMR0ON = 0; } // Appena scatta l'interrupt if (PIR1bits.TMR1IF) { // Ricarico il Timer1 WriteTimer1(15536); // Azzero il flag di interrupt del timer1 PIR1bits.TMR1IF = 0; // Avvio il servo SERVO_ON; // Converto i ms in numero di conteggi da levare a 65536 per ottenere il valore // iniziale del Timer0 timer0 = (unsigned int)(65536 - (pulse * FACTOR)); // Imposto la funzione WriteTimer0 con questo valore WriteTimer0 (timer0); // Abilito il Timer0 T0CONbits.TMR0ON = 1; // Pulisco il flag di interrupt INTCONbits.TMR0IF = 0; } } // ==================================== ROUTINE PRINCIPALE ============================== void main (void) { // Imposto TRISA tutti ingressi LATA = 0x00; TRISA = 0xFF; // Imposto TRISB tutti ingressi LATB = 0x00; TRISB = 0xFF; // Imposto PORTC tutti ingressi, RC1 come uscita (retroilluminazione del display) LATC = 0x00; TRISC = 0xFD; // Imposto PORTD tutte uscite (pilotaggio del display) LATD = 0x00; TRISD = 0x00; // Imposto TRISE tutti ingressi LATE = 0x00; TRISE = 0xFF; // Abilito AN0-AN1 come ingressi analogici // Le VREF sono impostate a massa e VCC ADCON1 = 0b00001101; // Seleziono AN1 come ingresso ADCON0 = 0b00000100; // Imposto i tempi di conversione e giustificazione a destra // TAD : FOSC/4 // TACQ: 16 TAD ADCON2 = 0b10110100; // Abilito l' ADC ADCON0bits.ADON = 0x01; // Avvio la routine di inizializzazione set_timer(); // Ciclo infinito while(1) { sommatoria = 0; // Effettuo 8 letture per fare una media for (i = 0; i < 8; i++) { // Avvio la conversione Analogico/Digitale ADCON0bits.GO = 1; // Attendo la fine della conversione while(ADCON0bits.GO); // Prelevo il valore della conversione valore_ADC = (((int) ADRESH) << 8) | ADRESL; // Sommatoria delle letture fatte sommatoria = sommatoria + valore_ADC; } // Per ottenere la media, invece di dividere per otto, faccio tre shift verso destra // ogni shift corrisponde rispettivamente a dividere per 2, per 4 e per 8 sommatoria = sommatoria >> 3; /* // Converto la media così ottenuta in un impulso, espresso in MICROSECONDI, utilizzando un fattore // di conversione ricavato dal rapporto: 2000/1023= 1.955, dove 2000 rappresenta il range di lavoro // dell'impulso applicato (2500-500 = 2000 uS) mentre 1023 rappresenta il range di lavoro (10 bit) // del convertitore ADC. A questo dato sommo il valore minimo dell'impulso, pari a 500 us. pulse = (sommatoria * 1.955) +500; */ if ( (sommatoria >= 562) && (sommatoria < 662) ) pulse = pulse + 1; if ( (sommatoria >= 662) && (sommatoria < 762) ) pulse = pulse + 3; if ( (sommatoria >= 762) && (sommatoria < 862) ) pulse = pulse + 5; if ( (sommatoria >= 862) && (sommatoria < 962) ) pulse = pulse + 7; if (sommatoria >= 962) pulse = pulse + 100; if (sommatoria < 462) pulse = pulse - 10; if (pulse > 2400) pulse = 2400; if (pulse <= 500) pulse = 500; } }