Emile Toupet
/
VISITY
IUT DE CACHAN, test robot
main.cpp
- Committer:
- emiletoupet
- Date:
- 2011-03-23
- Revision:
- 0:b07e06edae6f
File content as of revision 0:b07e06edae6f:
#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); } }