Diff: main.cpp

Revision:

9:0f63d4cb5613

Parent:

8:1e6d3d9ae1d3

Child:

10:fb3542f3c5e6

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;