Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@16:5454456b36f7, 2019-02-08 (annotated)

Committer:
gidiana
Date:
Fri Feb 08 20:36:19 2019 +0000
Revision:
16:5454456b36f7
Parent:
15:d058877eb501
Child:
17:2ab8feddb7c8
servo arm camera

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