//B. Vandeportaele 09/2020 //done: gestion du temps pour que l'asservissement ait des paramètres fixes/ contenu du programme #include //Servo myservo; #include //adresses codées sur 7bits #define SLAVE_ADDR_8574_A 0x38+6 #define SLAVE_ADDR_8574_B 0x38+7 //AF_DCMotor motor(3,MOTOR34_1KHZ);//, MOTOR12_8KHZ); AF_DCMotor motor(1,MOTOR12_1KHZ);//, MOTOR12_8KHZ); int led = 13; //numéro de la broche Arduino pilotant la LED sur le shield PERIPH ////////////////////////////////////////// char readPort8574(char addr, char * ptr_value) /*addr, l'adresse du PCF8574 ptr_value, pointeur pour renvoyer la valeur lue sur le port retourne -1 si échec 0 sinon*/ { Wire.requestFrom((byte)addr, (byte)1);// demande la lecture d'1 octet depuis l'adresse du pérpiphérique if (Wire.available()==1) //si l'octet est disponible { (* ptr_value) = Wire.read(); // lire l'octet return 0; } else { (* ptr_value) =0; //valeur par défaut si le composant n'a pas acquité return -1; } } ////////////////////////////////////////// char writePort8574(char addr, char value) /*addr, l'adresse du PCF8574 value, la valeur à écrire sur le port retourne -1 si échec 0 sinon */ { Wire.beginTransmission(addr);//démarre la transmission avec l'adresse du pérpiphérique Wire.write((byte)value); //envoie la donnée if (Wire.endTransmission()==0) //stoppe la transmission return 0; else return -1; } ////////////////////////////////////////// #define TAB_SIZE 10 int16_t erreur_tab[TAB_SIZE]; uint8_t ind_ecr_erreur_tab=0; int16_t integrale_erreur=0; int16_t derivee_erreur=0; void update_tab_erreur(int16_t new_erreur) { //calcul intégrale integrale_erreur=integrale_erreur-erreur_tab[ind_ecr_erreur_tab]; integrale_erreur=integrale_erreur+new_erreur; //calcul dérivée uint8_t ind_lec_erreur_tab=ind_ecr_erreur_tab+5; if (ind_lec_erreur_tab>=TAB_SIZE ) //gestion modulo ind_lec_erreur_tab-=TAB_SIZE; derivee_erreur=new_erreur-erreur_tab[ind_lec_erreur_tab]; //maj tableau erreur_tab[ind_ecr_erreur_tab]=new_erreur; ind_ecr_erreur_tab++; if (ind_ecr_erreur_tab>=TAB_SIZE) ind_ecr_erreur_tab=0; } ////////////////////////////////////////// void setup() { // myservo.attach(9); // initialize the digital pin as an output. pinMode(led, OUTPUT); Serial.begin(115200); // start serial port at 9600 bps: // Serial.print("Bonjour"); Wire.begin(); // joindre le bus i2c en tant que maître writePort8574( SLAVE_ADDR_8574_B , 0xff); //configure le composant B en entrée writePort8574(SLAVE_ADDR_8574_A,~0); //eteind les leds for (int i=0;i=time_next_it) { time_next_it=time_next_it+cadence_it; controleur() ; //test si la cadence peut etre tenue if (millis()>=time_next_it) writePort8574(SLAVE_ADDR_8574_A,~0xFF); } } void controleur() { /* digitalWrite(led, HIGH); // turn the LED off by making the voltage LOW delay(1000); // wait for a second digitalWrite(led, LOW); // turn the LED off by making the voltage LOW delay(1000); */ //Serial.write('X'); // delay(100); //motor.setSpeed(255); /*delay(1000); motor.run(BACKWARD); delay(1000); motor.run(RELEASE); delay(1000); */ static uint8_t vitesse=100; static double t=0; t=t+0.01; //vitesse+=155+(100*sin(t)); //Serial.println(vitesse); vitesse=vitesse+1; if (vitesse==255) vitesse=100; byte port_in; readPort8574(SLAVE_ADDR_8574_B,(char*)&port_in); //writePort8574(SLAVE_ADDR_8574_A,~port_in); int16_t sens; uint16_t consigne_position; if ((port_in&1)!=0) { //lecture consigne sur potentiomètre consigne_position= analogRead(A0); }else{ //génération échelons périodiques unsigned long time; time = millis(); if(((time/1000)%2)==0) consigne_position=512-250; else consigne_position=512+250; } uint16_t position= analogRead(A1); int16_t erreur=position-consigne_position; update_tab_erreur(erreur); /* //type commande toute simple en proportionnel avec valeur min pour vaincre les frottements int16_t commande_min=60; //pour vaincre les frottements int16_t commande=abs(erreur)+commande_min; int16_t commande_max=250; if (commande>commande_max) commande=commande_max; */ int16_t Kd=10; //de 0 à 50 int16_t Ki=2; int16_t Kp=1; //erreur=erreur+4*derivee_erreur+integrale_erreur/8; if ((port_in&2)!=0) { erreur=erreur*Kp; } if ((port_in&4)!=0) { erreur+=Kd*derivee_erreur; //quand l'erreur diminue, ce terme est négatif et permet d'éviter le dépassement } if ((port_in&8)!=0) { erreur+=(Ki*integrale_erreur)/TAB_SIZE; } int16_t commande=abs(erreur); int16_t commande_max=250; if (commande>commande_max) commande=commande_max; if (abs(erreur)<3) { motor.run(RELEASE); motor.setSpeed(0); sens=0; } else if (erreur<0) { motor.run(FORWARD); motor.setSpeed(commande); sens=-1; } else { motor.run(BACKWARD); motor.setSpeed(commande); sens=1; } /* Serial.print("consigne_position: "); Serial.print(consigne_position,DEC); Serial.print(" position: "); Serial.print(position,DEC); Serial.print(" erreur: "); Serial.print(erreur,DEC); Serial.print(" commande: "); Serial.print(commande,DEC); Serial.print(" sens: "); Serial.print(sens,DEC); Serial.println(""); */ Serial.print(consigne_position,DEC); Serial.print("\t"); Serial.print(position,DEC); Serial.print("\t"); Serial.print(erreur,DEC); Serial.print("\t"); Serial.print(0,DEC); Serial.print("\t"); Serial.print(1024,DEC); Serial.print("\t"); /*Serial.print(erreur,DEC); Serial.print("\t"); Serial.print(commande,DEC); Serial.print("\t"); Serial.print(sens,DEC); */ Serial.println(""); //delay(10); /* motor.run(BACKWARD); motor.setSpeed(120); /* uint16_t adc= analogRead(A0); if (adc>=512) { if (adc>1023) adc=1022; vitesse=(adc-512)/2; motor.run(FORWARD); motor.setSpeed(vitesse); } else { if (adc<2) adc=2; vitesse=(512-adc)/2; motor.run(BACKWARD); motor.setSpeed(vitesse); } /* vitesse=analogRead(A0)/4; motor.run(FORWARD); motor.setSpeed(vitesse); Serial.println(vitesse); */ /* char val; if (readPort8574(SLAVE_ADDR_8574_B,&val)==0) { Serial.print("lecture 8574 OK, "); if (writePort8574(SLAVE_ADDR_8574_A,val)==0) Serial.println("ecriture 8574 OK"); else Serial.println("ecriture 8574 NOK"); } else Serial.println("lecture 8574 NOK,"); */ //writePort8574(SLAVE_ADDR_8574_A,~vitesse); }