To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp@19:7f8c174448d0, 2019-04-16 (annotated)
- Committer:
- stebonicelli
- Date:
- Tue Apr 16 08:59:06 2019 +0000
- Revision:
- 19:7f8c174448d0
- Parent:
- 18:841ab8c1bdbd
- Child:
- 20:ff2dab77e3e9
procedural structure
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 | |
stebonicelli | 13:698bd4df9702 | 9 | // Utility |
stebonicelli | 13:698bd4df9702 | 10 | InterruptIn button(USER_BUTTON); |
stebonicelli | 13:698bd4df9702 | 11 | DigitalOut led(LED1); |
stebonicelli | 13:698bd4df9702 | 12 | |
stebonicelli | 13:698bd4df9702 | 13 | // Motor Control |
gidiana | 17:2ab8feddb7c8 | 14 | communication_1 wire(PA_9, PA_10, 57600); |
stebonicelli | 13:698bd4df9702 | 15 | MX106 motor_1(wire, 1, 1); |
stebonicelli | 13:698bd4df9702 | 16 | MX106 motor_2(wire, 2, 1); |
stebonicelli | 13:698bd4df9702 | 17 | MX106 motor_3(wire, 3, 1); |
gidiana | 14:c51c4e0f3bc9 | 18 | AX12 motor_4(wire, 4, 1); |
stebonicelli | 18:841ab8c1bdbd | 19 | |
stebonicelli | 18:841ab8c1bdbd | 20 | // Camera PanTilt Control |
gidiana | 17:2ab8feddb7c8 | 21 | Servo cam1 (D9); |
gidiana | 17:2ab8feddb7c8 | 22 | Servo cam2 (D10); |
stebonicelli | 18:841ab8c1bdbd | 23 | |
stebonicelli | 13:698bd4df9702 | 24 | void button_int_handler() |
stebonicelli | 13:698bd4df9702 | 25 | { |
stebonicelli | 13:698bd4df9702 | 26 | |
stebonicelli | 13:698bd4df9702 | 27 | } |
stebonicelli | 13:698bd4df9702 | 28 | |
stebonicelli | 13:698bd4df9702 | 29 | // CAN |
gidiana | 17:2ab8feddb7c8 | 30 | //Thread canrxa; |
stebonicelli | 13:698bd4df9702 | 31 | CAN can1(PA_11, PA_12); // RX, TX |
stebonicelli | 13:698bd4df9702 | 32 | |
stebonicelli | 13:698bd4df9702 | 33 | CANMessage messageIn; |
stebonicelli | 13:698bd4df9702 | 34 | CANMessage messageOut; |
stebonicelli | 13:698bd4df9702 | 35 | |
stebonicelli | 13:698bd4df9702 | 36 | int filter = can1.filter(0x000, 0x400, CANStandard); |
stebonicelli | 18:841ab8c1bdbd | 37 | |
stebonicelli | 18:841ab8c1bdbd | 38 | int pose; |
stebonicelli | 19:7f8c174448d0 | 39 | int current_pose[] = {0, 0, 0, 0, 0, 0}; |
stebonicelli | 18:841ab8c1bdbd | 40 | |
stebonicelli | 13:698bd4df9702 | 41 | int main() |
stebonicelli | 13:698bd4df9702 | 42 | { |
stebonicelli | 19:7f8c174448d0 | 43 | can1.frequency(125000); |
stebonicelli | 19:7f8c174448d0 | 44 | printf("CAN: Init DONE\n\r"); |
stebonicelli | 19:7f8c174448d0 | 45 | |
stebonicelli | 19:7f8c174448d0 | 46 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 47 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 48 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 49 | wire.trigger(); |
stebonicelli | 19:7f8c174448d0 | 50 | wait(1); |
gidiana | 17:2ab8feddb7c8 | 51 | |
stebonicelli | 19:7f8c174448d0 | 52 | // Setup Motor1 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 53 | motor_1.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 54 | motor_1.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 55 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 56 | printf("DYNAMIXEL: Init DONE 1\n\r"); |
stebonicelli | 19:7f8c174448d0 | 57 | |
stebonicelli | 19:7f8c174448d0 | 58 | // Setup Motor2 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 59 | motor_2.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 60 | motor_2.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 61 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 62 | printf("DYNAMIXEL: Init DONE 2\n\r"); |
clynamen | 11:19e8022f60ea | 63 | |
stebonicelli | 19:7f8c174448d0 | 64 | // Setup Motor3 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 65 | motor_3.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 66 | motor_3.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 67 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 68 | printf("DYNAMIXEL: Init DONE 3\n\r"); |
stebonicelli | 19:7f8c174448d0 | 69 | |
stebonicelli | 19:7f8c174448d0 | 70 | // Setup Motor4 MultiTurn |
stebonicelli | 19:7f8c174448d0 | 71 | motor_4.setMotorEnabled(1); |
stebonicelli | 19:7f8c174448d0 | 72 | motor_4.setMode(0); |
stebonicelli | 19:7f8c174448d0 | 73 | wait(3); |
stebonicelli | 19:7f8c174448d0 | 74 | printf("DYNAMIXEL: Init DONE 4\n\r"); |
stebonicelli | 19:7f8c174448d0 | 75 | |
stebonicelli | 19:7f8c174448d0 | 76 | printf("Running!\n\r"); |
stebonicelli | 19:7f8c174448d0 | 77 | |
stebonicelli | 19:7f8c174448d0 | 78 | while(true) |
stebonicelli | 19:7f8c174448d0 | 79 | { |
stebonicelli | 19:7f8c174448d0 | 80 | if(can1.read(messageIn, filter)) |
stebonicelli | 19:7f8c174448d0 | 81 | { |
stebonicelli | 19:7f8c174448d0 | 82 | pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); |
stebonicelli | 19:7f8c174448d0 | 83 | |
stebonicelli | 19:7f8c174448d0 | 84 | if(messageIn.id == 0x40 && pose != current_pose[0]) |
gidiana | 17:2ab8feddb7c8 | 85 | { |
stebonicelli | 19:7f8c174448d0 | 86 | if(pose == 1) |
stebonicelli | 19:7f8c174448d0 | 87 | { |
stebonicelli | 19:7f8c174448d0 | 88 | motor_1.setSpeed(-SPEED); |
stebonicelli | 19:7f8c174448d0 | 89 | } |
stebonicelli | 19:7f8c174448d0 | 90 | else if(pose == 2) |
stebonicelli | 19:7f8c174448d0 | 91 | { |
stebonicelli | 19:7f8c174448d0 | 92 | motor_1.setSpeed(SPEED); |
stebonicelli | 19:7f8c174448d0 | 93 | } |
stebonicelli | 19:7f8c174448d0 | 94 | else |
stebonicelli | 19:7f8c174448d0 | 95 | { |
stebonicelli | 19:7f8c174448d0 | 96 | motor_1.setSpeed(0); |
stebonicelli | 19:7f8c174448d0 | 97 | } |
gidiana | 17:2ab8feddb7c8 | 98 | |
stebonicelli | 19:7f8c174448d0 | 99 | current_pose[0] = pose; |
gidiana | 17:2ab8feddb7c8 | 100 | } |
stebonicelli | 19:7f8c174448d0 | 101 | else if(messageIn.id == 0x50 && pose != current_pose[1]) |
gidiana | 17:2ab8feddb7c8 | 102 | { |
stebonicelli | 19:7f8c174448d0 | 103 | if(pose == 1) |
stebonicelli | 19:7f8c174448d0 | 104 | { |
stebonicelli | 19:7f8c174448d0 | 105 | motor_2.setSpeed(-SPEED); |
stebonicelli | 19:7f8c174448d0 | 106 | } |
stebonicelli | 19:7f8c174448d0 | 107 | else if(pose == 2) |
stebonicelli | 19:7f8c174448d0 | 108 | { |
stebonicelli | 19:7f8c174448d0 | 109 | motor_2.setSpeed(SPEED); |
stebonicelli | 19:7f8c174448d0 | 110 | } |
stebonicelli | 19:7f8c174448d0 | 111 | else |
stebonicelli | 19:7f8c174448d0 | 112 | { |
stebonicelli | 19:7f8c174448d0 | 113 | motor_2.setSpeed(0); |
stebonicelli | 19:7f8c174448d0 | 114 | } |
stebonicelli | 19:7f8c174448d0 | 115 | |
stebonicelli | 19:7f8c174448d0 | 116 | current_pose[1] = pose; |
gidiana | 17:2ab8feddb7c8 | 117 | } |
stebonicelli | 19:7f8c174448d0 | 118 | else if(messageIn.id == 0x60 && pose != current_pose[2]) |
gidiana | 17:2ab8feddb7c8 | 119 | { |
stebonicelli | 19:7f8c174448d0 | 120 | if(pose == 1) |
stebonicelli | 19:7f8c174448d0 | 121 | { |
stebonicelli | 19:7f8c174448d0 | 122 | motor_3.setSpeed(-SPEED); |
stebonicelli | 19:7f8c174448d0 | 123 | } |
stebonicelli | 19:7f8c174448d0 | 124 | else if(pose == 2) |
stebonicelli | 19:7f8c174448d0 | 125 | { |
stebonicelli | 19:7f8c174448d0 | 126 | motor_3.setSpeed(SPEED); |
stebonicelli | 19:7f8c174448d0 | 127 | } |
stebonicelli | 19:7f8c174448d0 | 128 | else |
stebonicelli | 19:7f8c174448d0 | 129 | { |
stebonicelli | 19:7f8c174448d0 | 130 | motor_3.setSpeed(0); |
stebonicelli | 19:7f8c174448d0 | 131 | } |
gidiana | 17:2ab8feddb7c8 | 132 | |
stebonicelli | 19:7f8c174448d0 | 133 | current_pose[2] = pose; |
gidiana | 17:2ab8feddb7c8 | 134 | } |
stebonicelli | 19:7f8c174448d0 | 135 | else if(messageIn.id == 0x70 && pose != current_pose[3]) |
gidiana | 17:2ab8feddb7c8 | 136 | { |
stebonicelli | 19:7f8c174448d0 | 137 | if(pose == 1) |
stebonicelli | 19:7f8c174448d0 | 138 | { |
stebonicelli | 19:7f8c174448d0 | 139 | motor_4.setSpeed(-SPEED); |
stebonicelli | 19:7f8c174448d0 | 140 | } |
stebonicelli | 19:7f8c174448d0 | 141 | else if(pose == 2) |
stebonicelli | 19:7f8c174448d0 | 142 | { |
stebonicelli | 19:7f8c174448d0 | 143 | motor_4.setSpeed(SPEED); |
stebonicelli | 19:7f8c174448d0 | 144 | } |
stebonicelli | 19:7f8c174448d0 | 145 | else |
stebonicelli | 19:7f8c174448d0 | 146 | { |
stebonicelli | 19:7f8c174448d0 | 147 | motor_4.setSpeed(0); |
stebonicelli | 19:7f8c174448d0 | 148 | } |
gidiana | 17:2ab8feddb7c8 | 149 | |
stebonicelli | 19:7f8c174448d0 | 150 | current_pose[3] = pose; |
stebonicelli | 19:7f8c174448d0 | 151 | } |
stebonicelli | 19:7f8c174448d0 | 152 | } |
stebonicelli | 19:7f8c174448d0 | 153 | } |
stebonicelli | 19:7f8c174448d0 | 154 | } |