Diff: main.cpp

Revision:

23:47b8c7f9813e

Parent:

22:ffb26af4d5d8

Child:

24:adb6bac314d7

--- a/main.cpp	Mon Jul 29 22:34:34 2019 +0000
+++ b/main.cpp	Wed Aug 07 12:54:09 2019 +0000
@@ -4,6 +4,7 @@
 #include "AX12.h"
 #include <stdio.h>
 #include <string.h>
+#include "Servo.h"
 
 
 #define VERBOSE 1
@@ -11,17 +12,36 @@
 #define CAN_FREQUENCY 125000
 #define BAUDRATE  57600
 #define SLEEP 0.02
-#define JOINT_SET_POSITION 20
-#define JOINT_STATUS 30
-#define JOINT_ERROR 40
-#define JOINT1_ID 4
-#define JOINT2_ID 5
-#define JOINT3_ID 6
-#define JOINT4_ID 7
 
 
+typedef enum
+{
+  JOINT_SET_SPEED = 20,
+  JOINT_SET_POSITION,
+  JOINT_CURRENT_POSITION,
+  JOINT_CURRENT_SPEED,
+  JOINT_STATUS,
+  JOINT_ERROR,
+  JOINT_TORQUE,
+  JOINT_MAXTORQUE,
+  JOINT_ZERO,
+}CAN_COMMANDS;
+
+typedef enum
+{
+  BASE=1,
+  SHOULDER,
+  ELBOW,
+  WRIST1,
+  WRIST2,
+  WRIST3,
+  END_EFFECTOR,
+  CAMERA1,
+  CAMERA2,
+}JOINT;
+Servo camera(PA_5);
 int dxl_id[]={1,2,3,4};
-int joint_id[]={4,5,6,7};
+JOINT joint_id[]={WRIST1,WRIST2,WRIST3,CAMERA1,CAMERA2};
 int dxl_speed[]={50,25,50,50};
 float dxl_gear[]={3,2,3,1};
 float dxl_present_position[] = {0,0,0,0};
@@ -33,6 +53,7 @@
 float dxl_reset[]={0,0,0,0};
 uint16_t dxl_temperature[]={0,0,0,0};
 uint16_t dxl_limit[]={0,0,0,0};
+float camera_pose=0;
 int present, current, torque, temperature;
 int status;
 
@@ -52,6 +73,16 @@
 
 
 
+uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
+{
+  uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
+  id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
+  id |= 0x80000000;              // Send in Extended Frame Format.
+  return id;
+}
+
+
+
 int main ()
 {
   #if VERBOSE
@@ -142,39 +173,83 @@
        printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
        #endif
        led!=led;
-       if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT1_ID))
+       if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST1))
         {
            dxl_goal_position[0] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
         }
-        if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT2_ID))
+        if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST2))
          {
             dxl_goal_position[1] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
          }
-        if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT3_ID))
+        if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST3))
          {
              dxl_goal_position[2] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
          }
-        if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT4_ID))
+        if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA1))
          {
              dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
          }
+         if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA2))
+          {
+              camera_pose = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3])/100;
+          }
 
-         if(messageIn.id == ((JOINT_ERROR << 8) + JOINT1_ID))
+         if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST1))
           {
               dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
           }
-          if(messageIn.id == ((JOINT_ERROR << 8) + JOINT2_ID))
+          if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST2))
            {
                dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
            }
-           if(messageIn.id == ((JOINT_ERROR << 8) + JOINT3_ID))
+           if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST3))
             {
                 dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
             }
-            if(messageIn.id == ((JOINT_ERROR << 8) + JOINT4_ID))
+            if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, CAMERA1))
              {
                  dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
              }
+
+           if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST1))
+           {
+             if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+             {
+             w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+             wait(SLEEP);
+             }
+           }
+           if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST2))
+           {
+             if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+             {
+             w_2.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+             wait(SLEEP);
+             }
+           }
+           if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST3))
+           {
+             if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+             {
+             w_3.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+             wait(SLEEP);
+             }
+           }
+           if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA1))
+           {
+             if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+             {
+             a_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+             wait(SLEEP);
+             }
+           }
+           if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA2))
+           {
+             if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+             {
+              camera=0;
+             }
+           }
         }
 
           w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
