Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@18:841ab8c1bdbd, 2019-04-06 (annotated)

Committer:
stebonicelli
Date:
Sat Apr 06 17:00:28 2019 +0000
Revision:
18:841ab8c1bdbd
Parent:
17:2ab8feddb7c8
Child:
19:7f8c174448d0
commands update

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
gidiana 14:c51c4e0f3bc9 7 #define SPEED 100
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;
gidiana 17:2ab8feddb7c8 39 int current_pose[6];
stebonicelli 18:841ab8c1bdbd 40
stebonicelli 13:698bd4df9702 41 void canrx()
stebonicelli 13:698bd4df9702 42 {
stebonicelli 13:698bd4df9702 43 if(can1.read(messageIn, filter))
stebonicelli 13:698bd4df9702 44 {
gidiana 14:c51c4e0f3bc9 45 pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
gidiana 14:c51c4e0f3bc9 46 printf("CAN: mess %d\n\r", pose);
gidiana 14:c51c4e0f3bc9 47 printf("CAN: id %x \n\r ",messageIn.id);
stebonicelli 13:698bd4df9702 48
stebonicelli 13:698bd4df9702 49 if((messageIn.id & 0x0FF) == 0x40)
stebonicelli 13:698bd4df9702 50 {
gidiana 17:2ab8feddb7c8 51 //mutex.lock();
gidiana 17:2ab8feddb7c8 52 current_pose[0]=pose;
gidiana 17:2ab8feddb7c8 53 //mutex.unlock();
stebonicelli 13:698bd4df9702 54 }
stebonicelli 13:698bd4df9702 55 else if((messageIn.id & 0x0FF) == 0x50)
stebonicelli 13:698bd4df9702 56 {
gidiana 17:2ab8feddb7c8 57 //mutex.lock();
gidiana 17:2ab8feddb7c8 58 current_pose[1]=pose;
gidiana 17:2ab8feddb7c8 59 //mutex.unlock();
gidiana 14:c51c4e0f3bc9 60 }
gidiana 17:2ab8feddb7c8 61
stebonicelli 13:698bd4df9702 62 else if((messageIn.id & 0x0FF) == 0x60)
stebonicelli 13:698bd4df9702 63 {
gidiana 17:2ab8feddb7c8 64 //mutex.lock();
gidiana 17:2ab8feddb7c8 65 current_pose[2]=pose;
gidiana 17:2ab8feddb7c8 66 //mutex.unlock();
stebonicelli 13:698bd4df9702 67 }
stebonicelli 13:698bd4df9702 68 else if((messageIn.id & 0x0FF) == 0x70)
stebonicelli 13:698bd4df9702 69 {
gidiana 17:2ab8feddb7c8 70 //mutex.lock();
gidiana 17:2ab8feddb7c8 71 current_pose[3]=pose;
gidiana 17:2ab8feddb7c8 72 //mutex.unlock();
gidiana 17:2ab8feddb7c8 73 }
gidiana 16:5454456b36f7 74 else if((messageIn.id & 0x0FF) == 0x80)
gidiana 16:5454456b36f7 75 {
gidiana 17:2ab8feddb7c8 76 //mutex.lock();
gidiana 17:2ab8feddb7c8 77 current_pose[4]=pose;
gidiana 17:2ab8feddb7c8 78 //mutex.unlock();
gidiana 16:5454456b36f7 79 }
gidiana 16:5454456b36f7 80 else if((messageIn.id & 0x0FF) == 0x90)
gidiana 16:5454456b36f7 81 {
gidiana 17:2ab8feddb7c8 82 //mutex.lock();
gidiana 17:2ab8feddb7c8 83 current_pose[5]=pose;
gidiana 17:2ab8feddb7c8 84 //mutex.unlock();
gidiana 17:2ab8feddb7c8 85 }
stebonicelli 13:698bd4df9702 86 }
gidiana 17:2ab8feddb7c8 87
stebonicelli 13:698bd4df9702 88 }
ilaria 1:946c65496383 89
stebonicelli 13:698bd4df9702 90 int main()
stebonicelli 13:698bd4df9702 91 {
gidiana 17:2ab8feddb7c8 92 printf("DYNAMIXEL: Init \n\r");
gidiana 17:2ab8feddb7c8 93 led=1;
clynamen 11:19e8022f60ea 94 wire.trigger();
clynamen 11:19e8022f60ea 95 wire.trigger();
clynamen 11:19e8022f60ea 96 wire.trigger();
clynamen 11:19e8022f60ea 97 wire.trigger();
gidiana 17:2ab8feddb7c8 98 wait(5);
stebonicelli 13:698bd4df9702 99 // Setup Motor1 MultiTurn
stebonicelli 13:698bd4df9702 100 motor_1.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 101 motor_1.setMode(0);
gidiana 17:2ab8feddb7c8 102 //motor_1.setSpeed(0);
gidiana 17:2ab8feddb7c8 103 //printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition());
gidiana 17:2ab8feddb7c8 104
gidiana 17:2ab8feddb7c8 105 wait(5);
gidiana 17:2ab8feddb7c8 106 printf("DYNAMIXEL: Init DONE 1\n\r");
stebonicelli 13:698bd4df9702 107 // Setup Motor2 MultiTurn
stebonicelli 13:698bd4df9702 108 motor_2.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 109 motor_2.setMode(0);
gidiana 17:2ab8feddb7c8 110 //motor_2.setSpeed(0);
gidiana 17:2ab8feddb7c8 111 //printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition());
clynamen 11:19e8022f60ea 112
gidiana 17:2ab8feddb7c8 113 wait(5);
gidiana 17:2ab8feddb7c8 114 printf("DYNAMIXEL: Init DONE 2\n\r");
stebonicelli 13:698bd4df9702 115 // Setup Motor3 MultiTurn
stebonicelli 13:698bd4df9702 116 motor_3.setMotorEnabled(1);
gidiana 14:c51c4e0f3bc9 117 motor_3.setMode(0);
gidiana 17:2ab8feddb7c8 118 //motor_3.setSpeed(0);
gidiana 17:2ab8feddb7c8 119 //printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
gidiana 17:2ab8feddb7c8 120 wait(5);
gidiana 17:2ab8feddb7c8 121 printf("DYNAMIXEL: Init DONE 3\n\r");
stebonicelli 13:698bd4df9702 122 //motor_3.setGoalPosition(0);
gidiana 16:5454456b36f7 123 // Setup Motor4 MultiTurn
gidiana 17:2ab8feddb7c8 124 //motor_4.setMotorEnabled(1);
gidiana 17:2ab8feddb7c8 125 //motor_4.setMode(0);
gidiana 17:2ab8feddb7c8 126 //motor_4.setSpeed(0);
gidiana 17:2ab8feddb7c8 127 //printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition());
gidiana 17:2ab8feddb7c8 128 wait(5);
gidiana 17:2ab8feddb7c8 129
stebonicelli 13:698bd4df9702 130 printf("DYNAMIXEL: Init DONE\n\r");
clynamen 11:19e8022f60ea 131
stebonicelli 13:698bd4df9702 132 button.rise(&button_int_handler);
gidiana 17:2ab8feddb7c8 133 cam1=0,5;
gidiana 17:2ab8feddb7c8 134 cam2=0,5;
stebonicelli 13:698bd4df9702 135 // CAN Initialization
gidiana 17:2ab8feddb7c8 136 //canrxa.start(canrx);
clynamen 11:19e8022f60ea 137
stebonicelli 13:698bd4df9702 138 printf("DONE: CAN Init\n\r");
stebonicelli 13:698bd4df9702 139
stebonicelli 13:698bd4df9702 140
stebonicelli 13:698bd4df9702 141 printf("Running!\n\r");
stebonicelli 13:698bd4df9702 142
stebonicelli 13:698bd4df9702 143 while(true)
stebonicelli 13:698bd4df9702 144 {
gidiana 17:2ab8feddb7c8 145 canrx();
stebonicelli 18:841ab8c1bdbd 146 if (current_pose[0]==1)
gidiana 17:2ab8feddb7c8 147 {
gidiana 17:2ab8feddb7c8 148 motor_1.setSpeed(-SPEED);
gidiana 17:2ab8feddb7c8 149 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 17:2ab8feddb7c8 150 }
gidiana 17:2ab8feddb7c8 151
stebonicelli 18:841ab8c1bdbd 152 else if (current_pose[0]==0)
gidiana 17:2ab8feddb7c8 153 {
gidiana 17:2ab8feddb7c8 154 motor_1.setSpeed(0);
gidiana 17:2ab8feddb7c8 155 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 17:2ab8feddb7c8 156 }
stebonicelli 18:841ab8c1bdbd 157 else if (current_pose[0]==2)
gidiana 17:2ab8feddb7c8 158 {
gidiana 17:2ab8feddb7c8 159
gidiana 17:2ab8feddb7c8 160 motor_1.setSpeed(SPEED);
gidiana 17:2ab8feddb7c8 161 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
gidiana 17:2ab8feddb7c8 162
gidiana 17:2ab8feddb7c8 163 }
gidiana 17:2ab8feddb7c8 164
stebonicelli 18:841ab8c1bdbd 165 if (current_pose[1]==1)
gidiana 17:2ab8feddb7c8 166 {
gidiana 17:2ab8feddb7c8 167 motor_2.setSpeed(-SPEED);
gidiana 17:2ab8feddb7c8 168 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 17:2ab8feddb7c8 169 if (motor_2.getPosition()<-90) motor_2.setSpeed(0);
gidiana 17:2ab8feddb7c8 170 }
gidiana 17:2ab8feddb7c8 171
stebonicelli 18:841ab8c1bdbd 172 else if (current_pose[1]==0)
gidiana 17:2ab8feddb7c8 173 {
gidiana 17:2ab8feddb7c8 174 motor_2.setSpeed(0);
gidiana 17:2ab8feddb7c8 175 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 17:2ab8feddb7c8 176 }
stebonicelli 18:841ab8c1bdbd 177 else if (current_pose[1]==2)
gidiana 17:2ab8feddb7c8 178 {
gidiana 17:2ab8feddb7c8 179
gidiana 17:2ab8feddb7c8 180 motor_2.setSpeed(SPEED);
gidiana 17:2ab8feddb7c8 181 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
gidiana 17:2ab8feddb7c8 182 if (motor_2.getPosition()>90) motor_2.setSpeed(0);
gidiana 17:2ab8feddb7c8 183 }
gidiana 17:2ab8feddb7c8 184
stebonicelli 18:841ab8c1bdbd 185 if (current_pose[2]==1)
gidiana 17:2ab8feddb7c8 186 {
gidiana 17:2ab8feddb7c8 187 motor_3.setSpeed(-SPEED);
gidiana 17:2ab8feddb7c8 188 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 17:2ab8feddb7c8 189 }
gidiana 17:2ab8feddb7c8 190
stebonicelli 18:841ab8c1bdbd 191 else if (current_pose[2]==0)
gidiana 17:2ab8feddb7c8 192 {
gidiana 17:2ab8feddb7c8 193 motor_3.setSpeed(0);
gidiana 17:2ab8feddb7c8 194 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 17:2ab8feddb7c8 195 }
stebonicelli 18:841ab8c1bdbd 196 else if (current_pose[2]==2)
gidiana 17:2ab8feddb7c8 197 {
gidiana 17:2ab8feddb7c8 198
gidiana 17:2ab8feddb7c8 199 motor_3.setSpeed(SPEED);
gidiana 17:2ab8feddb7c8 200 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
gidiana 17:2ab8feddb7c8 201
gidiana 17:2ab8feddb7c8 202 }
gidiana 17:2ab8feddb7c8 203
stebonicelli 18:841ab8c1bdbd 204 if (current_pose[3]==1)
gidiana 17:2ab8feddb7c8 205 {
gidiana 17:2ab8feddb7c8 206 motor_4.setSpeed(-SPEED);
gidiana 17:2ab8feddb7c8 207 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
gidiana 17:2ab8feddb7c8 208 }
gidiana 17:2ab8feddb7c8 209
stebonicelli 18:841ab8c1bdbd 210 else if (current_pose[3]==0)
gidiana 17:2ab8feddb7c8 211 {
gidiana 17:2ab8feddb7c8 212 motor_4.setSpeed(0);
gidiana 17:2ab8feddb7c8 213 printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition());
gidiana 17:2ab8feddb7c8 214 }
stebonicelli 18:841ab8c1bdbd 215 else if (current_pose[3]==2)
gidiana 17:2ab8feddb7c8 216 {
gidiana 17:2ab8feddb7c8 217
gidiana 17:2ab8feddb7c8 218 motor_4.setSpeed(SPEED);
gidiana 17:2ab8feddb7c8 219 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
gidiana 17:2ab8feddb7c8 220 }
gidiana 17:2ab8feddb7c8 221
stebonicelli 18:841ab8c1bdbd 222 if (current_pose[4]==1)
gidiana 17:2ab8feddb7c8 223 {
gidiana 17:2ab8feddb7c8 224 if (cam1==0)
gidiana 17:2ab8feddb7c8 225 cam1=cam1;
gidiana 17:2ab8feddb7c8 226 else cam1 = cam1-0.2;
gidiana 17:2ab8feddb7c8 227
gidiana 17:2ab8feddb7c8 228 wait(0.2);
gidiana 17:2ab8feddb7c8 229
gidiana 17:2ab8feddb7c8 230 }
stebonicelli 18:841ab8c1bdbd 231 else if (current_pose[4]==2)
gidiana 17:2ab8feddb7c8 232 {
gidiana 17:2ab8feddb7c8 233
gidiana 17:2ab8feddb7c8 234 if (cam1==1)
gidiana 17:2ab8feddb7c8 235 cam1=cam1;
gidiana 17:2ab8feddb7c8 236 else cam1 = cam1+0.2;
gidiana 17:2ab8feddb7c8 237
gidiana 17:2ab8feddb7c8 238 wait(0.2);
gidiana 17:2ab8feddb7c8 239 }
stebonicelli 18:841ab8c1bdbd 240
stebonicelli 18:841ab8c1bdbd 241 if (current_pose[5]==1)
gidiana 17:2ab8feddb7c8 242 {
gidiana 17:2ab8feddb7c8 243 if (cam2==0)
gidiana 17:2ab8feddb7c8 244 cam1=cam1;
gidiana 17:2ab8feddb7c8 245 else cam2 = cam2-0.2;
gidiana 17:2ab8feddb7c8 246
gidiana 17:2ab8feddb7c8 247 wait(0.2);
gidiana 17:2ab8feddb7c8 248
gidiana 17:2ab8feddb7c8 249 }
stebonicelli 18:841ab8c1bdbd 250 else if (current_pose[5]==2)
gidiana 17:2ab8feddb7c8 251 {
gidiana 17:2ab8feddb7c8 252
gidiana 17:2ab8feddb7c8 253 if (cam2==1)
gidiana 17:2ab8feddb7c8 254 cam2=cam2;
gidiana 17:2ab8feddb7c8 255 else cam2 = cam2+0.2;
gidiana 17:2ab8feddb7c8 256
gidiana 17:2ab8feddb7c8 257 wait(0.2);
stebonicelli 18:841ab8c1bdbd 258 }
clynamen 11:19e8022f60ea 259 }
clynamen 10:2acfa1a84c96 260 }