Team DIANA / Mbed OS Arm_dynamixel_DEVEL

To be tested

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@25:1aa10432cda2, 2019-09-05 (annotated)

Committer:
gidiana
Date:
Thu Sep 05 20:26:40 2019 +0000
Revision:
25:1aa10432cda2
Parent:
24:adb6bac314d7 
revision and more verbosity

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 22:ffb26af4d5d8 5 #include <stdio.h>
gidiana 22:ffb26af4d5d8 6 #include <string.h>
gidiana 23:47b8c7f9813e 7 #include "Servo.h"
gidiana 22:ffb26af4d5d8 8
gidiana 22:ffb26af4d5d8 9
gidiana 22:ffb26af4d5d8 10 #define VERBOSE 1
stebonicelli 18:841ab8c1bdbd 11
gidiana 22:ffb26af4d5d8 12 #define CAN_FREQUENCY 125000
gidiana 22:ffb26af4d5d8 13 #define BAUDRATE 57600
gidiana 22:ffb26af4d5d8 14 #define SLEEP 0.02
gidiana 21:43740448011a 15
gidiana 22:ffb26af4d5d8 16
gidiana 23:47b8c7f9813e 17 typedef enum
gidiana 23:47b8c7f9813e 18 {
gidiana 23:47b8c7f9813e 19 JOINT_SET_SPEED = 20,
gidiana 23:47b8c7f9813e 20 JOINT_SET_POSITION,
gidiana 23:47b8c7f9813e 21 JOINT_CURRENT_POSITION,
gidiana 23:47b8c7f9813e 22 JOINT_CURRENT_SPEED,
gidiana 23:47b8c7f9813e 23 JOINT_STATUS,
gidiana 23:47b8c7f9813e 24 JOINT_ERROR,
gidiana 23:47b8c7f9813e 25 JOINT_TORQUE,
gidiana 23:47b8c7f9813e 26 JOINT_MAXTORQUE,
gidiana 23:47b8c7f9813e 27 JOINT_ZERO,
gidiana 23:47b8c7f9813e 28 }CAN_COMMANDS;
gidiana 23:47b8c7f9813e 29
gidiana 23:47b8c7f9813e 30 typedef enum
gidiana 23:47b8c7f9813e 31 {
gidiana 23:47b8c7f9813e 32 BASE=1,
gidiana 23:47b8c7f9813e 33 SHOULDER,
gidiana 23:47b8c7f9813e 34 ELBOW,
gidiana 23:47b8c7f9813e 35 WRIST1,
gidiana 23:47b8c7f9813e 36 WRIST2,
gidiana 23:47b8c7f9813e 37 WRIST3,
gidiana 23:47b8c7f9813e 38 END_EFFECTOR,
gidiana 23:47b8c7f9813e 39 CAMERA1,
gidiana 23:47b8c7f9813e 40 CAMERA2,
gidiana 23:47b8c7f9813e 41 }JOINT;
gidiana 24:adb6bac314d7 42 Servo camera(PA_8);
gidiana 24:adb6bac314d7 43 DigitalOut dyn_enable(PC_11);
gidiana 22:ffb26af4d5d8 44 int dxl_id[]={1,2,3,4};
gidiana 23:47b8c7f9813e 45 JOINT joint_id[]={WRIST1,WRIST2,WRIST3,CAMERA1,CAMERA2};
gidiana 25:1aa10432cda2 46 int dxl_speed[]={25,12,25,25};
gidiana 22:ffb26af4d5d8 47 float dxl_gear[]={3,2,3,1};
gidiana 22:ffb26af4d5d8 48 float dxl_present_position[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 49 float dxl_goal_position[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 50 float dxl_offset[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 51 float dxl_current[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 52 float dxl_torque[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 53 float dxl_max_torque[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 54 float dxl_reset[]={0,0,0,0};
gidiana 22:ffb26af4d5d8 55 uint16_t dxl_temperature[]={0,0,0,0};
gidiana 22:ffb26af4d5d8 56 uint16_t dxl_limit[]={0,0,0,0};
gidiana 23:47b8c7f9813e 57 float camera_pose=0;
gidiana 22:ffb26af4d5d8 58 int present, current, torque, temperature;
gidiana 22:ffb26af4d5d8 59 int status;
gidiana 22:ffb26af4d5d8 60
gidiana 24:adb6bac314d7 61 DigitalOut led(PA_15);
stebonicelli 13:698bd4df9702 62
gidiana 24:adb6bac314d7 63 communication_1 wire(PB_10, PC_5, BAUDRATE);
stebonicelli 18:841ab8c1bdbd 64
gidiana 22:ffb26af4d5d8 65 MX106 w_1(wire, dxl_id[0], dxl_gear[0]);
gidiana 22:ffb26af4d5d8 66 MX106 w_2(wire, dxl_id[1], dxl_gear[1]);
gidiana 22:ffb26af4d5d8 67 MX106 w_3(wire, dxl_id[2], dxl_gear[2]);
gidiana 22:ffb26af4d5d8 68 AX12 a_1(wire, dxl_id[3], dxl_gear[3]);
stebonicelli 18:841ab8c1bdbd 69
gidiana 24:adb6bac314d7 70 CAN can1(PA_11, PA_12); // RX, TX
stebonicelli 13:698bd4df9702 71
stebonicelli 13:698bd4df9702 72 CANMessage messageIn;
stebonicelli 13:698bd4df9702 73 CANMessage messageOut;
stebonicelli 13:698bd4df9702 74
gidiana 22:ffb26af4d5d8 75
stebonicelli 18:841ab8c1bdbd 76
gidiana 23:47b8c7f9813e 77 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
gidiana 23:47b8c7f9813e 78 {
gidiana 23:47b8c7f9813e 79 uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
gidiana 23:47b8c7f9813e 80 id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
gidiana 23:47b8c7f9813e 81 id |= 0x80000000; // Send in Extended Frame Format.
gidiana 23:47b8c7f9813e 82 return id;
gidiana 23:47b8c7f9813e 83 }
gidiana 23:47b8c7f9813e 84
gidiana 23:47b8c7f9813e 85
gidiana 23:47b8c7f9813e 86
gidiana 22:ffb26af4d5d8 87 int main ()
stebonicelli 13:698bd4df9702 88 {
gidiana 22:ffb26af4d5d8 89 #if VERBOSE
gidiana 22:ffb26af4d5d8 90 printf("START \n\r");
gidiana 22:ffb26af4d5d8 91 #endif
gidiana 22:ffb26af4d5d8 92 can1.frequency(CAN_FREQUENCY);
gidiana 22:ffb26af4d5d8 93 messageIn.format=CANExtended;
gidiana 22:ffb26af4d5d8 94 messageOut.format=CANExtended;
gidiana 24:adb6bac314d7 95 dyn_enable=1;
gidiana 22:ffb26af4d5d8 96 wire.trigger();
gidiana 22:ffb26af4d5d8 97 wire.trigger();
gidiana 22:ffb26af4d5d8 98 wire.trigger();
gidiana 22:ffb26af4d5d8 99 wire.trigger();
gidiana 22:ffb26af4d5d8 100
gidiana 22:ffb26af4d5d8 101 #if VERBOSE
gidiana 22:ffb26af4d5d8 102 printf("DYNAMIXEL: Init START \n\r");
gidiana 22:ffb26af4d5d8 103 #endif
gidiana 22:ffb26af4d5d8 104 w_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 105 wait(SLEEP);
gidiana 22:ffb26af4d5d8 106 #if VERBOSE
gidiana 22:ffb26af4d5d8 107 printf("DYNAMIXEL: Init 1 \n\r");
gidiana 22:ffb26af4d5d8 108 #endif
gidiana 22:ffb26af4d5d8 109 w_2.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 110 wait(SLEEP);
gidiana 22:ffb26af4d5d8 111 #if VERBOSE
gidiana 22:ffb26af4d5d8 112 printf("DYNAMIXEL: Init 2 \n\r");
gidiana 22:ffb26af4d5d8 113 #endif
gidiana 22:ffb26af4d5d8 114 w_3.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 115 wait(SLEEP);
gidiana 22:ffb26af4d5d8 116 #if VERBOSE
gidiana 22:ffb26af4d5d8 117 printf("DYNAMIXEL: Init 3\n\r");
gidiana 22:ffb26af4d5d8 118 #endif
gidiana 22:ffb26af4d5d8 119 a_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 120 wait(SLEEP);
gidiana 22:ffb26af4d5d8 121 #if VERBOSE
gidiana 22:ffb26af4d5d8 122 printf("DYNAMIXEL: Init 4\n\r");
gidiana 22:ffb26af4d5d8 123 #endif
stebonicelli 19:7f8c174448d0 124
gidiana 22:ffb26af4d5d8 125 w_1.setMode(2);
gidiana 22:ffb26af4d5d8 126 wait(SLEEP);
gidiana 22:ffb26af4d5d8 127 w_1.setMaxSpeed(dxl_speed[0]);
gidiana 22:ffb26af4d5d8 128 wait(SLEEP);
gidiana 22:ffb26af4d5d8 129 #if VERBOSE
gidiana 22:ffb26af4d5d8 130 printf("DYNAMIXEL 1: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 131 #endif
gidiana 22:ffb26af4d5d8 132 w_2.setMode(2);
gidiana 22:ffb26af4d5d8 133 wait(SLEEP);
gidiana 22:ffb26af4d5d8 134 w_2.setMaxSpeed(dxl_speed[1]);
gidiana 22:ffb26af4d5d8 135 wait(SLEEP);
gidiana 24:adb6bac314d7 136 //w_2.setCWLimitUnits(1900);
gidiana 22:ffb26af4d5d8 137 wait(SLEEP);
gidiana 24:adb6bac314d7 138 //w_2.setCCWLimitUnits(-1900);
gidiana 22:ffb26af4d5d8 139 wait(SLEEP);
gidiana 22:ffb26af4d5d8 140 #if VERBOSE
gidiana 22:ffb26af4d5d8 141 printf("DYNAMIXEL 2: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 142 #endif
gidiana 22:ffb26af4d5d8 143 w_3.setMode(2);
gidiana 22:ffb26af4d5d8 144 wait(SLEEP);
gidiana 22:ffb26af4d5d8 145 w_3.setMaxSpeed(dxl_speed[2]);
gidiana 22:ffb26af4d5d8 146 wait(SLEEP);
gidiana 22:ffb26af4d5d8 147 #if VERBOSE
gidiana 22:ffb26af4d5d8 148 printf("DYNAMIXEL 3: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 149 #endif
gidiana 24:adb6bac314d7 150 /*
gidiana 24:adb6bac314d7 151 a_1.setMode(2);
gidiana 22:ffb26af4d5d8 152 wait(SLEEP);
gidiana 22:ffb26af4d5d8 153 a_1.setMaxSpeed(dxl_speed[3]);
gidiana 22:ffb26af4d5d8 154 wait(SLEEP);
gidiana 22:ffb26af4d5d8 155 #if VERBOSE
gidiana 22:ffb26af4d5d8 156 printf("DYNAMIXEL 4: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 157 #endif
gidiana 24:adb6bac314d7 158 */
gidiana 22:ffb26af4d5d8 159 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 22:ffb26af4d5d8 160 wait(SLEEP);
gidiana 22:ffb26af4d5d8 161 w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
gidiana 22:ffb26af4d5d8 162 wait(SLEEP);
gidiana 22:ffb26af4d5d8 163 w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
gidiana 22:ffb26af4d5d8 164 wait(SLEEP);
gidiana 24:adb6bac314d7 165
gidiana 24:adb6bac314d7 166 //a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
gidiana 24:adb6bac314d7 167 //wait(SLEEP);
gidiana 22:ffb26af4d5d8 168
gidiana 22:ffb26af4d5d8 169 wait(1);
stebonicelli 19:7f8c174448d0 170
gidiana 22:ffb26af4d5d8 171 while (1)
gidiana 22:ffb26af4d5d8 172 {
gidiana 22:ffb26af4d5d8 173 if(can1.read(messageIn))
gidiana 22:ffb26af4d5d8 174 {
gidiana 22:ffb26af4d5d8 175 #if VERBOSE
gidiana 22:ffb26af4d5d8 176 printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
gidiana 22:ffb26af4d5d8 177 #endif
gidiana 22:ffb26af4d5d8 178 led!=led;
gidiana 23:47b8c7f9813e 179 if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST1))
gidiana 22:ffb26af4d5d8 180 {
gidiana 22:ffb26af4d5d8 181 dxl_goal_position[0] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 182 }
gidiana 23:47b8c7f9813e 183 if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST2))
gidiana 22:ffb26af4d5d8 184 {
gidiana 22:ffb26af4d5d8 185 dxl_goal_position[1] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 186 }
gidiana 23:47b8c7f9813e 187 if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST3))
gidiana 22:ffb26af4d5d8 188 {
gidiana 22:ffb26af4d5d8 189 dxl_goal_position[2] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 190 }
gidiana 23:47b8c7f9813e 191 if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA1))
gidiana 22:ffb26af4d5d8 192 {
gidiana 22:ffb26af4d5d8 193 dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 194 }
gidiana 23:47b8c7f9813e 195 if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA2))
gidiana 23:47b8c7f9813e 196 {
gidiana 23:47b8c7f9813e 197 camera_pose = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3])/100;
gidiana 23:47b8c7f9813e 198 }
clynamen 11:19e8022f60ea 199
gidiana 23:47b8c7f9813e 200 if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST1))
gidiana 22:ffb26af4d5d8 201 {
gidiana 22:ffb26af4d5d8 202 dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 203 }
gidiana 23:47b8c7f9813e 204 if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST2))
gidiana 22:ffb26af4d5d8 205 {
gidiana 22:ffb26af4d5d8 206 dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 207 }
gidiana 23:47b8c7f9813e 208 if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST3))
gidiana 22:ffb26af4d5d8 209 {
gidiana 22:ffb26af4d5d8 210 dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 211 }
gidiana 23:47b8c7f9813e 212 if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, CAMERA1))
gidiana 22:ffb26af4d5d8 213 {
gidiana 22:ffb26af4d5d8 214 dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 215 }
gidiana 23:47b8c7f9813e 216
gidiana 23:47b8c7f9813e 217 if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST1))
gidiana 23:47b8c7f9813e 218 {
gidiana 23:47b8c7f9813e 219 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 23:47b8c7f9813e 220 {
gidiana 23:47b8c7f9813e 221 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 23:47b8c7f9813e 222 wait(SLEEP);
gidiana 23:47b8c7f9813e 223 }
gidiana 23:47b8c7f9813e 224 }
gidiana 23:47b8c7f9813e 225 if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST2))
gidiana 23:47b8c7f9813e 226 {
gidiana 23:47b8c7f9813e 227 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 23:47b8c7f9813e 228 {
gidiana 23:47b8c7f9813e 229 w_2.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 23:47b8c7f9813e 230 wait(SLEEP);
gidiana 23:47b8c7f9813e 231 }
gidiana 23:47b8c7f9813e 232 }
gidiana 23:47b8c7f9813e 233 if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST3))
gidiana 23:47b8c7f9813e 234 {
gidiana 23:47b8c7f9813e 235 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 23:47b8c7f9813e 236 {
gidiana 23:47b8c7f9813e 237 w_3.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 23:47b8c7f9813e 238 wait(SLEEP);
gidiana 23:47b8c7f9813e 239 }
gidiana 23:47b8c7f9813e 240 }
gidiana 23:47b8c7f9813e 241 if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA1))
gidiana 23:47b8c7f9813e 242 {
gidiana 23:47b8c7f9813e 243 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 23:47b8c7f9813e 244 {
gidiana 23:47b8c7f9813e 245 a_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 23:47b8c7f9813e 246 wait(SLEEP);
gidiana 23:47b8c7f9813e 247 }
gidiana 23:47b8c7f9813e 248 }
gidiana 23:47b8c7f9813e 249 if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA2))
gidiana 23:47b8c7f9813e 250 {
gidiana 23:47b8c7f9813e 251 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 23:47b8c7f9813e 252 {
gidiana 23:47b8c7f9813e 253 camera=0;
gidiana 23:47b8c7f9813e 254 }
gidiana 23:47b8c7f9813e 255 }
gidiana 22:ffb26af4d5d8 256 }
stebonicelli 19:7f8c174448d0 257
gidiana 25:1aa10432cda2 258
gidiana 25:1aa10432cda2 259 #if VERBOSE
gidiana 25:1aa10432cda2 260 printf("DYNAMIXEL SET GOAL POSITION\n\r");
gidiana 25:1aa10432cda2 261 #endif
gidiana 25:1aa10432cda2 262
gidiana 22:ffb26af4d5d8 263 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 22:ffb26af4d5d8 264 wait(SLEEP);
gidiana 22:ffb26af4d5d8 265 w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
gidiana 22:ffb26af4d5d8 266 wait(SLEEP);
gidiana 22:ffb26af4d5d8 267 w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
gidiana 22:ffb26af4d5d8 268 wait(SLEEP);
gidiana 22:ffb26af4d5d8 269 a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
gidiana 22:ffb26af4d5d8 270 wait(SLEEP);
gidiana 23:47b8c7f9813e 271 camera=camera_pose;
gidiana 25:1aa10432cda2 272
gidiana 25:1aa10432cda2 273 #if VERBOSE
gidiana 25:1aa10432cda2 274 printf("DYNAMIXEL PRESENT POSITION\n\r");
gidiana 25:1aa10432cda2 275 #endif
gidiana 25:1aa10432cda2 276
gidiana 22:ffb26af4d5d8 277 dxl_present_position[0]=w_1.getPosition();
gidiana 22:ffb26af4d5d8 278 wait(SLEEP);
gidiana 22:ffb26af4d5d8 279 dxl_present_position[1]=w_2.getPosition();
gidiana 22:ffb26af4d5d8 280 wait(SLEEP);
gidiana 22:ffb26af4d5d8 281 dxl_present_position[2]=w_3.getPosition();
gidiana 22:ffb26af4d5d8 282 wait(SLEEP);
gidiana 22:ffb26af4d5d8 283 dxl_present_position[3]=a_1.getPosition();
gidiana 22:ffb26af4d5d8 284 wait(SLEEP);
stebonicelli 19:7f8c174448d0 285
gidiana 22:ffb26af4d5d8 286 #if VERBOSE
gidiana 22:ffb26af4d5d8 287 printf("DYNAMIXEL PRESENT POSITION %f %f %f %f\n\r", dxl_present_position[0],dxl_present_position[1], dxl_present_position[2] ,dxl_present_position[3] );
gidiana 22:ffb26af4d5d8 288 #endif
gidiana 25:1aa10432cda2 289 #if VERBOSE
gidiana 25:1aa10432cda2 290 printf("DYNAMIXEL CURRENT\n\r");
gidiana 25:1aa10432cda2 291 #endif
gidiana 22:ffb26af4d5d8 292 dxl_current[0]=w_1.getCurrent();
gidiana 22:ffb26af4d5d8 293 wait(SLEEP);
gidiana 22:ffb26af4d5d8 294 dxl_current[1]=w_2.getCurrent();
gidiana 22:ffb26af4d5d8 295 wait(SLEEP);
gidiana 22:ffb26af4d5d8 296 dxl_current[2]=w_3.getCurrent();
gidiana 22:ffb26af4d5d8 297 wait(SLEEP);
gidiana 22:ffb26af4d5d8 298 //dxl_current[3]=a_1.getCurrent();
gidiana 22:ffb26af4d5d8 299 //wait(SLEEP);
gidiana 22:ffb26af4d5d8 300 #if VERBOSE
gidiana 22:ffb26af4d5d8 301 printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] );
gidiana 22:ffb26af4d5d8 302 #endif
gidiana 25:1aa10432cda2 303 #if VERBOSE
gidiana 25:1aa10432cda2 304 printf("DYNAMIXEL TORQUE\n\r");
gidiana 25:1aa10432cda2 305 #endif
gidiana 22:ffb26af4d5d8 306 dxl_torque[0]=dxl_current[0]*0.875;
gidiana 22:ffb26af4d5d8 307 dxl_torque[1]=dxl_current[1]*0.875;
gidiana 22:ffb26af4d5d8 308 dxl_torque[2]=dxl_current[2]*0.875;
gidiana 22:ffb26af4d5d8 309 //dxl_torque[3]=dxl_current[3]*0.875;
gidiana 22:ffb26af4d5d8 310 #if VERBOSE
gidiana 22:ffb26af4d5d8 311 printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] );
gidiana 22:ffb26af4d5d8 312 #endif
gidiana 25:1aa10432cda2 313 #if VERBOSE
gidiana 25:1aa10432cda2 314 printf("DYNAMIXEL TEMP\n\r");
gidiana 25:1aa10432cda2 315 #endif
gidiana 22:ffb26af4d5d8 316 dxl_temperature[0]=w_1.getTemp();
gidiana 22:ffb26af4d5d8 317 wait(SLEEP);
gidiana 22:ffb26af4d5d8 318 dxl_temperature[1]=w_2.getTemp();
gidiana 22:ffb26af4d5d8 319 wait(SLEEP);
gidiana 22:ffb26af4d5d8 320 dxl_temperature[2]=w_3.getTemp();
gidiana 22:ffb26af4d5d8 321 wait(SLEEP);
gidiana 22:ffb26af4d5d8 322 dxl_temperature[3]=a_1.getTemp();
gidiana 22:ffb26af4d5d8 323 wait(SLEEP);
gidiana 22:ffb26af4d5d8 324
gidiana 22:ffb26af4d5d8 325 #if VERBOSE
gidiana 22:ffb26af4d5d8 326 printf("DYNAMIXEL TEMPERATURE %d %d %d %d\n\r", dxl_temperature[0],dxl_temperature[1], dxl_temperature[2] ,dxl_temperature[3] );
gidiana 22:ffb26af4d5d8 327 #endif
gidiana 22:ffb26af4d5d8 328
gidiana 22:ffb26af4d5d8 329 for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++)
gidiana 22:ffb26af4d5d8 330 {
gidiana 23:47b8c7f9813e 331 messageOut.len = 4;
gidiana 22:ffb26af4d5d8 332 present=(int)(dxl_present_position[i]*100);
gidiana 22:ffb26af4d5d8 333 current=(int)(dxl_current[i]*100);
gidiana 22:ffb26af4d5d8 334 torque=(int)(dxl_torque[i]*100);
gidiana 22:ffb26af4d5d8 335 temperature=(int)(dxl_temperature[i]*100);
gidiana 22:ffb26af4d5d8 336
gidiana 23:47b8c7f9813e 337 messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, joint_id[i]);
gidiana 23:47b8c7f9813e 338
gidiana 23:47b8c7f9813e 339 messageOut.data[0] = (present >> 24);
gidiana 23:47b8c7f9813e 340 messageOut.data[1] = (present>>16);
gidiana 23:47b8c7f9813e 341 messageOut.data[2] = (present>>8);
gidiana 23:47b8c7f9813e 342 messageOut.data[3] = (present);
gidiana 23:47b8c7f9813e 343 status = can1.write(messageOut);
gidiana 23:47b8c7f9813e 344 #if VERBOSE
gidiana 23:47b8c7f9813e 345 printf("CAN send present position Joint %d : %d", joint_id[i] , status );
gidiana 23:47b8c7f9813e 346 #endif
gidiana 23:47b8c7f9813e 347
gidiana 23:47b8c7f9813e 348 messageOut.id = gen_can_id(JOINT_STATUS, joint_id[i]);
gidiana 22:ffb26af4d5d8 349
gidiana 23:47b8c7f9813e 350 messageOut.data[0] = (current >> 8);
gidiana 23:47b8c7f9813e 351 messageOut.data[1] = (current);
gidiana 23:47b8c7f9813e 352 messageOut.data[2] = (temperature >> 8);
gidiana 23:47b8c7f9813e 353 messageOut.data[3] = (temperature);
gidiana 23:47b8c7f9813e 354 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 355
gidiana 23:47b8c7f9813e 356 #if VERBOSE
gidiana 23:47b8c7f9813e 357 printf("CAN send status Joint %d : %d", joint_id[i] , status );
gidiana 23:47b8c7f9813e 358 #endif
gidiana 23:47b8c7f9813e 359 messageOut.id = gen_can_id(JOINT_TORQUE, joint_id[i]);
gidiana 23:47b8c7f9813e 360
gidiana 23:47b8c7f9813e 361 messageOut.data[0] = (torque >> 24);
gidiana 23:47b8c7f9813e 362 messageOut.data[1] = (torque >> 16);
gidiana 23:47b8c7f9813e 363 messageOut.data[2] = (torque >> 8);
gidiana 23:47b8c7f9813e 364 messageOut.data[3] = (torque);
gidiana 22:ffb26af4d5d8 365 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 366
gidiana 22:ffb26af4d5d8 367 #if VERBOSE
gidiana 22:ffb26af4d5d8 368 printf("CAN send status Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 369 #endif
gidiana 22:ffb26af4d5d8 370 }
gidiana 23:47b8c7f9813e 371 int _camera_pose = camera_pose*100;
gidiana 23:47b8c7f9813e 372 messageOut.len = 4;
gidiana 23:47b8c7f9813e 373 messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, CAMERA2);
gidiana 23:47b8c7f9813e 374 messageOut.data[0] = (_camera_pose >> 24);
gidiana 23:47b8c7f9813e 375 messageOut.data[1] = (_camera_pose >> 16);
gidiana 23:47b8c7f9813e 376 messageOut.data[2] = (_camera_pose >> 8);
gidiana 23:47b8c7f9813e 377 messageOut.data[3] = (_camera_pose);
gidiana 23:47b8c7f9813e 378 status = can1.write(messageOut);
gidiana 23:47b8c7f9813e 379
gidiana 23:47b8c7f9813e 380 #if VERBOSE
gidiana 23:47b8c7f9813e 381
gidiana 23:47b8c7f9813e 382 printf("CAN send present position Joint %d : %d", CAMERA2 , status );
gidiana 23:47b8c7f9813e 383
gidiana 23:47b8c7f9813e 384 #endif
gidiana 22:ffb26af4d5d8 385
gidiana 25:1aa10432cda2 386 #if VERBOSE
gidiana 25:1aa10432cda2 387 printf("DYNAMIXEL TORQUE MAX\n\r");
gidiana 25:1aa10432cda2 388 #endif
gidiana 22:ffb26af4d5d8 389
gidiana 22:ffb26af4d5d8 390
gidiana 22:ffb26af4d5d8 391 dxl_max_torque[0]=w_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 392 wait(SLEEP);
gidiana 22:ffb26af4d5d8 393 dxl_max_torque[1]=w_2.getTorqueMax();
gidiana 22:ffb26af4d5d8 394 wait(SLEEP);
gidiana 22:ffb26af4d5d8 395 dxl_max_torque[2]=w_3.getTorqueMax();
gidiana 22:ffb26af4d5d8 396 wait(SLEEP);
gidiana 22:ffb26af4d5d8 397 dxl_max_torque[3]=a_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 398 wait(SLEEP);
gidiana 22:ffb26af4d5d8 399
gidiana 22:ffb26af4d5d8 400 for (int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++ )
gidiana 22:ffb26af4d5d8 401 {
gidiana 22:ffb26af4d5d8 402
gidiana 22:ffb26af4d5d8 403 messageOut.len = 4;
gidiana 23:47b8c7f9813e 404 messageOut.id= gen_can_id(JOINT_ERROR, joint_id[i]);
gidiana 22:ffb26af4d5d8 405
gidiana 23:47b8c7f9813e 406 messageOut.data[0]= ((int)(dxl_max_torque[i]) >> 24);
gidiana 23:47b8c7f9813e 407 messageOut.data[1]= ((int)(dxl_max_torque[i]) >> 16);
gidiana 23:47b8c7f9813e 408 messageOut.data[2]= ((int)(dxl_max_torque[i]) >> 8);
gidiana 23:47b8c7f9813e 409 messageOut.data[3]= ((int)(dxl_max_torque[i]) );
gidiana 22:ffb26af4d5d8 410 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 411
gidiana 22:ffb26af4d5d8 412 #if VERBOSE
gidiana 22:ffb26af4d5d8 413 printf("CAN send status ERROR Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 414 #endif
gidiana 22:ffb26af4d5d8 415
gidiana 22:ffb26af4d5d8 416 }
gidiana 22:ffb26af4d5d8 417 if(dxl_reset[0]!=0)
gidiana 22:ffb26af4d5d8 418 {
gidiana 22:ffb26af4d5d8 419 w_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 420 wait(SLEEP);
gidiana 22:ffb26af4d5d8 421 w_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 422 wait(SLEEP);
gidiana 22:ffb26af4d5d8 423 w_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 424 wait(SLEEP);
gidiana 22:ffb26af4d5d8 425 w_1.setGoal(0-dxl_offset[0]);
gidiana 22:ffb26af4d5d8 426 wait(SLEEP);
gidiana 22:ffb26af4d5d8 427 #if VERBOSE
gidiana 22:ffb26af4d5d8 428 printf("Dynamixel %d : RESET", joint_id[0] );
gidiana 22:ffb26af4d5d8 429 #endif
gidiana 22:ffb26af4d5d8 430 }
gidiana 22:ffb26af4d5d8 431 if(dxl_reset[1]!=0)
gidiana 22:ffb26af4d5d8 432 {
gidiana 22:ffb26af4d5d8 433 w_2.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 434 wait(SLEEP);
gidiana 22:ffb26af4d5d8 435 w_2.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 436 wait(SLEEP);
gidiana 22:ffb26af4d5d8 437 w_2.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 438 wait(SLEEP);
gidiana 22:ffb26af4d5d8 439 w_2.setGoal(0-dxl_offset[1]);
gidiana 22:ffb26af4d5d8 440 wait(SLEEP);
gidiana 22:ffb26af4d5d8 441 #if VERBOSE
gidiana 22:ffb26af4d5d8 442 printf("Dynamixel %d : RESET", joint_id[1] );
gidiana 22:ffb26af4d5d8 443 #endif
gidiana 22:ffb26af4d5d8 444 }
gidiana 22:ffb26af4d5d8 445 if(dxl_reset[2]!=0)
gidiana 22:ffb26af4d5d8 446 {
gidiana 22:ffb26af4d5d8 447 w_3.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 448 wait(SLEEP);
gidiana 22:ffb26af4d5d8 449 w_3.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 450 wait(SLEEP);
gidiana 22:ffb26af4d5d8 451 w_3.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 452 wait(SLEEP);
gidiana 22:ffb26af4d5d8 453 w_3.setGoal(0-dxl_offset[2]);
gidiana 22:ffb26af4d5d8 454 wait(SLEEP);
gidiana 22:ffb26af4d5d8 455 #if VERBOSE
gidiana 22:ffb26af4d5d8 456 printf("Dynamixel %d : RESET", joint_id[2] );
gidiana 22:ffb26af4d5d8 457 #endif
gidiana 22:ffb26af4d5d8 458 }
gidiana 23:47b8c7f9813e 459 /*
gidiana 22:ffb26af4d5d8 460 if(dxl_reset[3]!=0)
gidiana 22:ffb26af4d5d8 461 {
gidiana 22:ffb26af4d5d8 462 a_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 463 wait(SLEEP);
gidiana 22:ffb26af4d5d8 464 a_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 465 wait(SLEEP);
gidiana 22:ffb26af4d5d8 466 a_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 467 wait(SLEEP);
gidiana 22:ffb26af4d5d8 468 a_1.setGoal(0-dxl_offset[3]);
gidiana 22:ffb26af4d5d8 469 wait(SLEEP);
gidiana 22:ffb26af4d5d8 470 #if VERBOSE
gidiana 22:ffb26af4d5d8 471 printf("Dynamixel %d : RESET", joint_id[3] );
gidiana 22:ffb26af4d5d8 472 #endif
gidiana 22:ffb26af4d5d8 473 }
gidiana 23:47b8c7f9813e 474 */
gidiana 23:47b8c7f9813e 475
gidiana 22:ffb26af4d5d8 476
gidiana 21:43740448011a 477 }
gidiana 22:ffb26af4d5d8 478 }