main.cpp

00001 #include "mbed.h"
00002 #include "communication_1.h"
00003 #include "MX106.h"
00004 
00005 #define SPEED 50
00006 
00007 // Utility
00008 InterruptIn button(USER_BUTTON);
00009 DigitalOut led(LED1);
00010 
00011 // Motor Control
00012 communication_1 wire(PA_9, PA_10, 57600);
00013 MX106 motor_1(wire, 1, 1);
00014 MX106 motor_2(wire, 2, 1);
00015 MX106 motor_3(wire, 3, 1);
00016 
00017 void button_int_handler()
00018 {
00019 
00020 }
00021 
00022 // CAN
00023 Thread canrxa;
00024 
00025 CAN can1(PA_11, PA_12);     // RX, TX
00026 CANMessage messageIn;
00027 CANMessage messageOut;
00028 
00029 int filter = can1.filter(0x000, 0x400, CANStandard);
00030 
00031 int pose;
00032 int current_pose[] = {0, 0, 0};
00033 int desired_pose[] = {0, 0, 0};
00034 
00035 void canrx()
00036 {
00037   //while(1)
00038   {
00039     if(can1.read(messageIn, filter))
00040     {
00041       pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
00042       
00043       if((messageIn.id & 0x0FF) == 0x40)
00044       {
00045           desired_pose[0] = pose;
00046       }
00047       else if((messageIn.id & 0x0FF) == 0x50)
00048       {
00049           desired_pose[1] = pose;
00050       }
00051       else if((messageIn.id & 0x0FF) == 0x60)
00052       {
00053           desired_pose[2] = pose;
00054       }
00055     }
00056   }
00057 }
00058    
00059 int main()
00060 {
00061    can1.frequency(125000);
00062    
00063    printf("DYNAMIXEL: Init START");
00064    
00065    wire.trigger();
00066    wire.trigger();
00067    wire.trigger();
00068    wire.trigger();
00069    
00070    // Setup Motor1 MultiTurn
00071    motor_1.setMotorEnabled(1);
00072    motor_1.setMode(2);
00073    wait(1);
00074    printf("DYNAMIXEL: Init 1 DONE");
00075    
00076    // Setup Motor2 MultiTurn
00077    motor_2.setMotorEnabled(1);
00078    motor_2.setMode(2);
00079    wait(1);
00080    printf("DYNAMIXEL: Init 2 DONE");
00081    
00082    // Setup Motor3 MultiTurn
00083    motor_3.setMotorEnabled(1);
00084    motor_3.setMode(2);
00085    wait(1);
00086    printf("DYNAMIXEL: Init 3 DONE");
00087 
00088    printf("DYNAMIXEL: Init ALL DONE\n\r");
00089    
00090    button.rise(&button_int_handler);
00091   
00092    // CAN Initialization  
00093    //canrxa.start(canrx);
00094 
00095    printf("CAN: Init DONE\n\r");
00096   
00097    printf("Running!\n\r");
00098   
00099    while(true)
00100    {
00101       canrx();
00102       
00103       if(desired_pose[0] != current_pose[0])
00104       {
00105         if(desired_pose[0] == 1)
00106           motor_1.setSpeed(-SPEED);
00107         else if(desired_pose[0] == 2)
00108           motor_1.setSpeed(SPEED);
00109         else
00110           motor_1.setSpeed(0);
00111           
00112         current_pose[0] = desired_pose[0];
00113       }
00114       
00115       if(desired_pose[1] != current_pose[1])
00116       {
00117         if(desired_pose[1] == 1)
00118           motor_2.setSpeed(-SPEED);
00119         else if(desired_pose[1] == 2)
00120           motor_2.setSpeed(SPEED);
00121         else
00122           motor_2.setSpeed(0);
00123           
00124         current_pose[1] = desired_pose[1];
00125       }
00126       
00127       if(desired_pose[2] != current_pose[2])
00128       {
00129         if(desired_pose[2] == 1)
00130           motor_3.setSpeed(-SPEED);
00131         else if(desired_pose[2] == 2)
00132           motor_3.setSpeed(SPEED);
00133         else
00134           motor_3.setSpeed(0);
00135           
00136         current_pose[2] = desired_pose[2];
00137       }
00138    }
00139 }