Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@14:c51c4e0f3bc9, 2019-02-08 (annotated)

Committer:
gidiana
Date:
Fri Feb 08 19:24:15 2019 +0000
Revision:
14:c51c4e0f3bc9
Parent:
13:698bd4df9702
Child:
15:d058877eb501
wheel mode

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 14:c51c4e0f3bc9 5 #define SPEED 100
stebonicelli 13:698bd4df9702 6 // Utility
stebonicelli 13:698bd4df9702 7 InterruptIn button(USER_BUTTON);
stebonicelli 13:698bd4df9702 8 DigitalOut led(LED1);
stebonicelli 13:698bd4df9702 9
stebonicelli 13:698bd4df9702 10 // Motor Control
stebonicelli 13:698bd4df9702 11 Serial pc(USBTX, USBRX);
stebonicelli 13:698bd4df9702 12 communication_1 wire(PA_9, PA_10, 200000);
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);
gidiana 14:c51c4e0f3bc9 16 AX12 motor_4(wire, 4, 1);
clynamen 11:19e8022f60ea 17
stebonicelli 13:698bd4df9702 18 void button_int_handler()
stebonicelli 13:698bd4df9702 19 {
stebonicelli 13:698bd4df9702 20
stebonicelli 13:698bd4df9702 21 }
stebonicelli 13:698bd4df9702 22
stebonicelli 13:698bd4df9702 23 // CAN
stebonicelli 13:698bd4df9702 24 Thread canrxa;
stebonicelli 13:698bd4df9702 25
stebonicelli 13:698bd4df9702 26 CAN can1(PA_11, PA_12); // RX, TX
stebonicelli 13:698bd4df9702 27
stebonicelli 13:698bd4df9702 28 CANMessage messageIn;
stebonicelli 13:698bd4df9702 29 CANMessage messageOut;
stebonicelli 13:698bd4df9702 30
stebonicelli 13:698bd4df9702 31 int filter = can1.filter(0x000, 0x400, CANStandard);
gidiana 14:c51c4e0f3bc9 32 int pose;
stebonicelli 13:698bd4df9702 33 void canrx()
stebonicelli 13:698bd4df9702 34 {
stebonicelli 13:698bd4df9702 35 while(1)
stebonicelli 13:698bd4df9702 36 {
stebonicelli 13:698bd4df9702 37 if(can1.read(messageIn, filter))
stebonicelli 13:698bd4df9702 38 {
gidiana 14:c51c4e0f3bc9 39 pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
gidiana 14:c51c4e0f3bc9 40 printf("CAN: mess %d\n\r", pose);
gidiana 14:c51c4e0f3bc9 41 printf("CAN: id %x \n\r ",messageIn.id);
stebonicelli 13:698bd4df9702 42
stebonicelli 13:698bd4df9702 43 if((messageIn.id & 0x0FF) == 0x40)
stebonicelli 13:698bd4df9702 44 {
gidiana 14:c51c4e0f3bc9 45 if (pose==0)
gidiana 14:c51c4e0f3bc9 46 {
gidiana 14:c51c4e0f3bc9 47 motor_1.setSpeed(-SPEED);
gidiana 14:c51c4e0f3bc9 48 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 14:c51c4e0f3bc9 49 }
gidiana 14:c51c4e0f3bc9 50
gidiana 14:c51c4e0f3bc9 51 else if (pose==50)
gidiana 14:c51c4e0f3bc9 52 {
gidiana 14:c51c4e0f3bc9 53 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 54 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 14:c51c4e0f3bc9 55 }
gidiana 14:c51c4e0f3bc9 56 else if (pose==100)
gidiana 14:c51c4e0f3bc9 57 {
gidiana 14:c51c4e0f3bc9 58
gidiana 14:c51c4e0f3bc9 59 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 60 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 14:c51c4e0f3bc9 61
gidiana 14:c51c4e0f3bc9 62 }
gidiana 14:c51c4e0f3bc9 63 else
gidiana 14:c51c4e0f3bc9 64 motor_1.setSpeed(0);
stebonicelli 13:698bd4df9702 65 }
stebonicelli 13:698bd4df9702 66 else if((messageIn.id & 0x0FF) == 0x50)
stebonicelli 13:698bd4df9702 67 {
gidiana 14:c51c4e0f3bc9 68 if (pose==0)
gidiana 14:c51c4e0f3bc9 69 {
gidiana 14:c51c4e0f3bc9 70 motor_2.setSpeed(-SPEED);
gidiana 14:c51c4e0f3bc9 71 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 14:c51c4e0f3bc9 72 if (motor_2.getPosition()<-90) motor_2.setSpeed(0);
gidiana 14:c51c4e0f3bc9 73 }
gidiana 14:c51c4e0f3bc9 74
gidiana 14:c51c4e0f3bc9 75 else if (pose==50)
gidiana 14:c51c4e0f3bc9 76 {
gidiana 14:c51c4e0f3bc9 77 motor_2.setSpeed(0);
gidiana 14:c51c4e0f3bc9 78 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 14:c51c4e0f3bc9 79 }
gidiana 14:c51c4e0f3bc9 80 else if (pose==100)
gidiana 14:c51c4e0f3bc9 81 {
gidiana 14:c51c4e0f3bc9 82
gidiana 14:c51c4e0f3bc9 83 motor_2.setSpeed(0);
gidiana 14:c51c4e0f3bc9 84 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 14:c51c4e0f3bc9 85 if (motor_2.getPosition()>90) motor_2.setSpeed(0);
gidiana 14:c51c4e0f3bc9 86 }
gidiana 14:c51c4e0f3bc9 87 else
gidiana 14:c51c4e0f3bc9 88 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 89 }
stebonicelli 13:698bd4df9702 90 else if((messageIn.id & 0x0FF) == 0x60)
stebonicelli 13:698bd4df9702 91 {
gidiana 14:c51c4e0f3bc9 92
gidiana 14:c51c4e0f3bc9 93 if (pose==0)
gidiana 14:c51c4e0f3bc9 94 {
gidiana 14:c51c4e0f3bc9 95 motor_3.setSpeed(-SPEED);
gidiana 14:c51c4e0f3bc9 96 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 14:c51c4e0f3bc9 97 }
gidiana 14:c51c4e0f3bc9 98
gidiana 14:c51c4e0f3bc9 99 else if (pose==50)
gidiana 14:c51c4e0f3bc9 100 {
gidiana 14:c51c4e0f3bc9 101 motor_3.setSpeed(0);
gidiana 14:c51c4e0f3bc9 102 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 14:c51c4e0f3bc9 103 }
gidiana 14:c51c4e0f3bc9 104 else if (pose==100)
gidiana 14:c51c4e0f3bc9 105 {
gidiana 14:c51c4e0f3bc9 106
gidiana 14:c51c4e0f3bc9 107 motor_3.setSpeed(0);
gidiana 14:c51c4e0f3bc9 108 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 14:c51c4e0f3bc9 109
gidiana 14:c51c4e0f3bc9 110 }
gidiana 14:c51c4e0f3bc9 111 else
gidiana 14:c51c4e0f3bc9 112 motor_3.setSpeed(0);
stebonicelli 13:698bd4df9702 113 }
stebonicelli 13:698bd4df9702 114 else if((messageIn.id & 0x0FF) == 0x70)
stebonicelli 13:698bd4df9702 115 {
stebonicelli 13:698bd4df9702 116
gidiana 14:c51c4e0f3bc9 117 if (pose==0)
gidiana 14:c51c4e0f3bc9 118 {
gidiana 14:c51c4e0f3bc9 119 motor_4.setSpeed(-SPEED);
gidiana 14:c51c4e0f3bc9 120 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
gidiana 14:c51c4e0f3bc9 121 }
gidiana 14:c51c4e0f3bc9 122
gidiana 14:c51c4e0f3bc9 123 else if (pose==50)
gidiana 14:c51c4e0f3bc9 124 {
gidiana 14:c51c4e0f3bc9 125 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 126 printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition());
gidiana 14:c51c4e0f3bc9 127 }
gidiana 14:c51c4e0f3bc9 128 else if (pose==100)
gidiana 14:c51c4e0f3bc9 129 {
gidiana 14:c51c4e0f3bc9 130
gidiana 14:c51c4e0f3bc9 131 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 132 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
gidiana 14:c51c4e0f3bc9 133
gidiana 14:c51c4e0f3bc9 134 }
gidiana 14:c51c4e0f3bc9 135 else
gidiana 14:c51c4e0f3bc9 136 motor_4.setSpeed(0);
stebonicelli 13:698bd4df9702 137 }
stebonicelli 13:698bd4df9702 138 }
stebonicelli 13:698bd4df9702 139 }
stebonicelli 13:698bd4df9702 140 }
ilaria 1:946c65496383 141
stebonicelli 13:698bd4df9702 142 int main()
stebonicelli 13:698bd4df9702 143 {
clynamen 11:19e8022f60ea 144 wire.trigger();
clynamen 11:19e8022f60ea 145 wire.trigger();
clynamen 11:19e8022f60ea 146 wire.trigger();
clynamen 11:19e8022f60ea 147 wire.trigger();
clynamen 11:19e8022f60ea 148
stebonicelli 13:698bd4df9702 149 // Setup Motor1 MultiTurn
stebonicelli 13:698bd4df9702 150 motor_1.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 151 motor_1.setMode(0);
gidiana 14:c51c4e0f3bc9 152 motor_1.setSpeed(0);
gidiana 14:c51c4e0f3bc9 153 printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition());
stebonicelli 13:698bd4df9702 154 //motor_1.setGoalPosition(0);
stebonicelli 13:698bd4df9702 155 wait(10);
clynamen 11:19e8022f60ea 156
stebonicelli 13:698bd4df9702 157 // Setup Motor2 MultiTurn
stebonicelli 13:698bd4df9702 158 motor_2.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 159 motor_2.setMode(0);
gidiana 14:c51c4e0f3bc9 160 motor_2.setSpeed(0);
gidiana 14:c51c4e0f3bc9 161 printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition());
stebonicelli 13:698bd4df9702 162 //motor_2.setGoalPosition(0);
stebonicelli 13:698bd4df9702 163 wait(10);
clynamen 11:19e8022f60ea 164
stebonicelli 13:698bd4df9702 165 // Setup Motor3 MultiTurn
stebonicelli 13:698bd4df9702 166 motor_3.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 167 motor_3.setMode(0);
gidiana 14:c51c4e0f3bc9 168 motor_3.setSpeed(0);
gidiana 14:c51c4e0f3bc9 169 printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
gidiana 14:c51c4e0f3bc9 170 wait(10);
stebonicelli 13:698bd4df9702 171 //motor_3.setGoalPosition(0);
gidiana 14:c51c4e0f3bc9 172 // Setup Motor4 MultiTurn
gidiana 14:c51c4e0f3bc9 173 motor_4.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 174 motor_4.setMode(0);
gidiana 14:c51c4e0f3bc9 175 motor_4.setSpeed(0);
gidiana 14:c51c4e0f3bc9 176 printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition());
stebonicelli 13:698bd4df9702 177 wait(10);
clynamen 11:19e8022f60ea 178
stebonicelli 13:698bd4df9702 179 printf("DYNAMIXEL: Init DONE\n\r");
clynamen 11:19e8022f60ea 180
stebonicelli 13:698bd4df9702 181 button.rise(&button_int_handler);
stebonicelli 13:698bd4df9702 182
stebonicelli 13:698bd4df9702 183 // CAN Initialization
stebonicelli 13:698bd4df9702 184 canrxa.start(canrx);
clynamen 11:19e8022f60ea 185
stebonicelli 13:698bd4df9702 186 printf("DONE: CAN Init\n\r");
stebonicelli 13:698bd4df9702 187
stebonicelli 13:698bd4df9702 188
stebonicelli 13:698bd4df9702 189 printf("Running!\n\r");
stebonicelli 13:698bd4df9702 190
stebonicelli 13:698bd4df9702 191 while(true)
stebonicelli 13:698bd4df9702 192 {
stebonicelli 13:698bd4df9702 193 wait(1000);
clynamen 11:19e8022f60ea 194 }
clynamen 10:2acfa1a84c96 195 }