To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp@21:43740448011a, 2019-06-21 (annotated)
- Committer:
- gidiana
- Date:
- Fri Jun 21 07:42:44 2019 +0000
- Revision:
- 21:43740448011a
- Parent:
- 20:ff2dab77e3e9
- Child:
- 22:ffb26af4d5d8
Paolone
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
clynamen | 0:23acabab7c11 | 1 | #include "mbed.h" |
mattiasub | 6:941fdda9d110 | 2 | #include "communication_1.h" |
dconsoli | 4:6450eb95727d | 3 | #include "MX106.h" |
gidiana | 14:c51c4e0f3bc9 | 4 | #include "AX12.h" |
gidiana | 16:5454456b36f7 | 5 | #include "Servo.h" |
stebonicelli | 18:841ab8c1bdbd | 6 | |
stebonicelli | 19:7f8c174448d0 | 7 | #define SPEED 50 |
stebonicelli | 18:841ab8c1bdbd | 8 | |
gidiana | 21:43740448011a | 9 | #define JOINT_SET_SPEED 20 |
gidiana | 21:43740448011a | 10 | |
gidiana | 21:43740448011a | 11 | #define JOINT1_ID 4 |
gidiana | 21:43740448011a | 12 | #define JOINT2_ID 5 |
gidiana | 21:43740448011a | 13 | #define JOINT3_ID 6 |
gidiana | 21:43740448011a | 14 | #define JOINT4_ID 7 |
gidiana | 21:43740448011a | 15 | |
stebonicelli | 13:698bd4df9702 | 16 | // Utility |
stebonicelli | 13:698bd4df9702 | 17 | InterruptIn button(USER_BUTTON); |
stebonicelli | 13:698bd4df9702 | 18 | DigitalOut led(LED1); |
stebonicelli | 13:698bd4df9702 | 19 | |
stebonicelli | 13:698bd4df9702 | 20 | // Motor Control |
gidiana | 17:2ab8feddb7c8 | 21 | communication_1 wire(PA_9, PA_10, 57600); |
stebonicelli | 13:698bd4df9702 | 22 | MX106 motor_1(wire, 1, 1); |
stebonicelli | 13:698bd4df9702 | 23 | MX106 motor_2(wire, 2, 1); |
stebonicelli | 13:698bd4df9702 | 24 | MX106 motor_3(wire, 3, 1); |
gidiana | 14:c51c4e0f3bc9 | 25 | AX12 motor_4(wire, 4, 1); |
stebonicelli | 18:841ab8c1bdbd | 26 | |
stebonicelli | 18:841ab8c1bdbd | 27 | // Camera PanTilt Control |
gidiana | 21:43740448011a | 28 | //Servo cam1 (PC_0); |
gidiana | 21:43740448011a | 29 | //Servo cam2 (PC_1); |
stebonicelli | 18:841ab8c1bdbd | 30 | |
stebonicelli | 13:698bd4df9702 | 31 | void button_int_handler() |
stebonicelli | 13:698bd4df9702 | 32 | { |
stebonicelli | 13:698bd4df9702 | 33 | |
stebonicelli | 13:698bd4df9702 | 34 | } |
stebonicelli | 13:698bd4df9702 | 35 | |
stebonicelli | 13:698bd4df9702 | 36 | // CAN |
gidiana | 17:2ab8feddb7c8 | 37 | //Thread canrxa; |
gidiana | 21:43740448011a | 38 | CAN can1(PB_5, PB_6); // RX, TX |
stebonicelli | 13:698bd4df9702 | 39 | |
stebonicelli | 13:698bd4df9702 | 40 | CANMessage messageIn; |
stebonicelli | 13:698bd4df9702 | 41 | CANMessage messageOut; |
stebonicelli | 13:698bd4df9702 | 42 | |
stebonicelli | 18:841ab8c1bdbd | 43 | int pose; |
stebonicelli | 19:7f8c174448d0 | 44 | int current_pose[] = {0, 0, 0, 0, 0, 0}; |
stebonicelli | 18:841ab8c1bdbd | 45 | |
stebonicelli | 13:698bd4df9702 | 46 | int main() |
stebonicelli | 13:698bd4df9702 | 47 | { |
stebonicelli | 19:7f8c174448d0 | 48 | can1.frequency(125000); |
gidiana | 21:43740448011a | 49 | |
gidiana | 21:43740448011a | 50 | messageIn.format=CANExtended; |
gidiana | 21:43740448011a | 51 | |
stebonicelli | 19:7f8c174448d0 | 52 | printf("CAN: Init DONE\n\r"); |
stebonicelli | 19:7f8c174448d0 | 53 | |
stebonicelli | 19:7f8c174448d0 | 54 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 55 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 56 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 57 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 58 | wait(1); |
gidiana | 17:2ab8feddb7c8 | 59 | |
stebonicelli | 19:7f8c174448d0 | 60 | // Setup Motor1 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 61 | motor_1.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 62 | motor_1.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 63 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 64 | printf("DYNAMIXEL: Init DONE 1\n\r"); |
stebonicelli | 19:7f8c174448d0 | 65 | |
stebonicelli | 19:7f8c174448d0 | 66 | // Setup Motor2 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 67 | motor_2.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 68 | motor_2.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 69 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 70 | printf("DYNAMIXEL: Init DONE 2\n\r"); |
clynamen | 11:19e8022f60ea | 71 | |
stebonicelli | 19:7f8c174448d0 | 72 | // Setup Motor3 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 73 | motor_3.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 74 | motor_3.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 75 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 76 | printf("DYNAMIXEL: Init DONE 3\n\r"); |
stebonicelli | 19:7f8c174448d0 | 77 | |
stebonicelli | 19:7f8c174448d0 | 78 | // Setup Motor4 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 79 | motor_4.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 80 | motor_4.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 81 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 82 | printf("DYNAMIXEL: Init DONE 4\n\r"); |
stebonicelli | 19:7f8c174448d0 | 83 | |
stebonicelli | 19:7f8c174448d0 | 84 | printf("Running!\n\r"); |
gidiana | 21:43740448011a | 85 | |
gidiana | 21:43740448011a | 86 | int32_t speed = 0; |
stebonicelli | 19:7f8c174448d0 | 87 | |
stebonicelli | 19:7f8c174448d0 | 88 | while(true) |
stebonicelli | 19:7f8c174448d0 | 89 | { |
gidiana | 21:43740448011a | 90 | if(can1.read(messageIn)) |
gidiana | 21:43740448011a | 91 | { |
gidiana | 21:43740448011a | 92 | if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT1_ID)) |
gidiana | 21:43740448011a | 93 | { |
gidiana | 21:43740448011a | 94 | speed = 0; |
gidiana | 21:43740448011a | 95 | speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); |
gidiana | 21:43740448011a | 96 | |
gidiana | 21:43740448011a | 97 | motor_1.setSpeed(speed); |
gidiana | 21:43740448011a | 98 | |
gidiana | 21:43740448011a | 99 | printf("CAN: mess %d\n\r", speed); |
gidiana | 21:43740448011a | 100 | } |
gidiana | 21:43740448011a | 101 | |
gidiana | 21:43740448011a | 102 | if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT2_ID)) |
stebonicelli | 19:7f8c174448d0 | 103 | { |
gidiana | 21:43740448011a | 104 | speed = 0; |
gidiana | 21:43740448011a | 105 | speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); |
gidiana | 21:43740448011a | 106 | |
gidiana | 21:43740448011a | 107 | motor_2.setSpeed(speed); |
gidiana | 21:43740448011a | 108 | |
gidiana | 21:43740448011a | 109 | printf("CAN: mess %d\n\r", speed); |
stebonicelli | 19:7f8c174448d0 | 110 | } |
gidiana | 21:43740448011a | 111 | |
gidiana | 21:43740448011a | 112 | if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT3_ID)) |
gidiana | 21:43740448011a | 113 | { |
gidiana | 21:43740448011a | 114 | speed = 0; |
gidiana | 21:43740448011a | 115 | speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); |
gidiana | 21:43740448011a | 116 | |
gidiana | 21:43740448011a | 117 | motor_3.setSpeed(speed); |
gidiana | 21:43740448011a | 118 | |
gidiana | 21:43740448011a | 119 | printf("CAN: mess %d\n\r", speed); |
gidiana | 21:43740448011a | 120 | } |
gidiana | 21:43740448011a | 121 | |
gidiana | 21:43740448011a | 122 | if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT4_ID)) |
gidiana | 21:43740448011a | 123 | { |
gidiana | 21:43740448011a | 124 | speed = 0; |
gidiana | 21:43740448011a | 125 | speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); |
gidiana | 21:43740448011a | 126 | |
gidiana | 21:43740448011a | 127 | motor_4.setSpeed(speed); |
gidiana | 21:43740448011a | 128 | |
gidiana | 21:43740448011a | 129 | printf("CAN: mess %d\n\r", speed); |
gidiana | 21:43740448011a | 130 | } |
gidiana | 21:43740448011a | 131 | } |
gidiana | 21:43740448011a | 132 | |
gidiana | 21:43740448011a | 133 | wait(0.1); |
stebonicelli | 19:7f8c174448d0 | 134 | } |
stebonicelli | 19:7f8c174448d0 | 135 | } |