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!
// 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--;
}
}
}