File content as of revision 21:43740448011a:

#include "mbed.h"
#include "communication_1.h"
#include "MX106.h"
#include "AX12.h"
#include "Servo.h"

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

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

// Camera PanTilt Control
//Servo cam1 (PC_0);
//Servo cam2 (PC_1);

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()
{
    can1.frequency(125000);
    
    messageIn.format=CANExtended;
    
    printf("CAN: Init DONE\n\r");

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

    // Setup Motor2 MultiTurn
    motor_2.setMotorEnabled(1);
    motor_2.setMode(0);
    wait(3);
    printf("DYNAMIXEL: Init DONE 2\n\r");

    // Setup Motor3 MultiTurn
    motor_3.setMotorEnabled(1);
    motor_3.setMode(0);
    wait(3);
    printf("DYNAMIXEL: Init DONE 3\n\r");

    // Setup Motor4 MultiTurn
    motor_4.setMotorEnabled(1);
    motor_4.setMode(0);
    wait(3);
    printf("DYNAMIXEL: Init DONE 4\n\r");

    printf("Running!\n\r");
    
    int32_t speed = 0;

    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);
        }
      }
    
      wait(0.1);
    }
}