version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
Diff: main.cpp
- Revision:
- 9:0f63d4cb5613
- Parent:
- 8:1e6d3d9ae1d3
- Child:
- 10:e4e1228604fb
diff -r 1e6d3d9ae1d3 -r 0f63d4cb5613 main.cpp --- a/main.cpp Tue May 15 10:02:20 2018 +0000 +++ b/main.cpp Fri May 18 09:03:25 2018 +0000 @@ -10,6 +10,7 @@ #define DEGRAD 0 #define NORMAL 1 + //#define ALPHA 425000 #define ALPHA 425000 DigitalOut MOTOR1_OP(LED1); @@ -187,7 +188,7 @@ p_actual = p_actual | msg.data[0]; } //timer_process_read = 1; - wait(0.0005); + wait(0.005); send(TPDO1_R, command, 'R', 0); //Ask position for first length if(can2.read(msg)){ p_actual2 = msg.data[3]; @@ -243,10 +244,12 @@ ///////////// 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)); + VM_L = ((dx*VX)-(dy*VY)); + VM_L = (VM_L/l1); + VM_L = VM_L*(ALPHA/42); + VM_R = ((-(lx-dx)*VX)-(dy*VY)); + VM_R = (VM_R/l2); + VM_R = VM_R*(ALPHA/42); command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian command[1]= (VM_L & 0x0000FF00)>>8;