Go Down

Topic: encoder rotativo recuperato da mouse (Read 3 times) previous topic - next topic

niki77

#5
May 09, 2012, 10:29 am Last Edit: May 09, 2012, 10:31 am by niki77 Reason: 1
E' allora daje, che in qualche modo mi aiuti a farlo funzionare !

Breve introduzione, è uno sketch per controllare la velocità di un motore attraverso la lettura dell'encoder.
Sinceramente in questo momento non me ne frega niente che riesca a mantenere il motore alla velocità desiderata , mi accontenterei di vedere che riesce a leggere la velocità corrente anche a regimi di rotazione molto elevati.
Per scrivere questo codice mi sono basato su un codice trovato sulla rete non ricordo esattamente dove!

Code: [Select]



// controllo rotazione motore tramite lettura di un encoder rotativo ottico a 2 canali

#define motorCW             10                  // CW motor controller (basato su L298)
#define motorCCW            11                  // CCW motor controller (basato su L298)
#define motorPwm            6                   // PWM motor  controller (basato su L298)
#define encodPinA1          3                   // encoder A pin
#define encodPinB1          8                   // encoder B pin
#define sampleIntervall     100                 // intervallo di tempo per il calcolo della velocità

unsigned long lastMilli = 0;                    // loop timing
unsigned long lastMilliPrint = 0;               // loop timing
int targetSpeed = 300;                          // velocità desiderata
int currentSpeed = 0;                           // velocità attuale
int pwmValue = 0;                               // valore del controllo pwm per il motore
volatile long pulse = 0;                        // accumulo impulsi letti da encoder
float Kp =   .4;                                // PID proportional control Gain  (non è farina del mio sacco,serve per calcolare l'incremmento del pwm)
float Kd =    1;                                // PID Derivitave control gain (idem come sopra)


void setup()
{
 Serial.begin(115600);
 pinMode(motorCW, OUTPUT);
 pinMode(motorCCW, OUTPUT);
 pinMode(motorPwm, OUTPUT);
 pinMode(encodPinA1, INPUT);
 pinMode(encodPinB1, INPUT);
 attachInterrupt(1, interruptEncoder, CHANGE);
 analogWrite(motorPwm, pwmValue);
 digitalWrite(motorCW, HIGH);
 digitalWrite(motorCCW, LOW);
}

void loop()
{
 if((millis()-lastMilli) >= sampleIntervall)  
 {                          
   lastMilli = millis();
   getMotorData();                                                          
   pwmValue= updatePid(pwmValue, targetSpeed, currentSpeed);                    
   analogWrite(motorPwm, pwmValue);                                          
 }
 sendDataToSerial();                                                          
}

void getMotorData()  
{                                                        
 static long pulseAnt = 0;                                                  
 currentSpeed = ((pulse - pulseAnt)*(60*(1000/sampleIntervall))) / 44;          // 44 implusi a rivoluzione letti solo sul fronte di salita
 pulseAnt = pulse;                                                  
}

// adegua il pwm del motore secondo uno specifico algoritmo (non è farina del mio sacco !)
int updatePid(int command, int targetValue, int currentValue)  
{  
 float pidTerm = 0;                                                          
 int error=0;                                  
 static int last_error=0;                            
 error = abs(targetValue) - abs(currentValue);
 pidTerm = (Kp * error) + (Kd * (error - last_error));                            
 last_error = error;
 return constrain(command + int(pidTerm), 0, 255);
}

// invia alla seriale i dati del motore ogni mezzo secondo
void sendDataToSerial()  
{                                                    
 if((millis()-lastMilliPrint) >= 500)  
 {                    
   lastMilliPrint = millis();
   Serial.print("SP:");            
   Serial.print(targetSpeed);  
   Serial.print("  RPM:");          
   Serial.print(currentSpeed);
   Serial.print("  PWM:");          
   Serial.print(pwmValue);              
 }
}

