Hi,
I am using SimpleTimer libary to control speed of motors of a robot but i have the following problem:
When making Serialprintln to see the motor speed, i find some disturbing peaks in regular interval after investigating, i found that the sampling frequency is modified after a the same regular period, i did this by printing the time in millis in each sample.
When seeing the results, the period is of 20 ms but after many sample it becomes of 10 or 11 which generate an impulsion in speed of motor.
Does anyone dealed with this problem with simple timer?
Thank you.
#include "Arduino.h"
#include "AffectationPins.h"
#include "DeclarationsEntreesSorties.h"
#include <Servo.h>
#include "DeclarationsVariables.h"
#include <SimpleTimer.h>
SimpleTimer timer;
#include "GestionTemprisation.h"
#include "GestionUltrason.h"
#include "GestionDuMoteur.h"
#include "GestionEncodeurs.h"
#include "mesureCourant.h"
#include "Vl.h"
#include "ChercheUneIssue.h"
#include "AlgorithmeGlobal.h"
void setup()
{
Serial.begin(9600);
//analogReference(INTERNAL2V56);
declarationsEntreesSorties();
myservo.attach(5,635,2525);
attachInterrupt(digitalPinToInterrupt(EncodeurDroite1),pointsDroite, CHANGE);
attachInterrupt(digitalPinToInterrupt(EncodeurDroite2),pointsDroite, CHANGE);
attachInterrupt(digitalPinToInterrupt(EncodeurGauche1),pointsGauche, CHANGE);
attachInterrupt(digitalPinToInterrupt(EncodeurGauche2),pointsGauche, CHANGE);
timerDevant=timer.setInterval(1000/freq_ech, marcheDuMoteur2); // toutes les 20ms, on appelle la fonction
}
void loop()
{
timer.run();
delay(10);
}
void marcheDuMoteur2()
{
devantMoteurDroite();
}
void devantMoteurDroite()
{
//Réinitialisation du nombre de tick de la codeuse
tick_d=encodeurDroite;
Serial.print(millis()-anc_millis);
Serial.print(" ");
Serial.println(millis());
anc_millis=millis();
encodeurDroite=0;
//calcul de l'erreur
nb_tr_par_sec_d=(freq_ech*tick_d)/(cpr*reducteur);// calcul du nombre de tour par seconde du moteur
erreur_droite=consigne_moteur_nombre_tours_par_seconde-nb_tr_par_sec_d; //calcul de l'erreur avec la consigne
somme_erreur_droite=somme_erreur_droite+erreur_droite;
//Serial.println(tick_d);
//Serial.print(" ");
//Serial.println("1");
// P : calcul de la commande
cmd_d = kp_d*erreur_droite+ki_d*somme_erreur_droite;
//Normalisation et contrôle du moteur
if(cmd_d < 0) cmd_d=0;
else if(cmd_d > 255) cmd_d = 255;
analogWrite(enableDroite, cmd_d);
digitalWrite(inputDroite1,LOW);
digitalWrite(inputDroite2,HIGH);
}
Variables
//Encodeur droite
volatile byte encodeurDroite=0;
byte tick_d;
//Asservissemnt vitesse
const float cpr=48;
const float reducteur=75;
const int freq_ech=50;
const int consigne_moteur_nombre_tours_par_seconde=1;
//kp
float nb_tr_par_sec_d;
float erreur_droite;
const float kp_d=1200;
int cmd_d;
//ki
float somme_erreur_droite=0;
const float ki_d=0;//40;