Revision 22:ffb26af4d5d8, committed 2019-07-29

Comitter:

gidiana

Date:

Mon Jul 29 22:34:34 2019 +0000

Parent:

21:43740448011a

Commit message:

refactor complete

Changed in this revision

--- a/MX.cpp	Fri Jun 21 07:42:44 2019 +0000
+++ b/MX.cpp	Mon Jul 29 22:34:34 2019 +0000
@@ -274,6 +274,7 @@
 
     // write the packet, return the error code
     _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
+    
 }
 
 //void MX::setCRSpeed(float speed) {
@@ -301,3 +302,10 @@
     float temp = data[0];
     return (temp);
 }
+float MX::getTorqueMax(void) {
+    char data [1];
+    int ErrorCode = _line.read(_ID, MX_REG_MAXTORQUE, 1, data);
+    float limit = data[0];
+    return limit;
+    
+}

--- a/MX.h	Fri Jun 21 07:42:44 2019 +0000
+++ b/MX.h	Mon Jul 29 22:34:34 2019 +0000
@@ -110,6 +110,7 @@
     float getTemp();
     float getCurrent();
     float getVolts();
+    float getTorqueMax();
     virtual float getPGain();
     virtual float getIGain();
     virtual float getDGain();

--- a/main.cpp	Fri Jun 21 07:42:44 2019 +0000
+++ b/main.cpp	Mon Jul 29 22:34:34 2019 +0000
@@ -2,134 +2,346 @@
 #include "communication_1.h"
 #include "MX106.h"
 #include "AX12.h"
-#include "Servo.h"
+#include <stdio.h>
+#include <string.h>
+
+
+#define VERBOSE 1
 
-#define SPEED 50
-
-#define JOINT_SET_SPEED 20
-
+#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
 
-// Utility
-InterruptIn button(USER_BUTTON);
+
+int dxl_id[]={1,2,3,4};
+int joint_id[]={4,5,6,7};
+int dxl_speed[]={50,25,50,50};
+float dxl_gear[]={3,2,3,1};
+float dxl_present_position[] = {0,0,0,0};
+float dxl_goal_position[] = {0,0,0,0};
+float dxl_offset[] = {0,0,0,0};
+float dxl_current[] = {0,0,0,0};
+float dxl_torque[] = {0,0,0,0};
+float dxl_max_torque[] = {0,0,0,0};
+float dxl_reset[]={0,0,0,0};
+uint16_t dxl_temperature[]={0,0,0,0};
+uint16_t dxl_limit[]={0,0,0,0};
+int present, current, torque, temperature;
+int status;
+
 DigitalOut led(LED1);
 
-// Motor Control
-communication_1 wire(PA_9, PA_10, 57600);
-MX106 motor_1(wire, 1, 1);
-MX106 motor_2(wire, 2, 1);
-MX106 motor_3(wire, 3, 1);
-AX12  motor_4(wire, 4, 1);
+communication_1 wire(PA_9, PA_10, BAUDRATE);
 
-// Camera PanTilt Control
-//Servo cam1 (PC_0);
-//Servo cam2 (PC_1);
+MX106 w_1(wire, dxl_id[0], dxl_gear[0]);
+MX106 w_2(wire, dxl_id[1], dxl_gear[1]);
+MX106 w_3(wire, dxl_id[2], dxl_gear[2]);
+AX12  a_1(wire, dxl_id[3], dxl_gear[3]);
 
-void button_int_handler()
-{
-
-}
-
-// CAN
-//Thread canrxa;
 CAN can1(PB_5, PB_6);     // RX, TX
 
 CANMessage messageIn;
 CANMessage messageOut;
 
-int pose;
-int current_pose[] = {0, 0, 0, 0, 0, 0};
+
 
