File content as of revision 16:5454456b36f7:

#include "mbed.h"
#include "communication_1.h"
#include "MX106.h"
#include "AX12.h"
#include "Servo.h"
#define SPEED 100
// Utility
InterruptIn button(USER_BUTTON);
DigitalOut led(LED1);

// Motor Control
Serial pc(USBTX, USBRX);
communication_1 wire(PA_9, PA_10, 200000);
MX106 motor_1(wire, 1, 1);
MX106 motor_2(wire, 2, 1);
MX106 motor_3(wire, 3, 1);
AX12  motor_4(wire, 4, 1);
Servo cam1 (PB_12);
Servo cam2 (PB_13);
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;
void canrx()
{
  while(1)
  {
    if(can1.read(messageIn, filter))
    {
      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
      printf("CAN: mess %d\n\r", pose);
      printf("CAN: id %x \n\r ",messageIn.id);
      
      if((messageIn.id & 0x0FF) == 0x40)
      {
            if (pose==0)
            {
                motor_1.setSpeed(-SPEED);
                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
            }
            
            else if (pose==50)
            {
                motor_1.setSpeed(0);
                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
            }
            else if (pose==100)
            {
               
                motor_1.setSpeed(SPEED);
                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
                
            }
            else  
                motor_1.setSpeed(0);        
      }
      else if((messageIn.id & 0x0FF) == 0x50)
      {
            if (pose==0)
            {
                motor_2.setSpeed(-SPEED);
                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
                if (motor_2.getPosition()<-90) motor_2.setSpeed(0);
            }
            
            else if (pose==50)
            {
                motor_2.setSpeed(0);
                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
            }
            else if (pose==100)
            {
               
                motor_2.setSpeed(SPEED);
                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
                if (motor_2.getPosition()>90) motor_2.setSpeed(0);
            }
            else 
                motor_1.setSpeed(0); 
       }
      else if((messageIn.id & 0x0FF) == 0x60)
      {
         
            if (pose==0)
            {
                motor_3.setSpeed(-SPEED);
                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
            }
            
            else if (pose==50)
            {
                motor_3.setSpeed(0);
                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
            }
            else if (pose==100)
            {
               
                motor_3.setSpeed(SPEED);
                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
                
            }
            else 
                motor_3.setSpeed(0); 
      }
      else if((messageIn.id & 0x0FF) == 0x70)
      {
          
            if (pose==0)
            {
                motor_4.setSpeed(-SPEED);
                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
            }
            
            else if (pose==50)
            {
                motor_1.setSpeed(0);
                printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition());
            }
            else if (pose==100)
            {
               
                motor_1.setSpeed(SPEED);
                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());    
            }
            else 
                motor_4.setSpeed(0); 
    }
        else if((messageIn.id & 0x0FF) == 0x80)
        {
            if (pose==0)
            {
                for(float p=0; p<1.0; p -= 0.1) 
                {
                cam1 = p;
                wait(0.2);
                }
            }
            else if (pose==100)
            {
                for(float p=0; p<1.0; p += 0.1) 
                {
                cam1 = p;
                wait(0.2);
                }
            } 
        }
         else if((messageIn.id & 0x0FF) == 0x90)
        {
            if (pose==0)
            {
                for(float p=0; p<1.0; p -= 0.1) 
                {
                cam2 = p;
                wait(0.2);
                }
          }
            else if (pose==100)
            {
                for(float p=0; p<1.0; p += 0.1) 
                {
                cam2 = p;
                wait(0.2);
                }
            }
                     
      }
    }
  }
}
   
int main()
{
   wire.trigger();
   wire.trigger();
   wire.trigger();
   wire.trigger();
   
   // Setup Motor1 MultiTurn
   motor_1.setMotorEnabled(1);
   motor_1.setMode(0);
   motor_1.setSpeed(0);
   printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition());
   //motor_1.setGoalPosition(0);
   wait(10);
   
   // Setup Motor2 MultiTurn
   motor_2.setMotorEnabled(1);
   motor_2.setMode(0);
   motor_2.setSpeed(0);
   printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition());
   //motor_2.setGoalPosition(0);
   wait(10);
   
   // Setup Motor3 MultiTurn
   motor_3.setMotorEnabled(1);
   motor_3.setMode(0);
   motor_3.setSpeed(0);
   printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
   wait(10);
   //motor_3.setGoalPosition(0);
   // Setup Motor4 MultiTurn
   motor_4.setMotorEnabled(1);
   motor_4.setMode(0);
   motor_4.setSpeed(0);
   printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition());
   wait(10);

   printf("DYNAMIXEL: Init DONE\n\r");
   
   button.rise(&button_int_handler);
  
   // CAN Initialization  
   canrxa.start(canrx);

   printf("DONE: CAN Init\n\r");
  
  
   printf("Running!\n\r");
  
   while(true)
   {
      wait(1000);
   }
}