Leopold Clement
/
PSL-VicTeam
faut que Val répare le truc
main.cpp
- Committer:
- leocle
- Date:
- 2020-06-10
- Revision:
- 19:6338e004c566
- Parent:
- 18:68a8f70707e6
File content as of revision 19:6338e004c566:
/* #include <mbed_events.h> */ #include "mbed.h" #define adrr_queue 2 #define adrr_gauche 1 #define adrr_droite 3 #define hex_queue 0x02 #define hex_gauche 0x01 #define hex_droite 0x03 DigitalOut alivenessLED(LED1); DigitalOut testLed(LED2); Serial coach(D1, D0); /*Serial coach(USBTX, USBRX);*/ Serial myserial_ax12(PC_4,PC_5,115200); // Modifier le baud rate en fonction de votre utilisation des moteurs const float rapport_vitesse = 0.019383809*0.026315; const float root3 = 1.7321; /*a=moteur de queue AX12 myax12a (PC_4, PC_5, 2,115200); b : moteur de gauche AX12 myax12b (PC_4, PC_5, 1,115200); c : moteur de droite AX12 myax12c (PC_4, PC_5, 3,115200);*/ int i = 0; char last_read; int indice_a_modifie = 1; char message_0[50] = {}; char message_1[50] = {}; char message_vide = '0'; float front, lat, rot, T_front, T_lat = 0; int flag_msg = 0; int flag_a_traite = 0; DigitalOut my_led1(LED1); DigitalOut my_led2(LED2); DigitalOut my_led3(LED3); DigitalIn bp(USER_BUTTON); void SetCRSpeed(int ID, float speed); void SetMode(int ID, int mode); void SetCWLimit (int ID, int degrees); void SetCCWLimit (int ID, int degrees); void Write(int ID, int start, int bytes, char* data); char tata[10] = {}; uint16_t position_1; uint16_t position_2; uint16_t position_3; int num_moteur = 1; int vitesse; char sens; int temps_ms = 0; //pour connaitre le nombre de tours int tour_1 = 0; int tour_2 = 0; int tour_3 = 0; Ticker ticker_1ms; uint16_t position_1_old; uint16_t position_2_old; uint16_t position_3_old; int16_t delta_position_1; int16_t delta_position_2; int16_t delta_position_3; //Définition des trames de requête char trame_demande_pos_1[8]= {0xFF,0xFF,hex_queue,0x4,0x2,0x24,2,0xD2}; char trame_demande_pos_2[8]= {0xFF,0xFF,hex_gauche,0x4,0x2,0x24,2,0xD1}; char trame_demande_pos_3[8]= {0xFF,0xFF,hex_droite,0x4,0x2,0x24,2,0xD0}; //Fonction d'interruption sur réception de caractère sur liaison série void Rx_Irq_fonction(void) { uint16_t position_lu = 0; my_led1 = !my_led1; static int i_rx=0; tata[i_rx]=myserial_ax12.getc(); if (tata[0]!=0xFF) // On vérifie que le premier et le deuxième octet sont biens 0xFF i_rx=0; else i_rx++; if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0; //Une fois qu'on sait que la trame est "juste" on récupère le reste if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) { my_led3 = !my_led3; i_rx=0; if(tata[4] == 0) { //si erreur est nulle //On transforme les données en fonction de ce qu'on lit depuis les moteurs position_lu = tata[5]+((uint16_t)tata[6]<<8); if((0<= position_lu)&&(position_lu<=4095)) { position_1 = position_lu; } } for(int i=0; i<10; i++) { tata[i]=0; } } if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) { my_led3 = !my_led3; i_rx=0; if(tata[4]==00) { //si erreur nulle //On transforme les données en fonction de ce qu'on lit depuis les moteurs position_lu = tata[5]+((uint16_t)tata[6]<<8); if((0<= position_lu)&&(position_lu<=4095)) { position_2 = position_lu; } } for(int i=0; i<10; i++) { tata[i]=0; } } if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) { my_led3 = !my_led3; i_rx=0; if(tata[4]==00) { //si erreur nulle //On transforme les données en fonction de ce qu'on lit depuis les moteurs position_lu = tata[5]+((uint16_t)tata[6]<<8); if((0<= position_lu)&&(position_lu<=4095)) { position_3 = position_lu; } } for(int i=0; i<10; i++) { tata[i]=0; } } if (i_rx>9) { for(int i=0; i<10; i++) tata[i]=0; i_rx=0; } } void fonction_1ms(void) { temps_ms++; } void asser(float Vx, float Vy, float gammaz) { float wa = rapport_vitesse*(Vy + gammaz); float wb = rapport_vitesse*( -0.5 * Vy + (root3 / 2) * Vx + gammaz); float wc = rapport_vitesse*( -0.5 * Vy - (root3 / 2) * Vx + gammaz); /*moteur a */ SetCRSpeed(adrr_queue, wa); wait(0.1) /*moteur b */ SetCRSpeed(adrr_gauche, wb); wait(0.1) /* moteur c */ SetCRSpeed(adrr_droite, wc); wait(0.1) } void send_stat() { coach.printf("S%f,%f,%f,%f,%f \r\n", front, lat, rot, T_front, T_lat ); } void send_roues() { coach.printf("\n\rR %5d ; %5d ; %5d ; %5d ; %5d ; %5d\r\n", position_1, delta_position_1, position_2, delta_position_2, position_3, delta_position_3); } void mise_a_zero_msg_0() { for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères message_0[i] = message_vide; } } void mise_a_zero_msg_1() { for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères message_1[i] = message_vide; } } void callback_serial() { last_read = coach.getc(); if (last_read == 'S') { send_stat(); } else if (last_read == 'R') { send_roues(); } else if (last_read == 'Z'){ front = 0; lat = 0; rot = 0; } else if (last_read == ';') { coach.puts("EOL\n"); /*coach.puts(message_0); coach.puts(message_1); */ if (flag_msg == 0) { message_0[indice_a_modifie] = last_read; mise_a_zero_msg_1(); flag_msg = 1; } else { message_1[indice_a_modifie] = last_read; mise_a_zero_msg_0(); flag_msg = 0; } flag_a_traite = 1; indice_a_modifie = 0; } else { if (flag_msg == 0) { message_0[indice_a_modifie] = last_read; } else { message_1[indice_a_modifie] = last_read; } indice_a_modifie++; } } void reception_com() { if (flag_a_traite == 1) { coach.puts("traitement\n"); if (flag_msg == 0) { sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); } else { sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); } flag_a_traite = 0; } } int main() { coach.baud(115200); coach.puts("slt\n\r"); coach.attach(&callback_serial, Serial::RxIrq); //myax12.SetMode(1); //passage en Continuous rotation //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6}; //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5}; //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4}; //pour regarder les 3 moteurs les uns à la suite des autres int etat=1; int fm_etat=1; SetMode(adrr_queue , 1); wait(0.1); SetMode(adrr_droite, 1); wait(0.1); SetMode(adrr_gauche, 1); /* SetCRSpeed(1,0.05); wait(0.1); SetCRSpeed(2,0.05); wait(0.1); SetCRSpeed(3,0.05); */ char a; //On passe le mode retour en 1 : retour seulement sur demande de lecture a=1; Write(adrr_queue,16,1,&a); Write(adrr_droite,16,1,&a); Write(adrr_gauche,16,1,&a); //On passe le délai de retour à 250µs. C'est bien de le diminuer ensuite. a=250; Write(adrr_queue,5,1,&a); Write(adrr_droite,5,1,&a); Write(adrr_gauche,5,1,&a); /* On coupe de le couple du moteur pour pouvoir le faire tourner à la main a=0; Write(2,24,1,&a); Write(1,24,1,&a); Write(3,24,1,&a); */ myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq); ticker_1ms.attach(&fonction_1ms,0.001); etat=2; coach.printf("fin_initialisation"); position_1 = 0; position_2 = 0; position_3 = 0; position_1_old = 0; position_2_old = 0; position_3_old = 0; delta_position_1 = 0; delta_position_2 = 0; delta_position_3 = 0; while (true) { //on regarde la position des moteurs switch(etat) { case 2 : if(fm_etat) { position_1_old = position_1; for(int i=0; i<=7; i++) { //On envoi la trame que l'on souhaite myserial_ax12.putc(trame_demande_pos_1[i]); //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures } fm_etat=0; } if(temps_ms>=3) { etat=3; fm_etat=1; } break; case 3 : if(fm_etat) { delta_position_1 = (int16_t)position_1 - (int16_t)position_1_old; if(delta_position_1 > 2048) { delta_position_1 = delta_position_1 - 4096; } if(delta_position_1 < -2048) { delta_position_1 = delta_position_1 + 4096; } position_2_old = position_2; for(int i=0; i<=7; i++) { //On envoi la trame que l'on souhaite myserial_ax12.putc(trame_demande_pos_2[i]); //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures } fm_etat=0; } if(temps_ms>=6) { etat=4; fm_etat=1; } break; case 4 : if(fm_etat) { delta_position_2 = (int16_t)position_2 - (int16_t)position_2_old; if(delta_position_2 > 2048) { delta_position_2 = delta_position_2 - 4096; } if(delta_position_2 < -2048) { delta_position_2 = delta_position_2 + 4096; } position_3_old = position_3; for(int i=0; i<=7; i++) { //On envoi la trame que l'on souhaite myserial_ax12.putc(trame_demande_pos_3[i]); } fm_etat=0; } if(temps_ms>=9) { etat=5; fm_etat=1; } break; case 5 : if(fm_etat == 1) { delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old; if(delta_position_3 > 2048) { delta_position_3 = delta_position_3 - 4096; } if(delta_position_3 < -2048) { delta_position_3 = delta_position_3 + 4096; } send_roues(); } fm_etat =0; if (flag_a_traite == 1) { etat = 6; temps_ms = 0; fm_etat =1; } break; case 6 : if(fm_etat ==1){ coach.puts("traitement\n"); if (flag_msg == 0) { sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); } else { sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); } flag_a_traite = 0; temps_ms = 0; asser(front, lat, rot); fm_etat = 0; } if(temps_ms>= 5) { etat = 2; fm_etat =1; temps_ms = 0; } break; } } } void SetCRSpeed(int ID, float speed) { // bit 10 = direction, 0 = CCW, 1=CW // bits 9-0 = Speed char data[2]; int goal = (0x3ff * abs(speed)); // Set direction CW if we have a negative speed if (speed < 0) { goal |= (0x1 << 10); } data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits // write the packet, return the error code Write(ID, 0x20, 2, data); } void SetMode(int ID, int mode) { if (mode == 1) { // set CR SetCWLimit(ID,0); SetCCWLimit(ID,0); SetCRSpeed(ID,0.0); } else { SetCWLimit(ID,0); SetCCWLimit(ID,360); SetCRSpeed(ID,0.0); } } void SetCWLimit (int ID, int degrees) { char data[2]; short limit = (4095 * degrees) / 360; data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits Write(ID, 0x06, 2, data); } void SetCCWLimit (int ID, int degrees) { char data[2]; short limit = (4095 * degrees) / 360; data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits Write(ID, 0x08, 2, data); } void Write(int ID, int start, int bytes, char* data) { char TxBuf[16]; char sum = 0; TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; // packet Length TxBuf[3] = 3+bytes; sum += TxBuf[3]; // Instruction TxBuf[4]=0x03; sum += TxBuf[4]; // Start Address TxBuf[5] = start; sum += TxBuf[5]; // data for (char i=0; i<bytes ; i++) { TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; } // checksum TxBuf[6+bytes] = 0xFF - sum; // Transmit the packet in one burst with no pausing for (int i = 0; i < (7 + bytes) ; i++) { myserial_ax12.putc(TxBuf[i]); } wait(0.00002); }