![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version 2.6 mode normal en cours(problème de décalage)
Dependencies: mbed
Fork of Robot_a_cables_v2_5 by
Revision 8:1e6d3d9ae1d3, committed 2018-05-15
- Comitter:
- protongamer
- Date:
- Tue May 15 10:02:20 2018 +0000
- Parent:
- 7:e87feff62bfd
- Commit message:
- v2.6;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
parameters.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon May 14 15:27:10 2018 +0000 +++ b/main.cpp Tue May 15 10:02:20 2018 +0000 @@ -7,7 +7,10 @@ */ + #define DEGRAD 0 + #define NORMAL 1 + #define ALPHA 425000 DigitalOut MOTOR1_OP(LED1); DigitalOut MOTOR2_OP(LED2); @@ -42,19 +45,19 @@ //l1 = longueur moteur gauche à l'effecteur //l2 = longueur moteur droite à l'effecteur int32_t lx, l1, l2; - +float pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur void display_mode() { - if(mode == 0) // MODE DEGRADE + if(mode == DEGRAD) // MODE DEGRADE { MOD_DEG = 1; MOD_NOR = 0; } - else if(mode == 1) // MODE NORMAL + else if(mode == NORMAL) // MODE NORMAL { MOD_DEG = 0; MOD_NOR = 1; @@ -154,16 +157,48 @@ void read_position(){ - - - - + //timer_process_read++;//var used to set time for reading value + l1 = LENGTH_L1_PO - (p1/2545); //Length 1 + l2 = LENGTH_L2_PO - (p2/2545); //Length 2 + lx = LENGTH_MOTOR; + dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx); + dy = sqrt(((double) l1*(double) l1) - (dx*dx)); + + if(BPO == 0){ - - + p_pom = p_actual; + p_pom2 = p_actual2; + mode = !mode; //Change mode + wait(1.0); + } - - + p1 = p_pom - p_actual; + p2 = p_pom2 - p_actual2; + +//Left motor + send(TPDO1_L, command, 'R', 0); //Ask position for second length + if(can2.read(msg)){ + p_actual = msg.data[3]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[2]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[1]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[0]; + } + //timer_process_read = 1; + wait(0.0005); + send(TPDO1_R, command, 'R', 0); //Ask position for first length + if(can2.read(msg)){ + p_actual2 = msg.data[3]; + p_actual2 = p_actual2 << 8; + p_actual2 = p_actual2 | msg.data[2]; + p_actual2 = p_actual2 << 8; + p_actual2 = p_actual2 | msg.data[1]; + p_actual2 = p_actual2 << 8; + p_actual2 = p_actual2 | msg.data[0]; + } + }//fin de fonction @@ -177,14 +212,18 @@ ctr_y = int((joystick_ctr.read()*100)-50); if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;} // Detection x avec correction ctr de -42 à +42 if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr - + float VX; // Calcule brut vélocité x, y + float VY; + long int VM_L; // Calcule des vélocités exactes de chaques moteurs + long int VM_R; + if(mode ==0) { ///////////// MODE DEGRADE ///////////////////////////////////// - float VX = (float(x)/42)*425000; // Calcule brut vélocité x, y - float VY = (float(y)/42)*425000; - long int VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs - long int VM_R = -VX - VY; + VX = (float(x)/42)*425000; // Calcule brut vélocité x, y + VY = (float(y)/42)*425000; + VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs + VM_R = -VX - VY; command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian command[1]= (VM_L & 0x0000FF00)>>8; @@ -201,8 +240,28 @@ } else if(mode ==1) { - // MODE NORMAL + ///////////// MODE NORMAL ////////////////////////////////////// + VX = x; + VY = y; + VM_L = (ALPHA/42); + VM_L = (VM_L/l1)*((dx*VX)-(dy*VY)); + VM_R = (ALPHA/42); + VM_R = (VM_R/l2)*((-(lx-dx)*VX)-(dy*VY)); + + command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian + command[1]= (VM_L & 0x0000FF00)>>8; + command[2]= (VM_L & 0x00FF0000)>>16; + command[3]= (VM_L & 0xFF000000)>>24; + send(RPDO1_L, command, 'D', 4); // Controle moteur gauche + + command[0]= (VM_R & 0x000000FF); + command[1]= (VM_R & 0x0000FF00)>>8; + command[2]= (VM_R & 0x00FF0000)>>16; + command[3]= (VM_R & 0xFF000000)>>24; + send(RPDO1_R, command, 'D', 4); // Controle moteur droit } + pVx = VM_R; + pVy = VM_L; ////////////////////////////////////////////////////////////////////////////////////////////////////////////// } @@ -217,93 +276,20 @@ - lx = LENGTH_MOTOR; + while(1) { - timer_process_read++; - l1 = LENGTH_L1_PO - (p1/2545); //Length 1 - l2 = LENGTH_L2_PO - (p2/2545); //Length 2 - // dx = (pow((double) l1, 2) - pow((double) l2, 2) + pow((double) lx, 2))/(2*(double) lx); - // dy = sqrt(pow((double) l1, 2) - pow(dx, 2)); - dx = (((double) l2*(double) l2) - ((double) l1*(double) l1) + ((double) lx*(double) lx))/(2*(double) lx); - dy = sqrt(((double) l1*(double) l1) - (dx*dx)); - - if(BPO == 0){ - timer_process_read = 0; - p_pom = p_actual; - p_pom2 = p_actual2; - wait(1.0); - } - - - - - - - - - - - - if(timer_process_read == 5){ - send(TPDO1_L, command, 'R', 0); //Ask position for second length - if(can2.read(msg)){ - p_actual2 = msg.data[3]; - p_actual2 = p_actual2 << 8; - p_actual2 = p_actual2 | msg.data[2]; - p_actual2 = p_actual2 << 8; - p_actual2 = p_actual2 | msg.data[1]; - p_actual2 = p_actual2 << 8; - p_actual2 = p_actual2 | msg.data[0]; - } - - - } - - if(timer_process_read >= 10){ - send(TPDO1_R, command, 'R', 0); //Ask position for first length - if(can2.read(msg)){ - p_actual = msg.data[3]; - p_actual = p_actual << 8; - p_actual = p_actual | msg.data[2]; - p_actual = p_actual << 8; - p_actual = p_actual | msg.data[1]; - p_actual = p_actual << 8; - p_actual = p_actual | msg.data[0]; - } - timer_process_read = 0; - } - - - - - - - pc.printf("L1 = %d", l1); pc.printf("\t L2 = %d", l2); pc.printf("\t x = %f", dx); - pc.printf("\t y = %f", dy);/* - pc.printf("\t %d", p_actual); - pc.printf("\t %d", p_actual2); - pc.printf("\t %d", p_pom); - pc.printf("\t %d\r\n", p_pom2);*/ - pc.printf("\t %d", p1); - pc.printf("\t %d \r\n", p2); - - - - p1 = p_pom - p_actual; - p2 = p_pom2 - p_actual2; - + pc.printf("\t y = %f", dy); + pc.printf("\t VM_L = %f", pVx); + pc.printf("\t VM_R = %f \r\n", pVy); - - - - //read_position(); + read_position(); control_Moteur(); // Controle du meteur mode degradé et normal display_mode(); // Controle des led affichant le mode
--- a/parameters.h Mon May 14 15:27:10 2018 +0000 +++ b/parameters.h Tue May 15 10:02:20 2018 +0000 @@ -98,8 +98,8 @@ //Length from motors to cursor in millimeters(mm) when the origin is taken //Left motor to cursor -#define LENGTH_L1_PO 1470 +#define LENGTH_L1_PO 781 //Right motor to cursor -#define LENGTH_L2_PO 1470 +#define LENGTH_L2_PO 781 //Left motor to Right motor #define LENGTH_MOTOR 985