File content as of revision 15:c1a790a1e999:

#include "mbed.h"
#include "communication_1.h"
#include "MX106.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);

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};
int desired_pose[] = {0, 0, 0};

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);
      
      if((messageIn.id & 0x0FF) == 0x40)
      {
          desired_pose[0] = pose;
      }
      else if((messageIn.id & 0x0FF) == 0x50)
      {
          desired_pose[1] = pose;
      }
      else if((messageIn.id & 0x0FF) == 0x60)
      {
          desired_pose[2] = pose;
      }
    }
  }
}
   
int main()
{
   can1.frequency(125000);
   
   printf("DYNAMIXEL: Init START");
   
   wire.trigger();
   wire.trigger();
   wire.trigger();
   wire.trigger();
   
   // Setup Motor1 MultiTurn
   motor_1.setMotorEnabled(1);
   motor_1.setMode(2);
   wait(1);
   printf("DYNAMIXEL: Init 1 DONE");
   
   // Setup Motor2 MultiTurn
   motor_2.setMotorEnabled(1);
   motor_2.setMode(2);
   wait(1);
   printf("DYNAMIXEL: Init 2 DONE");
   
   // Setup Motor3 MultiTurn
   motor_3.setMotorEnabled(1);
   motor_3.setMode(2);
   wait(1);
   printf("DYNAMIXEL: Init 3 DONE");

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

   printf("CAN: Init DONE\n\r");
  
   printf("Running!\n\r");
  
   while(true)
   {
      canrx();
      
      if(desired_pose[0] != current_pose[0])
      {
        if(desired_pose[0] == 1)
          motor_1.setSpeed(-SPEED);
        else if(desired_pose[0] == 2)
          motor_1.setSpeed(SPEED);
        else
          motor_1.setSpeed(0);
          
        current_pose[0] = desired_pose[0];
      }
      
      if(desired_pose[1] != current_pose[1])
      {
        if(desired_pose[1] == 1)
          motor_2.setSpeed(-SPEED);
        else if(desired_pose[1] == 2)
          motor_2.setSpeed(SPEED);
        else
          motor_2.setSpeed(0);
          
        current_pose[1] = desired_pose[1];
      }
      
      if(desired_pose[2] != current_pose[2])
      {
        if(desired_pose[2] == 1)
          motor_3.setSpeed(-SPEED);
        else if(desired_pose[2] == 2)
          motor_3.setSpeed(SPEED);
        else
          motor_3.setSpeed(0);
          
        current_pose[2] = desired_pose[2];
      }
   }
}