FOrk
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp@18:841ab8c1bdbd, 2019-04-06 (annotated)
- Committer:
- stebonicelli
- Date:
- Sat Apr 06 17:00:28 2019 +0000
- Revision:
- 18:841ab8c1bdbd
- Parent:
- 17:2ab8feddb7c8
- Child:
- 19:7f8c174448d0
commands update
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 | |
gidiana | 14:c51c4e0f3bc9 | 7 | #define SPEED 100 |
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; |
gidiana | 17:2ab8feddb7c8 | 39 | int current_pose[6]; |
stebonicelli | 18:841ab8c1bdbd | 40 | |
stebonicelli | 13:698bd4df9702 | 41 | void canrx() |
stebonicelli | 13:698bd4df9702 | 42 | { |
stebonicelli | 13:698bd4df9702 | 43 | if(can1.read(messageIn, filter)) |
stebonicelli | 13:698bd4df9702 | 44 | { |
gidiana | 14:c51c4e0f3bc9 | 45 | pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); |
gidiana | 14:c51c4e0f3bc9 | 46 | printf("CAN: mess %d\n\r", pose); |
gidiana | 14:c51c4e0f3bc9 | 47 | printf("CAN: id %x \n\r ",messageIn.id); |
stebonicelli | 13:698bd4df9702 | 48 | |
stebonicelli | 13:698bd4df9702 | 49 | if((messageIn.id & 0x0FF) == 0x40) |
stebonicelli | 13:698bd4df9702 | 50 | { |
gidiana | 17:2ab8feddb7c8 | 51 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 52 | current_pose[0]=pose; |
gidiana | 17:2ab8feddb7c8 | 53 | //mutex.unlock(); |
stebonicelli | 13:698bd4df9702 | 54 | } |
stebonicelli | 13:698bd4df9702 | 55 | else if((messageIn.id & 0x0FF) == 0x50) |
stebonicelli | 13:698bd4df9702 | 56 | { |
gidiana | 17:2ab8feddb7c8 | 57 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 58 | current_pose[1]=pose; |
gidiana | 17:2ab8feddb7c8 | 59 | //mutex.unlock(); |
gidiana | 14:c51c4e0f3bc9 | 60 | } |
gidiana | 17:2ab8feddb7c8 | 61 | |
stebonicelli | 13:698bd4df9702 | 62 | else if((messageIn.id & 0x0FF) == 0x60) |
stebonicelli | 13:698bd4df9702 | 63 | { |
gidiana | 17:2ab8feddb7c8 | 64 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 65 | current_pose[2]=pose; |
gidiana | 17:2ab8feddb7c8 | 66 | //mutex.unlock(); |
stebonicelli | 13:698bd4df9702 | 67 | } |
stebonicelli | 13:698bd4df9702 | 68 | else if((messageIn.id & 0x0FF) == 0x70) |
stebonicelli | 13:698bd4df9702 | 69 | { |
gidiana | 17:2ab8feddb7c8 | 70 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 71 | current_pose[3]=pose; |
gidiana | 17:2ab8feddb7c8 | 72 | //mutex.unlock(); |
gidiana | 17:2ab8feddb7c8 | 73 | } |
gidiana | 16:5454456b36f7 | 74 | else if((messageIn.id & 0x0FF) == 0x80) |
gidiana | 16:5454456b36f7 | 75 | { |
gidiana | 17:2ab8feddb7c8 | 76 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 77 | current_pose[4]=pose; |
gidiana | 17:2ab8feddb7c8 | 78 | //mutex.unlock(); |
gidiana | 16:5454456b36f7 | 79 | } |
gidiana | 16:5454456b36f7 | 80 | else if((messageIn.id & 0x0FF) == 0x90) |
gidiana | 16:5454456b36f7 | 81 | { |
gidiana | 17:2ab8feddb7c8 | 82 | //mutex.lock(); |
gidiana | 17:2ab8feddb7c8 | 83 | current_pose[5]=pose; |
gidiana | 17:2ab8feddb7c8 | 84 | //mutex.unlock(); |
gidiana | 17:2ab8feddb7c8 | 85 | } |
stebonicelli | 13:698bd4df9702 | 86 | } |
gidiana | 17:2ab8feddb7c8 | 87 | |
stebonicelli | 13:698bd4df9702 | 88 | } |
ilaria | 1:946c65496383 | 89 | |
stebonicelli | 13:698bd4df9702 | 90 | int main() |
stebonicelli | 13:698bd4df9702 | 91 | { |
gidiana | 17:2ab8feddb7c8 | 92 | printf("DYNAMIXEL: Init \n\r"); |
gidiana | 17:2ab8feddb7c8 | 93 | led=1; |
clynamen | 11:19e8022f60ea | 94 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 95 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 96 | wire.trigger(); |
clynamen | 11:19e8022f60ea | 97 | wire.trigger(); |
gidiana | 17:2ab8feddb7c8 | 98 | wait(5); |
stebonicelli | 13:698bd4df9702 | 99 | // Setup Motor1 MultiTurn |
stebonicelli | 13:698bd4df9702 | 100 | motor_1.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 101 | motor_1.setMode(0); |
gidiana | 17:2ab8feddb7c8 | 102 | //motor_1.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 103 | //printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 104 | |
gidiana | 17:2ab8feddb7c8 | 105 | wait(5); |
gidiana | 17:2ab8feddb7c8 | 106 | printf("DYNAMIXEL: Init DONE 1\n\r"); |
stebonicelli | 13:698bd4df9702 | 107 | // Setup Motor2 MultiTurn |
stebonicelli | 13:698bd4df9702 | 108 | motor_2.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 109 | motor_2.setMode(0); |
gidiana | 17:2ab8feddb7c8 | 110 | //motor_2.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 111 | //printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition()); |
clynamen | 11:19e8022f60ea | 112 | |
gidiana | 17:2ab8feddb7c8 | 113 | wait(5); |
gidiana | 17:2ab8feddb7c8 | 114 | printf("DYNAMIXEL: Init DONE 2\n\r"); |
stebonicelli | 13:698bd4df9702 | 115 | // Setup Motor3 MultiTurn |
stebonicelli | 13:698bd4df9702 | 116 | motor_3.setMotorEnabled(1); |
gidiana | 14:c51c4e0f3bc9 | 117 | motor_3.setMode(0); |
gidiana | 17:2ab8feddb7c8 | 118 | //motor_3.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 119 | //printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 120 | wait(5); |
gidiana | 17:2ab8feddb7c8 | 121 | printf("DYNAMIXEL: Init DONE 3\n\r"); |
stebonicelli | 13:698bd4df9702 | 122 | //motor_3.setGoalPosition(0); |
gidiana | 16:5454456b36f7 | 123 | // Setup Motor4 MultiTurn |
gidiana | 17:2ab8feddb7c8 | 124 | //motor_4.setMotorEnabled(1); |
gidiana | 17:2ab8feddb7c8 | 125 | //motor_4.setMode(0); |
gidiana | 17:2ab8feddb7c8 | 126 | //motor_4.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 127 | //printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 128 | wait(5); |
gidiana | 17:2ab8feddb7c8 | 129 | |
stebonicelli | 13:698bd4df9702 | 130 | printf("DYNAMIXEL: Init DONE\n\r"); |
clynamen | 11:19e8022f60ea | 131 | |
stebonicelli | 13:698bd4df9702 | 132 | button.rise(&button_int_handler); |
gidiana | 17:2ab8feddb7c8 | 133 | cam1=0,5; |
gidiana | 17:2ab8feddb7c8 | 134 | cam2=0,5; |
stebonicelli | 13:698bd4df9702 | 135 | // CAN Initialization |
gidiana | 17:2ab8feddb7c8 | 136 | //canrxa.start(canrx); |
clynamen | 11:19e8022f60ea | 137 | |
stebonicelli | 13:698bd4df9702 | 138 | printf("DONE: CAN Init\n\r"); |
stebonicelli | 13:698bd4df9702 | 139 | |
stebonicelli | 13:698bd4df9702 | 140 | |
stebonicelli | 13:698bd4df9702 | 141 | printf("Running!\n\r"); |
stebonicelli | 13:698bd4df9702 | 142 | |
stebonicelli | 13:698bd4df9702 | 143 | while(true) |
stebonicelli | 13:698bd4df9702 | 144 | { |
gidiana | 17:2ab8feddb7c8 | 145 | canrx(); |
stebonicelli | 18:841ab8c1bdbd | 146 | if (current_pose[0]==1) |
gidiana | 17:2ab8feddb7c8 | 147 | { |
gidiana | 17:2ab8feddb7c8 | 148 | motor_1.setSpeed(-SPEED); |
gidiana | 17:2ab8feddb7c8 | 149 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 150 | } |
gidiana | 17:2ab8feddb7c8 | 151 | |
stebonicelli | 18:841ab8c1bdbd | 152 | else if (current_pose[0]==0) |
gidiana | 17:2ab8feddb7c8 | 153 | { |
gidiana | 17:2ab8feddb7c8 | 154 | motor_1.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 155 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 156 | } |
stebonicelli | 18:841ab8c1bdbd | 157 | else if (current_pose[0]==2) |
gidiana | 17:2ab8feddb7c8 | 158 | { |
gidiana | 17:2ab8feddb7c8 | 159 | |
gidiana | 17:2ab8feddb7c8 | 160 | motor_1.setSpeed(SPEED); |
gidiana | 17:2ab8feddb7c8 | 161 | printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 162 | |
gidiana | 17:2ab8feddb7c8 | 163 | } |
gidiana | 17:2ab8feddb7c8 | 164 | |
stebonicelli | 18:841ab8c1bdbd | 165 | if (current_pose[1]==1) |
gidiana | 17:2ab8feddb7c8 | 166 | { |
gidiana | 17:2ab8feddb7c8 | 167 | motor_2.setSpeed(-SPEED); |
gidiana | 17:2ab8feddb7c8 | 168 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 169 | if (motor_2.getPosition()<-90) motor_2.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 170 | } |
gidiana | 17:2ab8feddb7c8 | 171 | |
stebonicelli | 18:841ab8c1bdbd | 172 | else if (current_pose[1]==0) |
gidiana | 17:2ab8feddb7c8 | 173 | { |
gidiana | 17:2ab8feddb7c8 | 174 | motor_2.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 175 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 176 | } |
stebonicelli | 18:841ab8c1bdbd | 177 | else if (current_pose[1]==2) |
gidiana | 17:2ab8feddb7c8 | 178 | { |
gidiana | 17:2ab8feddb7c8 | 179 | |
gidiana | 17:2ab8feddb7c8 | 180 | motor_2.setSpeed(SPEED); |
gidiana | 17:2ab8feddb7c8 | 181 | printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 182 | if (motor_2.getPosition()>90) motor_2.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 183 | } |
gidiana | 17:2ab8feddb7c8 | 184 | |
stebonicelli | 18:841ab8c1bdbd | 185 | if (current_pose[2]==1) |
gidiana | 17:2ab8feddb7c8 | 186 | { |
gidiana | 17:2ab8feddb7c8 | 187 | motor_3.setSpeed(-SPEED); |
gidiana | 17:2ab8feddb7c8 | 188 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 189 | } |
gidiana | 17:2ab8feddb7c8 | 190 | |
stebonicelli | 18:841ab8c1bdbd | 191 | else if (current_pose[2]==0) |
gidiana | 17:2ab8feddb7c8 | 192 | { |
gidiana | 17:2ab8feddb7c8 | 193 | motor_3.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 194 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 195 | } |
stebonicelli | 18:841ab8c1bdbd | 196 | else if (current_pose[2]==2) |
gidiana | 17:2ab8feddb7c8 | 197 | { |
gidiana | 17:2ab8feddb7c8 | 198 | |
gidiana | 17:2ab8feddb7c8 | 199 | motor_3.setSpeed(SPEED); |
gidiana | 17:2ab8feddb7c8 | 200 | printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 201 | |
gidiana | 17:2ab8feddb7c8 | 202 | } |
gidiana | 17:2ab8feddb7c8 | 203 | |
stebonicelli | 18:841ab8c1bdbd | 204 | if (current_pose[3]==1) |
gidiana | 17:2ab8feddb7c8 | 205 | { |
gidiana | 17:2ab8feddb7c8 | 206 | motor_4.setSpeed(-SPEED); |
gidiana | 17:2ab8feddb7c8 | 207 | printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 208 | } |
gidiana | 17:2ab8feddb7c8 | 209 | |
stebonicelli | 18:841ab8c1bdbd | 210 | else if (current_pose[3]==0) |
gidiana | 17:2ab8feddb7c8 | 211 | { |
gidiana | 17:2ab8feddb7c8 | 212 | motor_4.setSpeed(0); |
gidiana | 17:2ab8feddb7c8 | 213 | printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 214 | } |
stebonicelli | 18:841ab8c1bdbd | 215 | else if (current_pose[3]==2) |
gidiana | 17:2ab8feddb7c8 | 216 | { |
gidiana | 17:2ab8feddb7c8 | 217 | |
gidiana | 17:2ab8feddb7c8 | 218 | motor_4.setSpeed(SPEED); |
gidiana | 17:2ab8feddb7c8 | 219 | printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); |
gidiana | 17:2ab8feddb7c8 | 220 | } |
gidiana | 17:2ab8feddb7c8 | 221 | |
stebonicelli | 18:841ab8c1bdbd | 222 | if (current_pose[4]==1) |
gidiana | 17:2ab8feddb7c8 | 223 | { |
gidiana | 17:2ab8feddb7c8 | 224 | if (cam1==0) |
gidiana | 17:2ab8feddb7c8 | 225 | cam1=cam1; |
gidiana | 17:2ab8feddb7c8 | 226 | else cam1 = cam1-0.2; |
gidiana | 17:2ab8feddb7c8 | 227 | |
gidiana | 17:2ab8feddb7c8 | 228 | wait(0.2); |
gidiana | 17:2ab8feddb7c8 | 229 | |
gidiana | 17:2ab8feddb7c8 | 230 | } |
stebonicelli | 18:841ab8c1bdbd | 231 | else if (current_pose[4]==2) |
gidiana | 17:2ab8feddb7c8 | 232 | { |
gidiana | 17:2ab8feddb7c8 | 233 | |
gidiana | 17:2ab8feddb7c8 | 234 | if (cam1==1) |
gidiana | 17:2ab8feddb7c8 | 235 | cam1=cam1; |
gidiana | 17:2ab8feddb7c8 | 236 | else cam1 = cam1+0.2; |
gidiana | 17:2ab8feddb7c8 | 237 | |
gidiana | 17:2ab8feddb7c8 | 238 | wait(0.2); |
gidiana | 17:2ab8feddb7c8 | 239 | } |
stebonicelli | 18:841ab8c1bdbd | 240 | |
stebonicelli | 18:841ab8c1bdbd | 241 | if (current_pose[5]==1) |
gidiana | 17:2ab8feddb7c8 | 242 | { |
gidiana | 17:2ab8feddb7c8 | 243 | if (cam2==0) |
gidiana | 17:2ab8feddb7c8 | 244 | cam1=cam1; |
gidiana | 17:2ab8feddb7c8 | 245 | else cam2 = cam2-0.2; |
gidiana | 17:2ab8feddb7c8 | 246 | |
gidiana | 17:2ab8feddb7c8 | 247 | wait(0.2); |
gidiana | 17:2ab8feddb7c8 | 248 | |
gidiana | 17:2ab8feddb7c8 | 249 | } |
stebonicelli | 18:841ab8c1bdbd | 250 | else if (current_pose[5]==2) |
gidiana | 17:2ab8feddb7c8 | 251 | { |
gidiana | 17:2ab8feddb7c8 | 252 | |
gidiana | 17:2ab8feddb7c8 | 253 | if (cam2==1) |
gidiana | 17:2ab8feddb7c8 | 254 | cam2=cam2; |
gidiana | 17:2ab8feddb7c8 | 255 | else cam2 = cam2+0.2; |
gidiana | 17:2ab8feddb7c8 | 256 | |
gidiana | 17:2ab8feddb7c8 | 257 | wait(0.2); |
stebonicelli | 18:841ab8c1bdbd | 258 | } |
clynamen | 11:19e8022f60ea | 259 | } |
clynamen | 10:2acfa1a84c96 | 260 | } |