// routin di gestione interrupt su cambio stato del pin collegato al canale A dell'encoder
// al momento leggo solo i fronti di salita
void interruptEncoder()  
{                    
 // compara i valori rilevati dall'encoder , se sono entrambi a stato logico alto è un modivmento CW altrimenti CCW
 // potrebbe essere più efficiente non usare digitalRead ma comparare direttamente il registro della porta per capire la direzione  
 if(digitalRead(encodPinA1)==1)
 {
   if(digitalRead(encodPinA1)==digitalRead(encodPinB1))  
   {
     pulse ++;
   }
   else      
   {  
     pulse--;
   }
 }
}


Vi è una spiegazione scientifica a tutto.
La fede è solo quell'anello che si porta al dito dopo il matrimonio.

Janos

Esattamente, devi lavorare senza la digitalRead, è troppo lenta per un gestore di interrupt... Devi andare a vedere quei pin su quale porte del micro solo collegati e lavorare con quelle.

Leggiti sul datasheet cosa fanno i registri PORTx, PINx e DDRx. x sta per la porta in questione: i pin sono raggruppati in gruppi da 8 chiamati porte e ogniuna di queste porte hanno 3 registri per essere pilotati. Ad esempio per il primo gruppo avrai i registri PORTA, PINA e DDRA.

lesto

imagni che interrupt encoder sia attivato su uno dei due pin. Quindi, sapendo nella fase di setup se il pin è alto o basso, a questo punto puoi evitare una digitalRead sapendo che lo stato si inverte ogni interrupt.

la mia opinione è che il segnale alto/basso dall'encoder dura talmente poco da essere più veloce delle digitalRead. Una soluzione può essere quella di usare 2 interrupt, uno per pin, e poi dal loop verificare se sono avvenuti entrambi (se il loop è veloce), altrimenti c'è da rivedere il codice...

per test dovresti contare il numero di interrupt però solo su un pin alla volta, e darci così le frequenze (e magari pure le durate del segnale alto/basso) per capire da dove arriva sto casino

edit: ovvio che per contare gli interrupt niente digitalread! un semplice count++ basta e avanza
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

niki77


... i registri PORTx, PINx e DDRx. x sta per la porta in questione: i pin sono raggruppati in gruppi da 8 chiamati porte e ogniuna di queste porte hanno 3 registri per essere pilotati. Ad esempio per il primo gruppo avrai i registri PORTA, PINA e DDRA.


Si sapevo dell'esistenza e della possibilità di accedere direttamente ai registri ma non pensavo che digitalRead fosse stato veramente così lento (ammesso che dipenda da quello)

Potrei anche collegare l'interrupt invece che al CHANGE, al RISING (salita) e quindi andare come dice lesto a controllare solo il valore dell'altro canale per capire la direzione, che ne dite?

L'idea di collegare interrupt ad entrambi i canali non mi sembra delle migliori... però non si sà mai, ora vediamo.

Nessuno di voi pensa  che con questo harware (encoder del mouse a rotellina a fotodiodi) ci possano essere problemi fisico meccanici del sistema ad impedire letture di rotazioni molto elevate? (es. 2000 rpm)

Vi è una spiegazione scientifica a tutto.
La fede è solo quell'anello che si porta al dito dopo il matrimonio.

lesto

allora, i tempi di risposta dei fotodiodi (quindi anche dei led) sono nell'ordine dei nanosecondi, quindi più veloci fino a 1000 volte di un arduino :)
magari dopo c'è un minimo di circuiteria per incrementare la durata del segnale alto, ma dubito che superi i 100microsecondi

Code: [Select]
Si sapevo dell'esistenza e della possibilità di accedere direttamente ai registri ma non pensavo che digitalRead fosse stato veramente così lento (ammesso che dipenda da quello)

se non erro una digitalread dura un centinaio di microsecondi, che scendono a 10/20 se usi i registri

fai questo conteggio di interrupt rising, prima su un pin e poi sull'alto. In base a questo capiamo dovè il problema
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Go Up