Team DIANA / Mbed OS Arm_dynamixel_can

Dynamixel controller with CAN interface

Dependencies:   AX12_final MX106_not_working comunication_1

main.cpp@13:698bd4df9702, 2019-02-08 (annotated)

Committer:
stebonicelli
Date:
Fri Feb 08 15:23:40 2019 +0000
Revision:
13:698bd4df9702
Parent:
11:19e8022f60ea
Child:
15:c1a790a1e999
First

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 13:698bd4df9702 5 // Utility
stebonicelli 13:698bd4df9702 6 InterruptIn button(USER_BUTTON);
stebonicelli 13:698bd4df9702 7 DigitalOut led(LED1);
stebonicelli 13:698bd4df9702 8
stebonicelli 13:698bd4df9702 9 // Motor Control
stebonicelli 13:698bd4df9702 10 Serial pc(USBTX, USBRX);
stebonicelli 13:698bd4df9702 11 communication_1 wire(PA_9, PA_10, 200000);
stebonicelli 13:698bd4df9702 12 MX106 motor_1(wire, 1, 1);
stebonicelli 13:698bd4df9702 13 MX106 motor_2(wire, 2, 1);
stebonicelli 13:698bd4df9702 14 MX106 motor_3(wire, 3, 1);
clynamen 11:19e8022f60ea 15
stebonicelli 13:698bd4df9702 16 void button_int_handler()
stebonicelli 13:698bd4df9702 17 {
stebonicelli 13:698bd4df9702 18
stebonicelli 13:698bd4df9702 19 }
stebonicelli 13:698bd4df9702 20
stebonicelli 13:698bd4df9702 21 // CAN
stebonicelli 13:698bd4df9702 22 Thread canrxa;
stebonicelli 13:698bd4df9702 23
stebonicelli 13:698bd4df9702 24 CAN can1(PA_11, PA_12); // RX, TX
stebonicelli 13:698bd4df9702 25
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 13:698bd4df9702 31 void canrx()
stebonicelli 13:698bd4df9702 32 {
stebonicelli 13:698bd4df9702 33 while(1)
stebonicelli 13:698bd4df9702 34 {
stebonicelli 13:698bd4df9702 35 if(can1.read(messageIn, filter))
stebonicelli 13:698bd4df9702 36 {
stebonicelli 13:698bd4df9702 37 printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
stebonicelli 13:698bd4df9702 38 printf("CANaacc: id %x \n\r ",messageIn.id);
stebonicelli 13:698bd4df9702 39
stebonicelli 13:698bd4df9702 40 if((messageIn.id & 0x0FF) == 0x40)
stebonicelli 13:698bd4df9702 41 {
stebonicelli 13:698bd4df9702 42
stebonicelli 13:698bd4df9702 43 }
stebonicelli 13:698bd4df9702 44 else if((messageIn.id & 0x0FF) == 0x50)
stebonicelli 13:698bd4df9702 45 {
stebonicelli 13:698bd4df9702 46
stebonicelli 13:698bd4df9702 47 }
stebonicelli 13:698bd4df9702 48 else if((messageIn.id & 0x0FF) == 0x60)
stebonicelli 13:698bd4df9702 49 {
stebonicelli 13:698bd4df9702 50
stebonicelli 13:698bd4df9702 51 }
stebonicelli 13:698bd4df9702 52 else if((messageIn.id & 0x0FF) == 0x70)
stebonicelli 13:698bd4df9702 53 {
stebonicelli 13:698bd4df9702 54
stebonicelli 13:698bd4df9702 55 }
stebonicelli 13:698bd4df9702 56 }
stebonicelli 13:698bd4df9702 57 }
stebonicelli 13:698bd4df9702 58 }
ilaria 1:946c65496383 59
stebonicelli 13:698bd4df9702 60 int main()
stebonicelli 13:698bd4df9702 61 {
clynamen 11:19e8022f60ea 62 wire.trigger();
clynamen 11:19e8022f60ea 63 wire.trigger();
clynamen 11:19e8022f60ea 64 wire.trigger();
clynamen 11:19e8022f60ea 65 wire.trigger();
clynamen 11:19e8022f60ea 66
stebonicelli 13:698bd4df9702 67 // Setup Motor1 MultiTurn
stebonicelli 13:698bd4df9702 68 motor_1.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 69 motor_1.setMode(2);
stebonicelli 13:698bd4df9702 70 motor_1.setSpeed(90);
stebonicelli 13:698bd4df9702 71 //motor_1.setGoalPosition(0);
stebonicelli 13:698bd4df9702 72 wait(10);
clynamen 11:19e8022f60ea 73
stebonicelli 13:698bd4df9702 74 // Setup Motor2 MultiTurn
stebonicelli 13:698bd4df9702 75 motor_2.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 76 motor_2.setMode(2);
stebonicelli 13:698bd4df9702 77 motor_2.setSpeed(90);
stebonicelli 13:698bd4df9702 78 //motor_2.setGoalPosition(0);
stebonicelli 13:698bd4df9702 79 wait(10);
clynamen 11:19e8022f60ea 80
stebonicelli 13:698bd4df9702 81 // Setup Motor3 MultiTurn
stebonicelli 13:698bd4df9702 82 motor_3.setMotorEnabled(1);
stebonicelli 13:698bd4df9702 83 motor_3.setMode(2);
stebonicelli 13:698bd4df9702 84 motor_3.setSpeed(90);
stebonicelli 13:698bd4df9702 85 //motor_3.setGoalPosition(0);
stebonicelli 13:698bd4df9702 86 wait(10);
clynamen 11:19e8022f60ea 87
stebonicelli 13:698bd4df9702 88 printf("DYNAMIXEL: Init 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 13:698bd4df9702 93 canrxa.start(canrx);
clynamen 11:19e8022f60ea 94
stebonicelli 13:698bd4df9702 95 printf("DONE: CAN Init\n\r");
stebonicelli 13:698bd4df9702 96
stebonicelli 13:698bd4df9702 97
stebonicelli 13:698bd4df9702 98 printf("Running!\n\r");
stebonicelli 13:698bd4df9702 99
stebonicelli 13:698bd4df9702 100 while(true)
stebonicelli 13:698bd4df9702 101 {
stebonicelli 13:698bd4df9702 102 wait(1000);
clynamen 11:19e8022f60ea 103 }
clynamen 10:2acfa1a84c96 104 }