FOrk
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 21:43740448011a
- Parent:
- 20:ff2dab77e3e9
- Child:
- 22:ffb26af4d5d8
--- a/main.cpp Tue Apr 16 09:02:27 2019 +0000 +++ b/main.cpp Fri Jun 21 07:42:44 2019 +0000 @@ -6,6 +6,13 @@ #define SPEED 50 +#define JOINT_SET_SPEED 20 + +#define JOINT1_ID 4 +#define JOINT2_ID 5 +#define JOINT3_ID 6 +#define JOINT4_ID 7 + // Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); @@ -18,8 +25,8 @@ AX12 motor_4(wire, 4, 1); // Camera PanTilt Control -Servo cam1 (D9); -Servo cam2 (D10); +//Servo cam1 (PC_0); +//Servo cam2 (PC_1); void button_int_handler() { @@ -28,19 +35,20 @@ // CAN //Thread canrxa; -CAN can1(PA_11, PA_12); // RX, TX +CAN can1(PB_5, PB_6); // RX, TX CANMessage messageIn; CANMessage messageOut; -int filter = can1.filter(0x000, 0x400, CANStandard); - int pose; int current_pose[] = {0, 0, 0, 0, 0, 0}; int main() { can1.frequency(125000); + + messageIn.format=CANExtended; + printf("CAN: Init DONE\n\r"); wire.trigger(); @@ -74,81 +82,54 @@ printf("DYNAMIXEL: Init DONE 4\n\r"); printf("Running!\n\r"); + + int32_t speed = 0; while(true) { - if(can1.read(messageIn, filter)) + if(can1.read(messageIn)) + { + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT1_ID)) + { + speed = 0; + speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); + + motor_1.setSpeed(speed); + + printf("CAN: mess %d\n\r", speed); + } + + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT2_ID)) { - pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); - - if(messageIn.id == 0x40 && pose != current_pose[0]) - { - if(pose == 1) - { - motor_1.setSpeed(-SPEED); - } - else if(pose == 2) - { - motor_1.setSpeed(SPEED); - } - else - { - motor_1.setSpeed(0); - } - - current_pose[0] = pose; - } - else if(messageIn.id == 0x50 && pose != current_pose[1]) - { - if(pose == 1) - { - motor_2.setSpeed(SPEED); - } - else if(pose == 2) - { - motor_2.setSpeed(-SPEED); - } - else - { - motor_2.setSpeed(0); - } - - current_pose[1] = pose; - } - else if(messageIn.id == 0x60 && pose != current_pose[2]) - { - if(pose == 1) - { - motor_3.setSpeed(-SPEED); - } - else if(pose == 2) - { - motor_3.setSpeed(SPEED); - } - else - { - motor_3.setSpeed(0); - } - - current_pose[2] = pose; - } - else if(messageIn.id == 0x70 && pose != current_pose[3]) - { - if(pose == 1) - { - motor_4.setSpeed(-SPEED); - } - else if(pose == 2) - { - motor_4.setSpeed(SPEED); - } - else - { - motor_4.setSpeed(0); - } - - current_pose[3] = pose; - } + speed = 0; + speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); + + motor_2.setSpeed(speed); + + printf("CAN: mess %d\n\r", speed); } + + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT3_ID)) + { + speed = 0; + speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); + + motor_3.setSpeed(speed); + + printf("CAN: mess %d\n\r", speed); + } + + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT4_ID)) + { + speed = 0; + speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); + + motor_4.setSpeed(speed); + + printf("CAN: mess %d\n\r", speed); + } + } + + wait(0.1); } } \ No newline at end of file