Dynamixel controller with CAN interface
Dependencies: AX12_final MX106_not_working comunication_1
main.cpp
- Committer:
- stebonicelli
- Date:
- 2019-04-16
- Revision:
- 15:c1a790a1e999
- Parent:
- 13:698bd4df9702
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]; } } }