-int main()
+int main ()
 {
-    can1.frequency(125000);
-    
-    messageIn.format=CANExtended;
-    
-    printf("CAN: Init DONE\n\r");
+  #if VERBOSE
+  printf("START \n\r");
+  #endif
+  can1.frequency(CAN_FREQUENCY);
+  messageIn.format=CANExtended;
+  messageOut.format=CANExtended;
+
+  wire.trigger();
+  wire.trigger();
+  wire.trigger();
+  wire.trigger();
+
+  #if VERBOSE
+  printf("DYNAMIXEL: Init START \n\r");
+  #endif
+  w_1.setMotorEnabled(1);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL: Init 1 \n\r");
+  #endif
+  w_2.setMotorEnabled(1);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL: Init 2 \n\r");
+  #endif
+  w_3.setMotorEnabled(1);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL: Init 3\n\r");
+   #endif
+  a_1.setMotorEnabled(1);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL: Init 4\n\r");
+  #endif
 
-    wire.trigger();
-    wire.trigger();
-    wire.trigger();
-    wire.trigger();
-    wait(1);
-    
-    // Setup Motor1 MultiTurn
-    motor_1.setMotorEnabled(1);
-    motor_1.setMode(0);
-    wait(3);
-    printf("DYNAMIXEL: Init DONE 1\n\r");
+  w_1.setMode(2);
+  wait(SLEEP);
+  w_1.setMaxSpeed(dxl_speed[0]);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL 1: SET MODE\n\r");
+  #endif
+  w_2.setMode(2);
+  wait(SLEEP);
+  w_2.setMaxSpeed(dxl_speed[1]);
+  wait(SLEEP);
+  w_2.setCWLimitUnits(1900);
+  wait(SLEEP);
+  w_2.setCCWLimitUnits(-1900);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL 2: SET MODE\n\r");
+  #endif
+  w_3.setMode(2);
+  wait(SLEEP);
+  w_3.setMaxSpeed(dxl_speed[2]);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL 3: SET MODE\n\r");
+  #endif
+  a_1.setMode(1);
+  wait(SLEEP);
+  a_1.setMaxSpeed(dxl_speed[3]);
+  wait(SLEEP);
+  #if VERBOSE
+  printf("DYNAMIXEL 4: SET MODE\n\r");
+  #endif
+
+  w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+  wait(SLEEP);
+  w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
+  wait(SLEEP);
+  w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
+  wait(SLEEP);
+  a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
+  wait(SLEEP);
+
+  wait(1);
 
-    // Setup Motor2 MultiTurn
-    motor_2.setMotorEnabled(1);
-    motor_2.setMode(0);
-    wait(3);
-    printf("DYNAMIXEL: Init DONE 2\n\r");
+  while (1)
+  {
+    if(can1.read(messageIn))
+     {
+       #if VERBOSE
+       printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
+       #endif
+       led!=led;
+       if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT1_ID))
+        {
+           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))
+         {
+            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))
+         {
+             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))
+         {
+             dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+         }
 
-    // Setup Motor3 MultiTurn
-    motor_3.setMotorEnabled(1);
-    motor_3.setMode(0);
-    wait(3);
-    printf("DYNAMIXEL: Init DONE 3\n\r");
+         if(messageIn.id == ((JOINT_ERROR << 8) + JOINT1_ID))
+          {
+              dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+          }
+          if(messageIn.id == ((JOINT_ERROR << 8) + JOINT2_ID))
+           {
+               dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+           }
+           if(messageIn.id == ((JOINT_ERROR << 8) + JOINT3_ID))
+            {
+                dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+            }
+            if(messageIn.id == ((JOINT_ERROR << 8) + JOINT4_ID))
+             {
+                 dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+             }
+        }
 
-    // Setup Motor4 MultiTurn
-    motor_4.setMotorEnabled(1);
-    motor_4.setMode(0);
-    wait(3);
-    printf("DYNAMIXEL: Init DONE 4\n\r");
+          w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
+          wait(SLEEP);
+          w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
+          wait(SLEEP);
+          w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
+          wait(SLEEP);
+          a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
+          wait(SLEEP);
 
-    printf("Running!\n\r");
-    
-    int32_t speed = 0;
+          dxl_present_position[0]=w_1.getPosition();
+          wait(SLEEP);
+          dxl_present_position[1]=w_2.getPosition();
+          wait(SLEEP);
+          dxl_present_position[2]=w_3.getPosition();
+          wait(SLEEP);
+          dxl_present_position[3]=a_1.getPosition();
+          wait(SLEEP);
 
