Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

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?

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