Diff: main.cpp

Revision:

30:62e21e5b4445

Parent:

29:93a31c16467b

--- a/main.cpp	Sat Sep 14 20:33:53 2019 +0000
+++ b/main.cpp	Sat Sep 14 22:30:43 2019 +0000
@@ -46,6 +46,7 @@
 }   JOINT;
 
 //
+DigitalOut myled(LED1);
 
 static volatile uint16_t gLastError;
 static volatile uint8_t gStep = 0;
@@ -135,15 +136,31 @@
     bool status = can1.read(messageIn);
     printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status);
     
-    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
+    if(can1.read(messageIn))
     {
-        speed_elbow = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+        myled = 1;
+        wait(0.5);
+        myled = 0;
+        wait(0.5);
+        if(messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
+        {
+            speed_elbow = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
+        }
     }
     
-    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
+    /*
+    if(can1.read(messageIn))
     {
-        speed_ee = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+        myled = 1;
+        wait(0.5);
+        myled = 0;
+        wait(0.5);
+        if(messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
+        {
+            speed_ee = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
+        }
     }
+    */
     
     if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW))
     {