version 2.5(pour le grand FEARHEAVEN) [spoil] oui [/spoil]
Dependencies: mbed
Fork of Robot_a_cables_v2_5 by
Revision 7:e87feff62bfd, committed 2018-05-14
- Comitter:
- protongamer
- Date:
- Mon May 14 15:27:10 2018 +0000
- Parent:
- 6:ffffa9dfadfc
- Commit message:
- version 2.5;
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 |
diff -r ffffa9dfadfc -r e87feff62bfd main.cpp --- a/main.cpp Mon May 07 12:12:34 2018 +0000 +++ b/main.cpp Mon May 14 15:27:10 2018 +0000 @@ -13,8 +13,8 @@ DigitalOut MOTOR2_OP(LED2); DigitalOut DEB_MOD_DEG(LED3); DigitalOut DEB_MOD_NOR(LED4); -DigitalOut MOD_DEG(p26); -DigitalOut MOD_NOR(p25); +DigitalOut MOD_DEG(p25); +DigitalOut MOD_NOR(p26); DigitalIn SWH(p5); DigitalIn SWV(p6); DigitalIn BPO(p7); @@ -31,17 +31,17 @@ uint8_t ping = 0; //variable pour etat de processus uint8_t done = 0; char command[8]; //word to send command -uint64_t timer_process_read; //counter to make mbed read actual position -float dy = 1262, dx = 492.5;//coordinates values when origin is done +uint16_t timer_process_read; //counter to make mbed read actual position +double dy = 1262, dx = 492.5;//coordinates values when origin is done //these positions are in counters int32_t p1, p2; //motors positions -int32_t p_actual, p_pom; //p_actual position read by can, p_pom position done when origin is made +int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made //longueur câbles //lx = longueur entre les point des moteurs //l1 = longueur moteur gauche à l'effecteur //l2 = longueur moteur droite à l'effecteur -int64_t lx, l1, l2; +int32_t lx, l1, l2; @@ -158,21 +158,12 @@ - /* send(TPDO1_R, command, 'R', 0); //Ask position - timer_process_read = 0; - p2 = msg.data[3]; - p2 = p2 << 8; - p2 = p2 | msg.data[2]; - p2 = p2 << 8; - p2 = p2 | msg.data[1]; - p2 = p2 << 8; - p2 = p2 | msg.data[0];*/ + - lx = 985; - //l2 = LENGTH_L2_PO + (p2*1000)/2500; + }//fin de fonction @@ -226,70 +217,53 @@ - + lx = LENGTH_MOTOR; while(1) { - //timer_process_ping++; + timer_process_read++; - l1 = (LENGTH_L1_PO*1000) - (p1*1000)/2500; //Longueur 1 + 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){ - send(TPDO1_R, command, 'R', 0); //Ask position - - p_pom = msg.data[3]; - p_pom = p_pom << 8; - p_pom = p_pom | msg.data[2]; - p_pom = p_pom << 8; - p_pom = p_pom | msg.data[1]; - p_pom = p_pom << 8; - p_pom = p_pom | msg.data[0]; - wait(1.0); + if(BPO == 0){ + timer_process_read = 0; + p_pom = p_actual; + p_pom2 = p_actual2; + wait(1.0); } - - //dy = 525000 + (d_new*1000)/2500; - + + + + + + - //wait_ms(1); + + + - /* if(timer_process_ping >= 5000){ - send(TPDO2_R, command, 'R', 0); //Ask mod of motor - timer_process_ping = 0; - if(can2.read(msg)) { - for(int i = 0; i < msg.len; i++){ - pc.printf("0x%x ",msg.data[i]); - }//end of for - pc.printf("\r \n"); - }//can.read(msg) - - if(msg.data[0] != 0x27 || msg.data[1] != 0x56){//if device is not in operational mode - ping = 0; - while(ping == 0){ - pc.printf("ping...\r\n"); - send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode - MOD_NOR = 1; - if(can2.read(msg)) { - for(int i = 0; i < msg.len; i++){ - pc.printf("0x%x ",msg.data[i]); - ping = 1; - }//end of for - pc.printf("\r \n"); - }//can.read(msg) - wait(0.1); - MOD_NOR = 0; - wait(0.9); - - } - - } - - }*/ - pc.printf("%d", l1); - pc.printf("\t %d", p_pom); - pc.printf("\t %d", p_actual); - pc.printf("\t %d \r\n", p1); - if(timer_process_read >= 10){ - send(TPDO1_R, command, 'R', 0); //Ask position - timer_process_read = 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]; @@ -297,18 +271,37 @@ p_actual = p_actual | msg.data[1]; p_actual = p_actual << 8; p_actual = p_actual | msg.data[0]; + } + timer_process_read = 0; + } - } - - p1 = p_pom - p_actual; + + + + + + + + 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; - if(can2.read(msg)) { - for(int i = 0; i < msg.len; i++){ - //pc.printf("0x%x ",msg.data[i]); - }//end of for - //pc.printf("\r \n"); - }//can.read(msg) + + + //read_position(); control_Moteur(); // Controle du meteur mode degradé et normal
diff -r ffffa9dfadfc -r e87feff62bfd parameters.h --- a/parameters.h Mon May 07 12:12:34 2018 +0000 +++ b/parameters.h Mon May 14 15:27:10 2018 +0000 @@ -98,5 +98,8 @@ //Length from motors to cursor in millimeters(mm) when the origin is taken //Left motor to cursor -#define LENGTH_L1_PO 1355 -#define LENGTH_L2_PO 1355 +#define LENGTH_L1_PO 1470 +//Right motor to cursor +#define LENGTH_L2_PO 1470 +//Left motor to Right motor +#define LENGTH_MOTOR 985