Emile Toupet
IUT DE CACHAN, test robot
Diff: main.cpp
- Revision:
- 0:b07e06edae6f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 23 18:07:18 2011 +0000
@@ -0,0 +1,468 @@
+#include "mbed.h"
+#include "QEI.h"
+
+
+//***********************************************************//
+// #define _GAUCHE_ Control le moteur Gauche
+// #define _DROIT_ Control le moteur Gauche
+
+#define _DROIT_
+
+#ifdef _GAUCHE_
+ //*******************//
+ // MOTEUR GAUCHE //
+ //*******************//
+
+ #define CAN_ID_ECRITURE_ASSERVISSEMENT 0x050
+ #define CAN_ID_INFO_ASSERVISSEMENT 0x110
+ #define CAN_ID_INFO_HACHEUR 0x101
+
+ #define RECULE 0x01
+ #define AVANCE 0x02
+ #define PARAMETRE_MOTEUR 1
+
+#endif
+
+#ifdef _DROIT_
+ //******************//
+ // MOTEUR DROIT //
+ //******************//
+
+ #define CAN_ID_ECRITURE_ASSERVISSEMENT 0x060
+ #define CAN_ID_INFO_ASSERVISSEMENT 0x210
+ #define CAN_ID_INFO_HACHEUR 0x201
+
+ #define RECULE 0x01
+ #define AVANCE 0x02
+ #define PARAMETRE_MOTEUR -1
+
+#endif
+//***********************************************************//
+
+
+#define T_Time 0.5
+#define IMP_PAR_TOUR 36864 //impulsion par tour, codeur QEI
+#define DIAMETRE_ROUE 15 //diametre des roues en mm
+#define PI 3.14159
+
+#define SEUIL_POSITION 0.3
+#define Kp_position 1
+#define Kd_position 0.5
+#define Ki_position 1
+#define MAX_INTEG_POSITION 4 //8
+
+#define VITESSE_MAX_ASSERVISSEMENT 120
+#define Kp_vitesse 2
+
+
+DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
+
+//defintion du port I2C
+//I2C i2c_D(p28, p27);
+//I2C i2c_G(p9, p10);
+
+//QEI qei(pinA,pinB,index,puls/tour)
+//QEI qei_D(p26,p25,p24,1);
+//QEI qei_G(p23,p22,p21,1);
+
+//liason serie avec le PC
+Serial pc(USBTX, USBRX);
+
+
+//definition de la class Moteur
+class Moteur {
+public :
+
+ Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td);
+
+
+ //*********************//
+ // CALCUL DISTANCE //
+ //*********************//
+ float Calcul_Distance(void) {
+ if (pulse != qei.getPulses())
+ {
+ pulse = PARAMETRE_MOTEUR * qei.getPulses();
+ distance = (pulse * PI * DIAMETRE_ROUE) / IMP_PAR_TOUR;
+ //pc.printf("\r\n Distance = %f.2 ", distance);
+ }
+ return distance;
+ }
+
+
+ //********************//
+ // CALCUL VITESSE //
+ //********************//
+ void Calcul_Vitesse(void){
+
+ dist_1 = Calcul_Distance();
+ wait(0.01);
+ dist_2 = Calcul_Distance();
+
+ Vitesse_moteur = (dist_2 - dist_1)*100;
+ //pc.printf("\r\n Vitesse = %f.1 ", Vitesse_mesure);
+ }
+
+
+ //***************************//
+ // CALCUL ERREUR VITESSE //
+ //***************************//
+
+ void Calcul_Erreur_Vitesse(int Vitesse_voulu)
+ {
+
+ Last_Erreur_vitesse = Delta_Erreur_vitesse ;
+ Calcul_Vitesse();
+
+ if (Vitesse_moteur < 0 )
+ Vitesse_moteur = Vitesse_moteur * (-1);
+
+ Erreur_vitesse = Vitesse_voulu - Vitesse_moteur;
+
+ }
+
+ //****************************//
+ // ASSERVISSEMENT VITESSE //
+ //****************************//
+ void Asservissement_vitesse ( char DIRECTION ,char VITESSE , char VITESSE_MAX ,char ACCELERATION)
+ {
+
+ char result;
+ Calcul_Vitesse();
+ Vitesse_Sortie = Kp_vitesse * VITESSE ;
+
+ //Bridage du moteur
+ if (Vitesse_Sortie > VITESSE_MAX_ASSERVISSEMENT)
+ Vitesse_Sortie = VITESSE_MAX_ASSERVISSEMENT;
+
+ if (Vitesse_Sortie < 0)
+ Vitesse_Sortie = 0;
+
+ data_speed[1] = Vitesse_Sortie;
+ data_comma[1] = DIRECTION;
+ data_accel[1] = ACCELERATION;
+
+ /*
+ if (DIRECTION==0x00)
+ {
+ data_speed[1] = 0x44;
+ data_comma[1] = 0x00;
+ data_accel[1] = 0x00;
+ }
+ else
+ {
+ //Copie des valeur dans les tableaux de registre
+ data_speed[1] = Vitesse_Sortie;
+ data_comma[1] = DIRECTION;
+ data_accel[1] = ACCELERATION;
+ }
+ */
+ /*
+ if (Last_data_comma != data_comma[1])
+ {
+ //pc.printf("\r\n ASSERVISSEMENT DIRECTION");
+ i2c.write(0xB0, data_comma,2,1);
+ Last_data_comma = data_comma[1];
+ }
+
+ if (Last_data_accel != data_accel[1])
+ {
+ //pc.printf("\r\n ASSERVISSEMENT ACCELERATION");
+ i2c.write(0xB0, data_accel,2,1);
+ Last_data_accel = data_accel[1];
+ }
+
+ if (Last_data_speed != data_speed[1])
+ {
+ //pc.printf("\r\n ASSERVISSEMENT VITESSE");
+ i2c.write(0xB0, data_speed,2,1);
+ Last_data_speed = data_speed[1];
+ }
+ */
+
+ pc.printf("\r\n ASSERVISSEMENT DIRECTION");
+ //while(i2c.write(0xB0, data_comma,2,1));
+ result=i2c.write(0xB0, data_comma,2,1);
+ //while(i2c.write(0xB0, data_accel,2,1));
+ pc.printf("\r\n ASSERVISSEMENT VITESSE %d",result);
+ result=i2c.write(0xB0, data_speed,2,1);
+ //while(i2c.write(0xB0, data_speed,2,1));
+ pc.printf("\r\n ASSERVISSEMENT VITESSE fin %d",result);
+
+ }
+
+ //****************************//
+ // CALCUL ERREUR POSITION //
+ //****************************//
+ void Calcul_Erreur_Position(int Position_voulu)
+ {
+ Last_Erreur_position = Erreur_position;
+ Position_moteur = Calcul_Distance();
+ Erreur_position = Position_voulu - Position_moteur;
+
+ Delta_Erreur_position += 5 * Erreur_position ;
+
+ if (Delta_Erreur_position > MAX_INTEG_POSITION)
+ Delta_Erreur_position = MAX_INTEG_POSITION;
+
+ if (Delta_Erreur_position < - MAX_INTEG_POSITION)
+ Delta_Erreur_position = - MAX_INTEG_POSITION;
+ }
+
+ //********************************//
+ // ASSERVISSEMENT POSITION BIS //
+ //********************************//
+ void Asservissement_position()
+ {
+ Calcul_Erreur_Position(POSITION);
+ Speed = Kp_position * Erreur_position - Kd_position * Last_Erreur_position + Ki_position * Delta_Erreur_position;
+
+ if (Speed < 0 )
+ Speed = Speed * (-1);
+
+ if (Erreur_position < - SEUIL_POSITION )
+ {
+
+ led1=0;
+ led2=1;
+ led3=0;
+ led4=0;
+
+ Sens = RECULE;
+ }
+ else if (Erreur_position > SEUIL_POSITION )
+ {
+
+ led1=0;
+ led2=0;
+ led3=1;
+ led4=0;
+
+ Sens = AVANCE;
+ }
+ else
+ {
+
+ led1=1;
+ led2=0;
+ led3=0;
+ led4=1;
+
+ Sens = 0x00;
+ }
+
+ if (Speed > VITESSE*2 )
+ Speed = VITESSE*2;
+
+ if (Speed > 254 )
+ Speed = 254;
+
+ Asservissement_vitesse(Sens,Speed,VITESSE,ACCELERATION);
+ }
+
+ //********************//
+ // INITIALISATION //
+ //********************//
+ void Initialisation (void)
+ {
+ //initialisation du CAN
+ can.frequency(1000000);
+
+ i2c.frequency(100000);
+
+ //Initialisation des param�tre d'asservissement.
+ ACCELERATION = 0x00;
+ VITESSE = 0x0;
+ POSITION = 0x0000;
+
+ //initialisation des registres d'adresse
+ data_comma[0] = 0x00;
+ data_speed[0] = 0x02;
+ data_accel[0] = 0x03;
+ }
+
+
+ //******************************//
+ // DEBUG SERIAL I2C HACHEUR //
+ //******************************//
+ void debug_serial_i2c_hacheur (void)
+ {
+ char data_recu[8];
+ unsigned char i;
+
+ i2c.read(0xB0, data_recu ,8);
+
+ pc.printf("\r\n \r\n");
+ pc.printf("temp moto unus revi | comm stat spee acce\r\n");
+
+ for(i=0;i<8;i++)
+ {
+ if (i==4) pc.printf("| ");
+ pc.printf(" %02x ", data_recu[i] );
+ }
+ }
+
+
+ //***********************//
+ // DEBUG SERIAL mBed //
+ //***********************//
+ void debug_serial_mbed (void)
+ {
+ pc.printf(" %d | %.1f | %02x | %.lf | %d | %.1f \r\n ",
+ data_speed[1], Vitesse_moteur, Sens,Position_moteur,Speed,Erreur_position);
+ }
+
+
+ //*****************************************//
+ // DEBUG SERIAL I2C mBed ASSERVISSEMENT //
+ //*****************************************//
+ void debug_serial_i2c_mbed_asservissement(void)
+ {
+ pc.printf(" %d = %.lf - %.1f + %.lf \r\n ",
+ Speed, Kp_position * Erreur_position , Kd_position * Last_Erreur_position , Ki_position * Delta_Erreur_position);
+ }
+
+ //**********************//
+ // CAN MESSAGE SEND //
+ //**********************//
+ void CANMSG_Send(int CANID,char CAN_data[])
+ {
+ char i;
+ CANMessage canmsg = CANMessage();
+
+ canmsg.id = CANID; /*L'identification du message*/
+ canmsg.len = 8; /*Longeur de la trame en octet*/
+ canmsg.format = CANStandard; /*Format de la trame CANStandard 29 bits*/
+ canmsg.type = CANData; /*Type de la trame CANData*/
+
+ for(i=0;i<8;i++)
+ canmsg.data[i] = CAN_data[i];
+
+ can.write(canmsg); /*Envoi du message*/
+ }
+
+ //*****************************//
+ // CAN INFO ASSERVISSEMENT //
+ //*****************************//
+ void CAN_INFO_Asservissement (void)
+ {
+ char data_asservicement[8];
+ short Position_moteur_Can, Vitesse_moteur_Can;
+
+ Position_moteur_Can = Position_moteur;
+ Vitesse_moteur_Can = Vitesse_moteur;
+
+ if (Vitesse_moteur_Can <0)
+ Vitesse_moteur_Can = Vitesse_moteur_Can *(-1);
+
+ data_asservicement[0] = Sens;
+ data_asservicement[1] = Vitesse_moteur_Can & 0x00FF;
+ data_asservicement[2] = 0;
+ data_asservicement[3] = 0;
+
+ data_asservicement[4] = POSITION >>8;
+ data_asservicement[5] = POSITION & 0x00FF;
+ data_asservicement[6] = Position_moteur_Can >>8;
+ data_asservicement[7] = Position_moteur_Can & 0x00FF;
+
+ CANMSG_Send(CAN_ID_INFO_ASSERVISSEMENT,data_asservicement);
+ }
+
+ //**********************//
+ // CAN INFO Hacheur //
+ //**********************//
+ void CAN_INFO_Hacheur (void)
+ {
+ char data_Hacheur[8];
+ pc.printf("\r\n ENTRER INFO ");
+ i2c.read(0xB0, data_Hacheur ,8);
+ pc.printf("\r\n SORTIE INFO ");
+ //temp moto unus revi | comm stat spee acce
+ CANMSG_Send(CAN_ID_INFO_HACHEUR,data_Hacheur);
+ }
+
+ //*************************//
+ // CAN MESSAGE RECIEVE //
+ //*************************//
+ void CAN_RecieveInterrupt_Routine(void)
+ {
+
+ static CANMessage can_recieve_msg;
+
+ if(can.read(can_recieve_msg))
+ {
+ if(can_recieve_msg.id==CAN_ID_ECRITURE_ASSERVISSEMENT)
+ {
+
+ Sens = can_recieve_msg.data[0] ;
+ VITESSE = can_recieve_msg.data[1] ;
+ ACCELERATION = can_recieve_msg.data[2] ;
+ POSITION = (short) can_recieve_msg.data[6]<<8 | can_recieve_msg.data[7];
+
+ }
+
+ }
+ }
+
+// protected bite:
+ QEI qei;
+ I2C i2c;
+ CAN can;
+
+ short POSITION;
+ char VITESSE,ACCELERATION, Sens;
+
+ float Position_moteur, Erreur_position, Delta_Erreur_position, Last_Erreur_position ;
+ float distance , dist_1, dist_2;
+ float Vitesse_Sortie, Vitesse_moteur, Erreur_vitesse,Last_Erreur_vitesse,Delta_Erreur_vitesse;
+
+ int pulse,i;
+ int Speed;
+
+ char Last_data_speed, Last_data_comma, Last_data_accel;
+
+ // data_hacheur[2]={ registre, valeur}
+ char data_comma[2];
+ char data_speed[2];
+ char data_accel[2];
+
+};
+
+//constructeur de la class "Moteur"
+Moteur::Moteur(PinName qeiA, PinName qeiB, PinName qei_index, int pulsesPerRev, PinName sda, PinName scl, PinName rd, PinName td) :
+ qei(qeiA, qeiB, qei_index, pulsesPerRev),
+ i2c(sda,scl),
+ can(rd,td){
+ distance = 0.0;
+ pulse = 0;
+}
+
+//definiton des Classes
+Moteur moteur(p21,p22,p23,1,p9,p10,p30,p29);//, moteur_G(p23,p22,p21,1,p9,p10);
+
+
+//fonction principal
+int main()
+{
+ moteur.Initialisation();
+ pc.printf("\r\n DEBUT");
+
+ while (1)
+ {
+
+
+ pc.printf("\r\n ASSERVISSEMENT");
+ moteur.CAN_INFO_Asservissement();
+ //moteur.CAN_INFO_Hacheur();
+ pc.printf("\r\n RECEIVE");
+ moteur.CAN_RecieveInterrupt_Routine();
+ pc.printf("\r\n POSITION");
+ moteur.Asservissement_position();
+ //moteur.debug_serial_mbed();
+ //moteur.debug_serial_i2c_mbed_asservissement();
+
+
+ pc.printf("\r\n TEMPO");
+ wait(0.01);
+ }
+}
\ No newline at end of file