File content as of revision 20:ff2dab77e3e9:

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

#define SPEED 50

// 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 (D9);
Servo cam2 (D10);

void button_int_handler()
{

}

// CAN
//Thread canrxa;
CAN can1(PA_11, PA_12);     // 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);
    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");

    while(true)
    {
        if(can1.read(messageIn, filter))
        {
            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;
            }
        }
    }
}