Dynamixel controller with CAN interface
Dependencies: AX12_final MX106_not_working comunication_1
Revision 15:c1a790a1e999, committed 2019-04-16
- Comitter:
- stebonicelli
- Date:
- Tue Apr 16 08:58:02 2019 +0000
- Parent:
- 14:a3c4c15b8e6b
- Commit message:
- using raw serial
Changed in this revision
communication_1.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/communication_1.lib Wed Mar 27 08:25:05 2019 +0000 +++ b/communication_1.lib Tue Apr 16 08:58:02 2019 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/Team-DIANA/code/comunication_1/#a9648ebd6ab7 +http://developer.mbed.org/teams/Team-DIANA/code/comunication_1/#25f27fd1596e
--- a/main.cpp Wed Mar 27 08:25:05 2019 +0000 +++ b/main.cpp Tue Apr 16 08:58:02 2019 +0000 @@ -2,13 +2,14 @@ #include "communication_1.h" #include "MX106.h" +#define SPEED 50 + // Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); // Motor Control -Serial pc(USBTX, USBRX); -communication_1 wire(PA_9, PA_10, 200000); +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); @@ -22,36 +23,34 @@ 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) + //while(1) { if(can1.read(messageIn, filter)) { - printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))); - printf("CANaacc: id %x \n\r ",messageIn.id); + 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) { - - } - else if((messageIn.id & 0x0FF) == 0x70) - { - + desired_pose[2] = pose; } } } @@ -59,6 +58,10 @@ int main() { + can1.frequency(125000); + + printf("DYNAMIXEL: Init START"); + wire.trigger(); wire.trigger(); wire.trigger(); @@ -67,38 +70,70 @@ // Setup Motor1 MultiTurn motor_1.setMotorEnabled(1); motor_1.setMode(2); - motor_1.setSpeed(90); - //motor_1.setGoalPosition(0); - wait(10); + wait(1); + printf("DYNAMIXEL: Init 1 DONE"); // Setup Motor2 MultiTurn motor_2.setMotorEnabled(1); motor_2.setMode(2); - motor_2.setSpeed(90); - //motor_2.setGoalPosition(0); - wait(10); + wait(1); + printf("DYNAMIXEL: Init 2 DONE"); // Setup Motor3 MultiTurn motor_3.setMotorEnabled(1); motor_3.setMode(2); - motor_3.setSpeed(90); - //motor_3.setGoalPosition(0); - wait(10); + wait(1); + printf("DYNAMIXEL: Init 3 DONE"); - printf("DYNAMIXEL: Init DONE\n\r"); + printf("DYNAMIXEL: Init ALL DONE\n\r"); button.rise(&button_int_handler); // CAN Initialization - canrxa.start(canrx); + //canrxa.start(canrx); - printf("DONE: CAN Init\n\r"); - + printf("CAN: Init DONE\n\r"); printf("Running!\n\r"); while(true) { - wait(1000); + 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]; + } } }