@@ -185,6 +260,7 @@
           wait(SLEEP);
           a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
           wait(SLEEP);
+          camera=camera_pose;
 
           dxl_present_position[0]=w_1.getPosition();
           wait(SLEEP);
@@ -234,29 +310,60 @@
 
           for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++)
           {
-            messageOut.len = 8;
+            messageOut.len = 4;
             present=(int)(dxl_present_position[i]*100);
             current=(int)(dxl_current[i]*100);
             torque=(int)(dxl_torque[i]*100);
             temperature=(int)(dxl_temperature[i]*100);
 
-            messageOut.id = (JOINT_STATUS << 8) + joint_id[i];
+            messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, joint_id[i]);
+
+            messageOut.data[0] = (present >> 24);
+            messageOut.data[1] = (present>>16);
+            messageOut.data[2] = (present>>8);
+            messageOut.data[3] = (present);
+            status = can1.write(messageOut);
+            #if VERBOSE
+            printf("CAN send present position Joint %d : %d", joint_id[i] , status );
+            #endif
+
+            messageOut.id = gen_can_id(JOINT_STATUS, joint_id[i]);
 
-            messageOut.data[0] = (present >> 8);
-            messageOut.data[1] = (present);
-            messageOut.data[2] = (current >> 8);
-            messageOut.data[3] = (current);
-            messageOut.data[4] = (torque >> 8);
-            messageOut.data[5] = (torque);
-            messageOut.data[6] = (temperature >> 8);
-            messageOut.data[7] = (temperature);
+            messageOut.data[0] = (current >> 8);
+            messageOut.data[1] = (current);
+            messageOut.data[2] = (temperature >> 8);
+            messageOut.data[3] = (temperature);
+            status = can1.write(messageOut);
 
+            #if VERBOSE
+            printf("CAN send status Joint %d : %d", joint_id[i] , status );
+            #endif
+            messageOut.id = gen_can_id(JOINT_TORQUE, joint_id[i]);
+
+            messageOut.data[0] = (torque >> 24);
+            messageOut.data[1] = (torque >> 16);
+            messageOut.data[2] = (torque >>  8);
+            messageOut.data[3] = (torque);
             status = can1.write(messageOut);
 
             #if VERBOSE
             printf("CAN send status Joint %d : %d", joint_id[i] , status );
             #endif
           }
+          int _camera_pose = camera_pose*100;
+          messageOut.len = 4;
+          messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, CAMERA2);
+          messageOut.data[0] = (_camera_pose >> 24);
+          messageOut.data[1] = (_camera_pose >> 16);
+          messageOut.data[2] = (_camera_pose >>  8);
+          messageOut.data[3] = (_camera_pose);
+          status = can1.write(messageOut);
+
+          #if VERBOSE
+
+          printf("CAN send present position Joint %d : %d", CAMERA2 , status );
+
+          #endif
 
 
 
@@ -273,10 +380,12 @@
           {
 
             messageOut.len = 4;
-            messageOut.id= ( JOINT_ERROR << 8 ) + joint_id[i];
+            messageOut.id= gen_can_id(JOINT_ERROR, joint_id[i]);
 
-            messageOut.data[0]=(((int)(dxl_max_torque[i]))>>8);
-            messageOut.data[1]= ((int)(dxl_max_torque[i]));
+            messageOut.data[0]= ((int)(dxl_max_torque[i]) >> 24);
+            messageOut.data[1]= ((int)(dxl_max_torque[i]) >> 16);
+            messageOut.data[2]= ((int)(dxl_max_torque[i]) >>  8);
+            messageOut.data[3]= ((int)(dxl_max_torque[i])      );
             status = can1.write(messageOut);
 
             #if VERBOSE
@@ -326,6 +435,7 @@
             printf("Dynamixel %d : RESET", joint_id[2] );
              #endif
           }
+          /*
           if(dxl_reset[3]!=0)
           {
             a_1.setMotorEnabled(0);
@@ -340,8 +450,8 @@
             printf("Dynamixel %d : RESET", joint_id[3] );
              #endif
           }
-
+         */
+          
 
       }
-      return 0;
 }