version 2.5(pour le grand FEARHEAVEN) [spoil] oui [/spoil]
Dependencies: mbed
Fork of Robot_a_cables_v2_5 by
Diff: main.cpp
- Revision:
- 7:e87feff62bfd
- Parent:
- 6:ffffa9dfadfc
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