Diff: main.cpp

Revision:

21:43740448011a

Parent:

20:ff2dab77e3e9

Child:

22:ffb26af4d5d8

diff -r ff2dab77e3e9 -r 43740448011a main.cpp
--- a/main.cpp	Tue Apr 16 09:02:27 2019 +0000
+++ b/main.cpp	Fri Jun 21 07:42:44 2019 +0000
@@ -6,6 +6,13 @@
 
 #define SPEED 50
 
+#define JOINT_SET_SPEED 20
+
+#define JOINT1_ID 4
+#define JOINT2_ID 5
+#define JOINT3_ID 6
+#define JOINT4_ID 7
+
 // Utility
 InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
@@ -18,8 +25,8 @@
 AX12  motor_4(wire, 4, 1);
 
 // Camera PanTilt Control
-Servo cam1 (D9);
-Servo cam2 (D10);
+//Servo cam1 (PC_0);
+//Servo cam2 (PC_1);
 
 void button_int_handler()
 {
@@ -28,19 +35,20 @@
 
 // CAN
 //Thread canrxa;
-CAN can1(PA_11, PA_12);     // RX, TX
+CAN can1(PB_5, PB_6);     // RX, TX
 
 CANMessage messageIn;
 CANMessage messageOut;
 
-int filter = can1.filter(0x000, 0x400, CANStandard);
-
 int pose;
 int current_pose[] = {0, 0, 0, 0, 0, 0};
 
 int main()
 {
     can1.frequency(125000);
+    
+    messageIn.format=CANExtended;
+    
     printf("CAN: Init DONE\n\r");
 
     wire.trigger();
@@ -74,81 +82,54 @@
     printf("DYNAMIXEL: Init DONE 4\n\r");
 
     printf("Running!\n\r");
+    
+    int32_t speed = 0;
 
     while(true)
     {
-        if(can1.read(messageIn, filter))
+      if(can1.read(messageIn))
+      {
+        if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT1_ID))
+        {
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor_1.setSpeed(speed);
+          
+          printf("CAN: mess %d\n\r", speed);
+        }
+        
+        if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT2_ID))
         {
-            pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
-
-            if(messageIn.id == 0x40 && pose != current_pose[0])
-            {
-                if(pose == 1)
-                {
-                    motor_1.setSpeed(-SPEED);
-                }
-                else if(pose == 2)
-                {
-                    motor_1.setSpeed(SPEED);
-                }
-                else
-                {
-                    motor_1.setSpeed(0);
-                }
-                
-                current_pose[0] = pose;
-            }
-            else if(messageIn.id == 0x50 && pose != current_pose[1])
-            {
-                if(pose == 1)
-                {
-                    motor_2.setSpeed(SPEED);
-                }
-                else if(pose == 2)
-                {
-                    motor_2.setSpeed(-SPEED);
-                }
-                else
-                {
-                    motor_2.setSpeed(0);
-                }
-                
-                current_pose[1] = pose;
-            }
-            else if(messageIn.id == 0x60 && pose != current_pose[2])
-            {
-                if(pose == 1)
-                {
-                    motor_3.setSpeed(-SPEED);
-                }
-                else if(pose == 2)
-                {
-                    motor_3.setSpeed(SPEED);
-                }
-                else
-                {
-                    motor_3.setSpeed(0);
-                }
-                
-                current_pose[2] = pose;
-            }
-            else if(messageIn.id == 0x70 && pose != current_pose[3])
-            {
-                if(pose == 1)
-                {
-                    motor_4.setSpeed(-SPEED);
-                }
-                else if(pose == 2)
-                {
-                    motor_4.setSpeed(SPEED);
-                }
-                else
-                {
-                    motor_4.setSpeed(0);
-                }
-                
-                current_pose[3] = pose;
-            }
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor_2.setSpeed(speed);
+          
+          printf("CAN: mess %d\n\r", speed);
         }
+        
+        if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT3_ID))
+        {
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor_3.setSpeed(speed);
+          
+          printf("CAN: mess %d\n\r", speed);
+        }
+        
+        if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT4_ID))
+        {
+          speed = 0;
+          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+          
+          motor_4.setSpeed(speed);
+          
+          printf("CAN: mess %d\n\r", speed);
+        }
+      }
+    
+      wait(0.1);
     }
 }
\ No newline at end of file