To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp@16:5454456b36f7, 2019-02-08 (annotated)
- Committer:
- gidiana
- Date:
- Fri Feb 08 20:36:19 2019 +0000
- Revision:
- 16:5454456b36f7
- Parent:
- 15:d058877eb501
- Child:
- 17:2ab8feddb7c8
servo arm camera
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" |
gidiana | 14:c51c4e0f3bc9 | 6 | #define SPEED 100 |
stebonicelli | 13:698bd4df9702 | 7 | // Utility |
stebonicelli | 13:698bd4df9702 | 8 | InterruptIn button(USER_BUTTON); |
stebonicelli | 13:698bd4df9702 | 9 | DigitalOut led(LED1); |
stebonicelli | 13:698bd4df9702 | 10 | |
stebonicelli | 13:698bd4df9702 | 11 | // Motor Control |
stebonicelli | 13:698bd4df9702 | 12 | Serial pc(USBTX, USBRX); |
stebonicelli | 13:698bd4df9702 | 13 | communication_1 wire(PA_9, PA_10, 200000); |
stebonicelli | 13:698bd4df9702 | 14 | MX106 motor_1(wire, 1, 1); |
stebonicelli | 13:698bd4df9702 | 15 | MX106 motor_2(wire, 2, 1); |
stebonicelli | 13:698bd4df9702 | 16 | MX106 motor_3(wire, 3, 1); |
gidiana | 14:c51c4e0f3bc9 | 17 | AX12 motor_4(wire, 4, 1); |
gidiana | 16:5454456b36f7 | 18 | Servo cam1 (PB_12); |
gidiana | 16:5454456b36f7 | 19 | Servo cam2 (PB_13); |
stebonicelli | 13:698bd4df9702 | 20 | void button_int_handler() |
stebonicelli | 13:698bd4df9702 | 21 | { |
stebonicelli | 13:698bd4df9702 | 22 | |
stebonicelli | 13:698bd4df9702 | 23 | } |
stebonicelli | 13:698bd4df9702 | 24 | |
stebonicelli | 13:698bd4df9702 | 25 | // CAN |
stebonicelli | 13:698bd4df9702 | 26 | Thread canrxa; |
stebonicelli | 13:698bd4df9702 | 27 | |
stebonicelli | 13:698bd4df9702 | 28 | CAN can1(PA_11, PA_12); // RX, TX |
stebonicelli | 13:698bd4df9702 | 29 | |
stebonicelli | 13:698bd4df9702 | 30 | CANMessage messageIn; |
stebonicelli | 13:698bd4df9702 | 31 | CANMessage messageOut; |
stebonicelli | 13:698bd4df9702 | 32 | |
stebonicelli | 13:698bd4df9702 | 33 | int filter = can1.filter(0x000, 0x400, CANStandard); |
gidiana | 14:c51c4e0f3bc9 | 34 | int pose; |
stebonicelli | 13:698bd4df9702 | 35 | void canrx() |
stebonicelli | 13:698bd4df9702 | 36 | { |
stebonicelli | 13:698bd4df9702 | 37 | while(1) |
stebonicelli | 13:698bd4df9702 | 38 | { |
stebonicelli | 13:698bd4df9702 | 39 | if(can1.read(messageIn, filter)) |
stebonicelli | 13:698bd4df9702 | 40 | { |
gidiana | 14:c51c4e0f3bc9 | 41 | pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); |
gidiana | 14:c51c4e0f3bc9 | 42 | printf("CAN: mess %d\n\r", pose); |
gidiana | 14:c51c4e0f3bc9 | 43 | printf("CAN: id %x \n\r ",messageIn.id); |
stebonicelli | 13:698bd4df9702 | 44 | |
stebonicelli | 13:698bd4df9702 | 45 | if((messageIn.id & 0x0FF) == 0x40) |
stebonicelli | 13:698bd4df9702 | 46 | { |
gidiana | 14:c51c4e0f3bc9 | 47 | if (pose==0) |
gidiana | 14:c51c4e0f3bc9 | 48 | { |
gidiana | 14:c51c4e0f3bc9 | 49 | motor_1.setSpeed(-SPEED); |
gidiana | 14:c51c4e0f3bc9 | 50 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 51 | } |
gidiana | 14:c51c4e0f3bc9 | 52 | |
gidiana | 14:c51c4e0f3bc9 | 53 | else if (pose==50) |
gidiana | 14:c51c4e0f3bc9 | 54 | { |
gidiana | 14:c51c4e0f3bc9 | 55 | motor_1.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 56 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 57 | } |
gidiana | 14:c51c4e0f3bc9 | 58 | else if (pose==100) |
gidiana | 14:c51c4e0f3bc9 | 59 | { |
gidiana | 14:c51c4e0f3bc9 | 60 | |
gidiana | 15:d058877eb501 | 61 | motor_1.setSpeed(SPEED); |
gidiana | 14:c51c4e0f3bc9 | 62 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 63 | |
gidiana | 14:c51c4e0f3bc9 | 64 | } |
gidiana | 14:c51c4e0f3bc9 | 65 | else |
gidiana | 14:c51c4e0f3bc9 | 66 | motor_1.setSpeed(0); |
stebonicelli | 13:698bd4df9702 | 67 | } |
stebonicelli | 13:698bd4df9702 | 68 | else if((messageIn.id & 0x0FF) == 0x50) |
stebonicelli | 13:698bd4df9702 | 69 | { |
gidiana | 14:c51c4e0f3bc9 | 70 | if (pose==0) |
gidiana | 14:c51c4e0f3bc9 | 71 | { |
gidiana | 14:c51c4e0f3bc9 | 72 | motor_2.setSpeed(-SPEED); |
gidiana | 14:c51c4e0f3bc9 | 73 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 74 | if (motor_2.getPosition()<-90) motor_2.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 75 | } |
gidiana | 14:c51c4e0f3bc9 | 76 | |
gidiana | 14:c51c4e0f3bc9 | 77 | else if (pose==50) |
gidiana | 14:c51c4e0f3bc9 | 78 | { |
gidiana | 14:c51c4e0f3bc9 | 79 | motor_2.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 80 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 81 | } |
gidiana | 14:c51c4e0f3bc9 | 82 | else if (pose==100) |
gidiana | 14:c51c4e0f3bc9 | 83 | { |
gidiana | 14:c51c4e0f3bc9 | 84 | |
gidiana | 15:d058877eb501 | 85 | motor_2.setSpeed(SPEED); |
gidiana | 14:c51c4e0f3bc9 | 86 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 87 | if (motor_2.getPosition()>90) motor_2.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 88 | } |
gidiana | 14:c51c4e0f3bc9 | 89 | else |
gidiana | 14:c51c4e0f3bc9 | 90 | motor_1.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 91 | } |
stebonicelli | 13:698bd4df9702 | 92 | else if((messageIn.id & 0x0FF) == 0x60) |
stebonicelli | 13:698bd4df9702 | 93 | { |
gidiana | 14:c51c4e0f3bc9 | 94 | |
gidiana | 14:c51c4e0f3bc9 | 95 | if (pose==0) |
gidiana | 14:c51c4e0f3bc9 | 96 | { |
gidiana | 14:c51c4e0f3bc9 | 97 | motor_3.setSpeed(-SPEED); |
gidiana | 14:c51c4e0f3bc9 | 98 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 99 | } |
gidiana | 14:c51c4e0f3bc9 | 100 | |
gidiana | 14:c51c4e0f3bc9 | 101 | else if (pose==50) |
gidiana | 14:c51c4e0f3bc9 | 102 | { |
gidiana | 14:c51c4e0f3bc9 | 103 | motor_3.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 104 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 105 | } |
gidiana | 14:c51c4e0f3bc9 | 106 | else if (pose==100) |
gidiana | 14:c51c4e0f3bc9 | 107 | { |
gidiana | 14:c51c4e0f3bc9 | 108 | |
gidiana | 15:d058877eb501 | 109 | motor_3.setSpeed(SPEED); |
gidiana | 14:c51c4e0f3bc9 | 110 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 111 | |
gidiana | 14:c51c4e0f3bc9 | 112 | } |
gidiana | 14:c51c4e0f3bc9 | 113 | else |
gidiana | 14:c51c4e0f3bc9 | 114 | motor_3.setSpeed(0); |
stebonicelli | 13:698bd4df9702 | 115 | } |
stebonicelli | 13:698bd4df9702 | 116 | else if((messageIn.id & 0x0FF) == 0x70) |
stebonicelli | 13:698bd4df9702 | 117 | { |
stebonicelli | 13:698bd4df9702 | 118 | |
gidiana | 14:c51c4e0f3bc9 | 119 | if (pose==0) |
gidiana | 14:c51c4e0f3bc9 | 120 | { |
gidiana | 14:c51c4e0f3bc9 | 121 | motor_4.setSpeed(-SPEED); |
gidiana | 14:c51c4e0f3bc9 | 122 | printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 123 | } |
gidiana | 14:c51c4e0f3bc9 | 124 | |
gidiana | 14:c51c4e0f3bc9 | 125 | else if (pose==50) |
gidiana | 14:c51c4e0f3bc9 | 126 | { |
gidiana | 14:c51c4e0f3bc9 | 127 | motor_1.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 128 | printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 129 | } |
gidiana | 14:c51c4e0f3bc9 | 130 | else if (pose==100) |
gidiana | 14:c51c4e0f3bc9 | 131 | { |
gidiana | 14:c51c4e0f3bc9 | 132 | |
gidiana | 15:d058877eb501 | 133 | motor_1.setSpeed(SPEED); |
gidiana | 16:5454456b36f7 | 134 | printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 135 | } |
gidiana | 14:c51c4e0f3bc9 | 136 | else |
gidiana | 14:c51c4e0f3bc9 | 137 | motor_4.setSpeed(0); |
gidiana | 16:5454456b36f7 | 138 | } |
gidiana | 16:5454456b36f7 | 139 | else if((messageIn.id & 0x0FF) == 0x80) |
gidiana | 16:5454456b36f7 | 140 | { |
gidiana | 16:5454456b36f7 | 141 | if (pose==0) |
gidiana | 16:5454456b36f7 | 142 | { |
gidiana | 16:5454456b36f7 | 143 | for(float p=0; p<1.0; p -= 0.1) |
gidiana | 16:5454456b36f7 | 144 | { |
gidiana | 16:5454456b36f7 | 145 | cam1 = p; |
gidiana | 16:5454456b36f7 | 146 | wait(0.2); |
gidiana | 16:5454456b36f7 | 147 | } |
gidiana | 16:5454456b36f7 | 148 | } |
gidiana | 16:5454456b36f7 | 149 | else if (pose==100) |
gidiana | 16:5454456b36f7 | 150 | { |
gidiana | 16:5454456b36f7 | 151 | for(float p=0; p<1.0; p += 0.1) |
gidiana | 16:5454456b36f7 | 152 | { |
gidiana | 16:5454456b36f7 | 153 | cam1 = p; |
gidiana | 16:5454456b36f7 | 154 | wait(0.2); |
gidiana | 16:5454456b36f7 | 155 | } |
gidiana | 16:5454456b36f7 | 156 | } |
gidiana | 16:5454456b36f7 | 157 | } |
gidiana | 16:5454456b36f7 | 158 | else if((messageIn.id & 0x0FF) == 0x90) |
gidiana | 16:5454456b36f7 | 159 | { |
gidiana | 16:5454456b36f7 | 160 | if (pose==0) |
gidiana | 16:5454456b36f7 | 161 | { |
gidiana | 16:5454456b36f7 | 162 | for(float p=0; p<1.0; p -= 0.1) |
gidiana | 16:5454456b36f7 | 163 | { |
gidiana | 16:5454456b36f7 | 164 | cam2 = p; |
gidiana | 16:5454456b36f7 | 165 | wait(0.2); |
gidiana | 16:5454456b36f7 | 166 | } |
gidiana | 16:5454456b36f7 | 167 | } |
gidiana | 16:5454456b36f7 | 168 | else if (pose==100) |
gidiana | 16:5454456b36f7 | 169 | { |
gidiana | 16:5454456b36f7 | 170 | for(float p=0; p<1.0; p += 0.1) |
gidiana | 16:5454456b36f7 | 171 | { |
gidiana | 16:5454456b36f7 | 172 | cam2 = p; |
gidiana | 16:5454456b36f7 | 173 | wait(0.2); |
gidiana | 16:5454456b36f7 | 174 | } |
gidiana | 16:5454456b36f7 | 175 | } |
gidiana | 16:5454456b36f7 | 176 | |
stebonicelli | 13:698bd4df9702 | 177 | } |
stebonicelli | 13:698bd4df9702 | 178 | } |
stebonicelli | 13:698bd4df9702 | 179 | } |
stebonicelli | 13:698bd4df9702 | 180 | } |
ilaria | 1:946c65496383 | 181 | |
stebonicelli | 13:698bd4df9702 | 182 | int main() |
stebonicelli | 13:698bd4df9702 | 183 | { |
clynamen | 11:19e8022f60ea | 184 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 185 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 186 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 187 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 188 | |
stebonicelli | 13:698bd4df9702 | 189 | // Setup Motor1 MultiTurn |
stebonicelli | 13:698bd4df9702 | 190 | motor_1.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 191 | motor_1.setMode(0); |
gidiana | 14:c51c4e0f3bc9 | 192 | motor_1.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 193 | printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition()); |
stebonicelli | 13:698bd4df9702 | 194 | //motor_1.setGoalPosition(0); |
stebonicelli | 13:698bd4df9702 | 195 | wait(10); |
clynamen | 11:19e8022f60ea | 196 | |
stebonicelli | 13:698bd4df9702 | 197 | // Setup Motor2 MultiTurn |
stebonicelli | 13:698bd4df9702 | 198 | motor_2.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 199 | motor_2.setMode(0); |
gidiana | 14:c51c4e0f3bc9 | 200 | motor_2.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 201 | printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition()); |
stebonicelli | 13:698bd4df9702 | 202 | //motor_2.setGoalPosition(0); |
stebonicelli | 13:698bd4df9702 | 203 | wait(10); |
clynamen | 11:19e8022f60ea | 204 | |
stebonicelli | 13:698bd4df9702 | 205 | // Setup Motor3 MultiTurn |
stebonicelli | 13:698bd4df9702 | 206 | motor_3.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 207 | motor_3.setMode(0); |
gidiana | 14:c51c4e0f3bc9 | 208 | motor_3.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 209 | printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition()); |
gidiana | 14:c51c4e0f3bc9 | 210 | wait(10); |
stebonicelli | 13:698bd4df9702 | 211 | //motor_3.setGoalPosition(0); |
gidiana | 16:5454456b36f7 | 212 | // Setup Motor4 MultiTurn |
gidiana | 14:c51c4e0f3bc9 | 213 | motor_4.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 214 | motor_4.setMode(0); |
gidiana | 14:c51c4e0f3bc9 | 215 | motor_4.setSpeed(0); |
gidiana | 14:c51c4e0f3bc9 | 216 | printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition()); |
stebonicelli | 13:698bd4df9702 | 217 | wait(10); |
clynamen | 11:19e8022f60ea | 218 | |
stebonicelli | 13:698bd4df9702 | 219 | printf("DYNAMIXEL: Init DONE\n\r"); |
clynamen | 11:19e8022f60ea | 220 | |
stebonicelli | 13:698bd4df9702 | 221 | button.rise(&button_int_handler); |
stebonicelli | 13:698bd4df9702 | 222 | |
stebonicelli | 13:698bd4df9702 | 223 | // CAN Initialization |
stebonicelli | 13:698bd4df9702 | 224 | canrxa.start(canrx); |
clynamen | 11:19e8022f60ea | 225 | |
stebonicelli | 13:698bd4df9702 | 226 | printf("DONE: CAN Init\n\r"); |
stebonicelli | 13:698bd4df9702 | 227 | |
stebonicelli | 13:698bd4df9702 | 228 | |
stebonicelli | 13:698bd4df9702 | 229 | printf("Running!\n\r"); |
stebonicelli | 13:698bd4df9702 | 230 | |
stebonicelli | 13:698bd4df9702 | 231 | while(true) |
stebonicelli | 13:698bd4df9702 | 232 | { |
stebonicelli | 13:698bd4df9702 | 233 | wait(1000); |
clynamen | 11:19e8022f60ea | 234 | } |
clynamen | 10:2acfa1a84c96 | 235 | } |