Team DIANA / Mbed OS Arm_dynamixel_can

Dynamixel controller with CAN interface

Dependencies:   AX12_final MX106_not_working comunication_1

main.cpp@15:c1a790a1e999, 2019-04-16 (annotated)

Committer:
stebonicelli
Date:
Tue Apr 16 08:58:02 2019 +0000
Revision:
15:c1a790a1e999
Parent:
13:698bd4df9702 
using raw serial

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"
stebonicelli 13:698bd4df9702 4
stebonicelli 15:c1a790a1e999 5 #define SPEED 50
stebonicelli 15:c1a790a1e999 6
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 15:c1a790a1e999 12 communication_1 wire(PA_9, PA_10, 57600);
stebonicelli 13:698bd4df9702 13 MX106 motor_1(wire, 1, 1);
stebonicelli 13:698bd4df9702 14 MX106 motor_2(wire, 2, 1);
stebonicelli 13:698bd4df9702 15 MX106 motor_3(wire, 3, 1);
clynamen 11:19e8022f60ea 16
stebonicelli 13:698bd4df9702 17 void button_int_handler()
stebonicelli 13:698bd4df9702 18 {
stebonicelli 13:698bd4df9702 19
stebonicelli 13:698bd4df9702 20 }
stebonicelli 13:698bd4df9702 21
stebonicelli 13:698bd4df9702 22 // CAN
stebonicelli 13:698bd4df9702 23 Thread canrxa;
stebonicelli 13:698bd4df9702 24
stebonicelli 13:698bd4df9702 25 CAN can1(PA_11, PA_12); // RX, TX
stebonicelli 13:698bd4df9702 26 CANMessage messageIn;
stebonicelli 13:698bd4df9702 27 CANMessage messageOut;
stebonicelli 13:698bd4df9702 28
stebonicelli 13:698bd4df9702 29 int filter = can1.filter(0x000, 0x400, CANStandard);
dconsoli 4:6450eb95727d 30
stebonicelli 15:c1a790a1e999 31 int pose;
stebonicelli 15:c1a790a1e999 32 int current_pose[] = {0, 0, 0};
stebonicelli 15:c1a790a1e999 33 int desired_pose[] = {0, 0, 0};
stebonicelli 15:c1a790a1e999 34
stebonicelli 13:698bd4df9702 35 void canrx()
stebonicelli 13:698bd4df9702 36 {
stebonicelli 15:c1a790a1e999 37 //while(1)
stebonicelli 13:698bd4df9702 38 {
stebonicelli 13:698bd4df9702 39 if(can1.read(messageIn, filter))
stebonicelli 13:698bd4df9702 40 {
stebonicelli 15:c1a790a1e999 41 pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
stebonicelli 13:698bd4df9702 42
stebonicelli 13:698bd4df9702 43 if((messageIn.id & 0x0FF) == 0x40)
stebonicelli 13:698bd4df9702 44 {
stebonicelli 15:c1a790a1e999 45 desired_pose[0] = pose;
stebonicelli 13:698bd4df9702 46 }
stebonicelli 13:698bd4df9702 47 else if((messageIn.id & 0x0FF) == 0x50)
stebonicelli 13:698bd4df9702 48 {
stebonicelli 15:c1a790a1e999 49 desired_pose[1] = pose;
stebonicelli 13:698bd4df9702 50 }
stebonicelli 13:698bd4df9702 51 else if((messageIn.id & 0x0FF) == 0x60)
stebonicelli 13:698bd4df9702 52 {
stebonicelli 15:c1a790a1e999 53 desired_pose[2] = pose;
stebonicelli 13:698bd4df9702 54 }
stebonicelli 13:698bd4df9702 55 }
stebonicelli 13:698bd4df9702 56 }
stebonicelli 13:698bd4df9702 57 }
ilaria 1:946c65496383 58
stebonicelli 13:698bd4df9702 59 int main()
stebonicelli 13:698bd4df9702 60 {
stebonicelli 15:c1a790a1e999 61 can1.frequency(125000);
stebonicelli 15:c1a790a1e999 62
stebonicelli 15:c1a790a1e999 63 printf("DYNAMIXEL: Init START");
stebonicelli 15:c1a790a1e999 64
clynamen 11:19e8022f60ea 65 wire.trigger();
clynamen 11:19e8022f60ea 66 wire.trigger();
clynamen 11:19e8022f60ea 67 wire.trigger();
clynamen 11:19e8022f60ea 68 wire.trigger();
clynamen 11:19e8022f60ea 69
stebonicelli 13:698bd4df9702 70 // Setup Motor1 MultiTurn
stebonicelli 13:698bd4df9702 71 motor_1.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 72 motor_1.setMode(2);
stebonicelli 15:c1a790a1e999 73 wait(1);
stebonicelli 15:c1a790a1e999 74 printf("DYNAMIXEL: Init 1 DONE");
clynamen 11:19e8022f60ea 75
stebonicelli 13:698bd4df9702 76 // Setup Motor2 MultiTurn
stebonicelli 13:698bd4df9702 77 motor_2.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 78 motor_2.setMode(2);
stebonicelli 15:c1a790a1e999 79 wait(1);
stebonicelli 15:c1a790a1e999 80 printf("DYNAMIXEL: Init 2 DONE");
clynamen 11:19e8022f60ea 81
stebonicelli 13:698bd4df9702 82 // Setup Motor3 MultiTurn
stebonicelli 13:698bd4df9702 83 motor_3.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 84 motor_3.setMode(2);
stebonicelli 15:c1a790a1e999 85 wait(1);
stebonicelli 15:c1a790a1e999 86 printf("DYNAMIXEL: Init 3 DONE");
clynamen 11:19e8022f60ea 87
stebonicelli 15:c1a790a1e999 88 printf("DYNAMIXEL: Init ALL DONE\n\r");
clynamen 11:19e8022f60ea 89
stebonicelli 13:698bd4df9702 90 button.rise(&button_int_handler);
stebonicelli 13:698bd4df9702 91
stebonicelli 13:698bd4df9702 92 // CAN Initialization
stebonicelli 15:c1a790a1e999 93 //canrxa.start(canrx);
clynamen 11:19e8022f60ea 94
stebonicelli 15:c1a790a1e999 95 printf("CAN: Init DONE\n\r");
stebonicelli 13:698bd4df9702 96
stebonicelli 13:698bd4df9702 97 printf("Running!\n\r");
stebonicelli 13:698bd4df9702 98
stebonicelli 13:698bd4df9702 99 while(true)
stebonicelli 13:698bd4df9702 100 {
stebonicelli 15:c1a790a1e999 101 canrx();
stebonicelli 15:c1a790a1e999 102
stebonicelli 15:c1a790a1e999 103 if(desired_pose[0] != current_pose[0])
stebonicelli 15:c1a790a1e999 104 {
stebonicelli 15:c1a790a1e999 105 if(desired_pose[0] == 1)
stebonicelli 15:c1a790a1e999 106 motor_1.setSpeed(-SPEED);
stebonicelli 15:c1a790a1e999 107 else if(desired_pose[0] == 2)
stebonicelli 15:c1a790a1e999 108 motor_1.setSpeed(SPEED);
stebonicelli 15:c1a790a1e999 109 else
stebonicelli 15:c1a790a1e999 110 motor_1.setSpeed(0);
stebonicelli 15:c1a790a1e999 111
stebonicelli 15:c1a790a1e999 112 current_pose[0] = desired_pose[0];
stebonicelli 15:c1a790a1e999 113 }
stebonicelli 15:c1a790a1e999 114
stebonicelli 15:c1a790a1e999 115 if(desired_pose[1] != current_pose[1])
stebonicelli 15:c1a790a1e999 116 {
stebonicelli 15:c1a790a1e999 117 if(desired_pose[1] == 1)
stebonicelli 15:c1a790a1e999 118 motor_2.setSpeed(-SPEED);
stebonicelli 15:c1a790a1e999 119 else if(desired_pose[1] == 2)
stebonicelli 15:c1a790a1e999 120 motor_2.setSpeed(SPEED);
stebonicelli 15:c1a790a1e999 121 else
stebonicelli 15:c1a790a1e999 122 motor_2.setSpeed(0);
stebonicelli 15:c1a790a1e999 123
stebonicelli 15:c1a790a1e999 124 current_pose[1] = desired_pose[1];
stebonicelli 15:c1a790a1e999 125 }
stebonicelli 15:c1a790a1e999 126
stebonicelli 15:c1a790a1e999 127 if(desired_pose[2] != current_pose[2])
stebonicelli 15:c1a790a1e999 128 {
stebonicelli 15:c1a790a1e999 129 if(desired_pose[2] == 1)
stebonicelli 15:c1a790a1e999 130 motor_3.setSpeed(-SPEED);
stebonicelli 15:c1a790a1e999 131 else if(desired_pose[2] == 2)
stebonicelli 15:c1a790a1e999 132 motor_3.setSpeed(SPEED);
stebonicelli 15:c1a790a1e999 133 else
stebonicelli 15:c1a790a1e999 134 motor_3.setSpeed(0);
stebonicelli 15:c1a790a1e999 135
stebonicelli 15:c1a790a1e999 136 current_pose[2] = desired_pose[2];
stebonicelli 15:c1a790a1e999 137 }
clynamen 11:19e8022f60ea 138 }
clynamen 10:2acfa1a84c96 139 }