Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

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?

UserRevisionLine numberNew 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 }