Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@17:2ab8feddb7c8, 2019-02-09 (annotated)

Committer:
gidiana
Date:
Sat Feb 09 16:53:04 2019 +0000
Revision:
17:2ab8feddb7c8
Parent:
16:5454456b36f7
Child:
18:841ab8c1bdbd
init

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