-    while(true)
-    {
-      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))
-        {
-          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);
-        }
+          #if VERBOSE
+          printf("DYNAMIXEL PRESENT POSITION %f %f %f %f\n\r", dxl_present_position[0],dxl_present_position[1], dxl_present_position[2] ,dxl_present_position[3] );
+          #endif
+
+          dxl_current[0]=w_1.getCurrent();
+          wait(SLEEP);
+          dxl_current[1]=w_2.getCurrent();
+          wait(SLEEP);
+          dxl_current[2]=w_3.getCurrent();
+          wait(SLEEP);
+          //dxl_current[3]=a_1.getCurrent();
+          //wait(SLEEP);
+          #if VERBOSE
+          printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] );
+           #endif
+
+          dxl_torque[0]=dxl_current[0]*0.875;
+          dxl_torque[1]=dxl_current[1]*0.875;
+          dxl_torque[2]=dxl_current[2]*0.875;
+          //dxl_torque[3]=dxl_current[3]*0.875;
+          #if VERBOSE
+          printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] );
+          #endif
+
+          dxl_temperature[0]=w_1.getTemp();
+          wait(SLEEP);
+          dxl_temperature[1]=w_2.getTemp();
+          wait(SLEEP);
+          dxl_temperature[2]=w_3.getTemp();
+          wait(SLEEP);
+          dxl_temperature[3]=a_1.getTemp();
+          wait(SLEEP);
+
+          #if VERBOSE
+          printf("DYNAMIXEL TEMPERATURE %d %d %d %d\n\r", dxl_temperature[0],dxl_temperature[1], dxl_temperature[2] ,dxl_temperature[3] );
+          #endif
+
+          for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++)
+          {
+            messageOut.len = 8;
+            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.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);
+
+            status = can1.write(messageOut);
+
+            #if VERBOSE
+            printf("CAN send status Joint %d : %d", joint_id[i] , status );
+            #endif
+          }
+
+
+
+          dxl_max_torque[0]=w_1.getTorqueMax();
+          wait(SLEEP);
+          dxl_max_torque[1]=w_2.getTorqueMax();
+          wait(SLEEP);
+          dxl_max_torque[2]=w_3.getTorqueMax();
+          wait(SLEEP);
+          dxl_max_torque[3]=a_1.getTorqueMax();
+          wait(SLEEP);
+
+          for (int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++ )
+          {
+
+            messageOut.len = 4;
+            messageOut.id= ( JOINT_ERROR << 8 ) + joint_id[i];
+
+            messageOut.data[0]=(((int)(dxl_max_torque[i]))>>8);
+            messageOut.data[1]= ((int)(dxl_max_torque[i]));
+            status = can1.write(messageOut);
+
+            #if VERBOSE
+            printf("CAN send status ERROR Joint %d : %d", joint_id[i] , status );
+             #endif
+
+          }
+          if(dxl_reset[0]!=0)
+          {
+            w_1.setMotorEnabled(0);
+            wait(SLEEP);
+            w_1.setMaxTorque(1);
+            wait(SLEEP);
+            w_1.setMotorEnabled(1);
+            wait(SLEEP);
+            w_1.setGoal(0-dxl_offset[0]);
+            wait(SLEEP);
+            #if VERBOSE
+            printf("Dynamixel %d : RESET", joint_id[0] );
+             #endif
+          }
+          if(dxl_reset[1]!=0)
+          {
+            w_2.setMotorEnabled(0);
+            wait(SLEEP);
+            w_2.setMaxTorque(1);
+            wait(SLEEP);
+            w_2.setMotorEnabled(1);
+            wait(SLEEP);
+            w_2.setGoal(0-dxl_offset[1]);
+            wait(SLEEP);
+            #if VERBOSE
+            printf("Dynamixel %d : RESET", joint_id[1] );
+             #endif
+          }
+          if(dxl_reset[2]!=0)
+          {
+            w_3.setMotorEnabled(0);
+            wait(SLEEP);
+            w_3.setMaxTorque(1);
+            wait(SLEEP);
+            w_3.setMotorEnabled(1);
+            wait(SLEEP);
+            w_3.setGoal(0-dxl_offset[2]);
+            wait(SLEEP);
+            #if VERBOSE
+            printf("Dynamixel %d : RESET", joint_id[2] );
+             #endif
+          }
+          if(dxl_reset[3]!=0)
+          {
+            a_1.setMotorEnabled(0);
+            wait(SLEEP);
+            a_1.setMaxTorque(1);
+            wait(SLEEP);
+            a_1.setMotorEnabled(1);
+            wait(SLEEP);
+            a_1.setGoal(0-dxl_offset[3]);
+            wait(SLEEP);
+            #if VERBOSE
+            printf("Dynamixel %d : RESET", joint_id[3] );
+             #endif
+          }
+
+
       }
-    
-      wait(0.1);
-    }
-}
\ No newline at end of file
+      return 0;
+}