File content as of revision 23:47b8c7f9813e:

#include "mbed.h"
#include "communication_1.h"
#include "MX106.h"
#include "AX12.h"
#include <stdio.h>
#include <string.h>
#include "Servo.h"


#define VERBOSE 1

#define CAN_FREQUENCY 125000
#define BAUDRATE  57600
#define SLEEP 0.02


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};
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};
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};
float camera_pose=0;
int present, current, torque, temperature;
int status;

DigitalOut led(LED1);

communication_1 wire(PA_9, PA_10, BAUDRATE);

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]);

CAN can1(PB_5, PB_6);     // RX, TX

CANMessage messageIn;
CANMessage messageOut;



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
  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

  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);

  while (1)
  {
    if(can1.read(messageIn))
     {
       #if VERBOSE
       printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
       #endif
       led!=led;
       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 == 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 == 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 == 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 == gen_can_id(JOINT_MAXTORQUE, WRIST1))
          {
              dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
          }
          if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST2))
           {
               dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
           }
           if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST3))
            {
                dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
            }
            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]));
          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);
          camera=camera_pose;

          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);

          #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 = 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 = 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] = (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



          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= gen_can_id(JOINT_ERROR, joint_id[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
            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
          }
         */
          

      }
}