Team DIANA / Mbed OS Arm_dynamixel_DEVEL

To be tested

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@24:adb6bac314d7, 2019-09-03 (annotated)

Committer:
gidiana
Date:
Tue Sep 03 16:29:39 2019 +0000
Revision:
24:adb6bac314d7
Parent:
23:47b8c7f9813e
Child:
25:1aa10432cda2
updates

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 22:ffb26af4d5d8 46 int dxl_speed[]={50,25,50,50};
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 22:ffb26af4d5d8 258 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 22:ffb26af4d5d8 259 wait(SLEEP);
gidiana 22:ffb26af4d5d8 260 w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
gidiana 22:ffb26af4d5d8 261 wait(SLEEP);
gidiana 22:ffb26af4d5d8 262 w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
gidiana 22:ffb26af4d5d8 263 wait(SLEEP);
gidiana 22:ffb26af4d5d8 264 a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
gidiana 22:ffb26af4d5d8 265 wait(SLEEP);
gidiana 23:47b8c7f9813e 266 camera=camera_pose;
stebonicelli 19:7f8c174448d0 267
gidiana 22:ffb26af4d5d8 268 dxl_present_position[0]=w_1.getPosition();
gidiana 22:ffb26af4d5d8 269 wait(SLEEP);
gidiana 22:ffb26af4d5d8 270 dxl_present_position[1]=w_2.getPosition();
gidiana 22:ffb26af4d5d8 271 wait(SLEEP);
gidiana 22:ffb26af4d5d8 272 dxl_present_position[2]=w_3.getPosition();
gidiana 22:ffb26af4d5d8 273 wait(SLEEP);
gidiana 22:ffb26af4d5d8 274 dxl_present_position[3]=a_1.getPosition();
gidiana 22:ffb26af4d5d8 275 wait(SLEEP);
stebonicelli 19:7f8c174448d0 276
gidiana 22:ffb26af4d5d8 277 #if VERBOSE
gidiana 22:ffb26af4d5d8 278 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 279 #endif
gidiana 22:ffb26af4d5d8 280
gidiana 22:ffb26af4d5d8 281 dxl_current[0]=w_1.getCurrent();
gidiana 22:ffb26af4d5d8 282 wait(SLEEP);
gidiana 22:ffb26af4d5d8 283 dxl_current[1]=w_2.getCurrent();
gidiana 22:ffb26af4d5d8 284 wait(SLEEP);
gidiana 22:ffb26af4d5d8 285 dxl_current[2]=w_3.getCurrent();
gidiana 22:ffb26af4d5d8 286 wait(SLEEP);
gidiana 22:ffb26af4d5d8 287 //dxl_current[3]=a_1.getCurrent();
gidiana 22:ffb26af4d5d8 288 //wait(SLEEP);
gidiana 22:ffb26af4d5d8 289 #if VERBOSE
gidiana 22:ffb26af4d5d8 290 printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] );
gidiana 22:ffb26af4d5d8 291 #endif
gidiana 22:ffb26af4d5d8 292
gidiana 22:ffb26af4d5d8 293 dxl_torque[0]=dxl_current[0]*0.875;
gidiana 22:ffb26af4d5d8 294 dxl_torque[1]=dxl_current[1]*0.875;
gidiana 22:ffb26af4d5d8 295 dxl_torque[2]=dxl_current[2]*0.875;
gidiana 22:ffb26af4d5d8 296 //dxl_torque[3]=dxl_current[3]*0.875;
gidiana 22:ffb26af4d5d8 297 #if VERBOSE
gidiana 22:ffb26af4d5d8 298 printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] );
gidiana 22:ffb26af4d5d8 299 #endif
gidiana 22:ffb26af4d5d8 300
gidiana 22:ffb26af4d5d8 301 dxl_temperature[0]=w_1.getTemp();
gidiana 22:ffb26af4d5d8 302 wait(SLEEP);
gidiana 22:ffb26af4d5d8 303 dxl_temperature[1]=w_2.getTemp();
gidiana 22:ffb26af4d5d8 304 wait(SLEEP);
gidiana 22:ffb26af4d5d8 305 dxl_temperature[2]=w_3.getTemp();
gidiana 22:ffb26af4d5d8 306 wait(SLEEP);
gidiana 22:ffb26af4d5d8 307 dxl_temperature[3]=a_1.getTemp();
gidiana 22:ffb26af4d5d8 308 wait(SLEEP);
gidiana 22:ffb26af4d5d8 309
gidiana 22:ffb26af4d5d8 310 #if VERBOSE
gidiana 22:ffb26af4d5d8 311 printf("DYNAMIXEL TEMPERATURE %d %d %d %d\n\r", dxl_temperature[0],dxl_temperature[1], dxl_temperature[2] ,dxl_temperature[3] );
gidiana 22:ffb26af4d5d8 312 #endif
gidiana 22:ffb26af4d5d8 313
gidiana 22:ffb26af4d5d8 314 for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++)
gidiana 22:ffb26af4d5d8 315 {
gidiana 23:47b8c7f9813e 316 messageOut.len = 4;
gidiana 22:ffb26af4d5d8 317 present=(int)(dxl_present_position[i]*100);
gidiana 22:ffb26af4d5d8 318 current=(int)(dxl_current[i]*100);
gidiana 22:ffb26af4d5d8 319 torque=(int)(dxl_torque[i]*100);
gidiana 22:ffb26af4d5d8 320 temperature=(int)(dxl_temperature[i]*100);
gidiana 22:ffb26af4d5d8 321
gidiana 23:47b8c7f9813e 322 messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, joint_id[i]);
gidiana 23:47b8c7f9813e 323
gidiana 23:47b8c7f9813e 324 messageOut.data[0] = (present >> 24);
gidiana 23:47b8c7f9813e 325 messageOut.data[1] = (present>>16);
gidiana 23:47b8c7f9813e 326 messageOut.data[2] = (present>>8);
gidiana 23:47b8c7f9813e 327 messageOut.data[3] = (present);
gidiana 23:47b8c7f9813e 328 status = can1.write(messageOut);
gidiana 23:47b8c7f9813e 329 #if VERBOSE
gidiana 23:47b8c7f9813e 330 printf("CAN send present position Joint %d : %d", joint_id[i] , status );
gidiana 23:47b8c7f9813e 331 #endif
gidiana 23:47b8c7f9813e 332
gidiana 23:47b8c7f9813e 333 messageOut.id = gen_can_id(JOINT_STATUS, joint_id[i]);
gidiana 22:ffb26af4d5d8 334
gidiana 23:47b8c7f9813e 335 messageOut.data[0] = (current >> 8);
gidiana 23:47b8c7f9813e 336 messageOut.data[1] = (current);
gidiana 23:47b8c7f9813e 337 messageOut.data[2] = (temperature >> 8);
gidiana 23:47b8c7f9813e 338 messageOut.data[3] = (temperature);
gidiana 23:47b8c7f9813e 339 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 340
gidiana 23:47b8c7f9813e 341 #if VERBOSE
gidiana 23:47b8c7f9813e 342 printf("CAN send status Joint %d : %d", joint_id[i] , status );
gidiana 23:47b8c7f9813e 343 #endif
gidiana 23:47b8c7f9813e 344 messageOut.id = gen_can_id(JOINT_TORQUE, joint_id[i]);
gidiana 23:47b8c7f9813e 345
gidiana 23:47b8c7f9813e 346 messageOut.data[0] = (torque >> 24);
gidiana 23:47b8c7f9813e 347 messageOut.data[1] = (torque >> 16);
gidiana 23:47b8c7f9813e 348 messageOut.data[2] = (torque >> 8);
gidiana 23:47b8c7f9813e 349 messageOut.data[3] = (torque);
gidiana 22:ffb26af4d5d8 350 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 351
gidiana 22:ffb26af4d5d8 352 #if VERBOSE
gidiana 22:ffb26af4d5d8 353 printf("CAN send status Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 354 #endif
gidiana 22:ffb26af4d5d8 355 }
gidiana 23:47b8c7f9813e 356 int _camera_pose = camera_pose*100;
gidiana 23:47b8c7f9813e 357 messageOut.len = 4;
gidiana 23:47b8c7f9813e 358 messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, CAMERA2);
gidiana 23:47b8c7f9813e 359 messageOut.data[0] = (_camera_pose >> 24);
gidiana 23:47b8c7f9813e 360 messageOut.data[1] = (_camera_pose >> 16);
gidiana 23:47b8c7f9813e 361 messageOut.data[2] = (_camera_pose >> 8);
gidiana 23:47b8c7f9813e 362 messageOut.data[3] = (_camera_pose);
gidiana 23:47b8c7f9813e 363 status = can1.write(messageOut);
gidiana 23:47b8c7f9813e 364
gidiana 23:47b8c7f9813e 365 #if VERBOSE
gidiana 23:47b8c7f9813e 366
gidiana 23:47b8c7f9813e 367 printf("CAN send present position Joint %d : %d", CAMERA2 , status );
gidiana 23:47b8c7f9813e 368
gidiana 23:47b8c7f9813e 369 #endif
gidiana 22:ffb26af4d5d8 370
gidiana 22:ffb26af4d5d8 371
gidiana 22:ffb26af4d5d8 372
gidiana 22:ffb26af4d5d8 373 dxl_max_torque[0]=w_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 374 wait(SLEEP);
gidiana 22:ffb26af4d5d8 375 dxl_max_torque[1]=w_2.getTorqueMax();
gidiana 22:ffb26af4d5d8 376 wait(SLEEP);
gidiana 22:ffb26af4d5d8 377 dxl_max_torque[2]=w_3.getTorqueMax();
gidiana 22:ffb26af4d5d8 378 wait(SLEEP);
gidiana 22:ffb26af4d5d8 379 dxl_max_torque[3]=a_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 380 wait(SLEEP);
gidiana 22:ffb26af4d5d8 381
gidiana 22:ffb26af4d5d8 382 for (int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++ )
gidiana 22:ffb26af4d5d8 383 {
gidiana 22:ffb26af4d5d8 384
gidiana 22:ffb26af4d5d8 385 messageOut.len = 4;
gidiana 23:47b8c7f9813e 386 messageOut.id= gen_can_id(JOINT_ERROR, joint_id[i]);
gidiana 22:ffb26af4d5d8 387
gidiana 23:47b8c7f9813e 388 messageOut.data[0]= ((int)(dxl_max_torque[i]) >> 24);
gidiana 23:47b8c7f9813e 389 messageOut.data[1]= ((int)(dxl_max_torque[i]) >> 16);
gidiana 23:47b8c7f9813e 390 messageOut.data[2]= ((int)(dxl_max_torque[i]) >> 8);
gidiana 23:47b8c7f9813e 391 messageOut.data[3]= ((int)(dxl_max_torque[i]) );
gidiana 22:ffb26af4d5d8 392 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 393
gidiana 22:ffb26af4d5d8 394 #if VERBOSE
gidiana 22:ffb26af4d5d8 395 printf("CAN send status ERROR Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 396 #endif
gidiana 22:ffb26af4d5d8 397
gidiana 22:ffb26af4d5d8 398 }
gidiana 22:ffb26af4d5d8 399 if(dxl_reset[0]!=0)
gidiana 22:ffb26af4d5d8 400 {
gidiana 22:ffb26af4d5d8 401 w_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 402 wait(SLEEP);
gidiana 22:ffb26af4d5d8 403 w_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 404 wait(SLEEP);
gidiana 22:ffb26af4d5d8 405 w_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 406 wait(SLEEP);
gidiana 22:ffb26af4d5d8 407 w_1.setGoal(0-dxl_offset[0]);
gidiana 22:ffb26af4d5d8 408 wait(SLEEP);
gidiana 22:ffb26af4d5d8 409 #if VERBOSE
gidiana 22:ffb26af4d5d8 410 printf("Dynamixel %d : RESET", joint_id[0] );
gidiana 22:ffb26af4d5d8 411 #endif
gidiana 22:ffb26af4d5d8 412 }
gidiana 22:ffb26af4d5d8 413 if(dxl_reset[1]!=0)
gidiana 22:ffb26af4d5d8 414 {
gidiana 22:ffb26af4d5d8 415 w_2.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 416 wait(SLEEP);
gidiana 22:ffb26af4d5d8 417 w_2.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 418 wait(SLEEP);
gidiana 22:ffb26af4d5d8 419 w_2.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 420 wait(SLEEP);
gidiana 22:ffb26af4d5d8 421 w_2.setGoal(0-dxl_offset[1]);
gidiana 22:ffb26af4d5d8 422 wait(SLEEP);
gidiana 22:ffb26af4d5d8 423 #if VERBOSE
gidiana 22:ffb26af4d5d8 424 printf("Dynamixel %d : RESET", joint_id[1] );
gidiana 22:ffb26af4d5d8 425 #endif
gidiana 22:ffb26af4d5d8 426 }
gidiana 22:ffb26af4d5d8 427 if(dxl_reset[2]!=0)
gidiana 22:ffb26af4d5d8 428 {
gidiana 22:ffb26af4d5d8 429 w_3.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 430 wait(SLEEP);
gidiana 22:ffb26af4d5d8 431 w_3.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 432 wait(SLEEP);
gidiana 22:ffb26af4d5d8 433 w_3.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 434 wait(SLEEP);
gidiana 22:ffb26af4d5d8 435 w_3.setGoal(0-dxl_offset[2]);
gidiana 22:ffb26af4d5d8 436 wait(SLEEP);
gidiana 22:ffb26af4d5d8 437 #if VERBOSE
gidiana 22:ffb26af4d5d8 438 printf("Dynamixel %d : RESET", joint_id[2] );
gidiana 22:ffb26af4d5d8 439 #endif
gidiana 22:ffb26af4d5d8 440 }
gidiana 23:47b8c7f9813e 441 /*
gidiana 22:ffb26af4d5d8 442 if(dxl_reset[3]!=0)
gidiana 22:ffb26af4d5d8 443 {
gidiana 22:ffb26af4d5d8 444 a_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 445 wait(SLEEP);
gidiana 22:ffb26af4d5d8 446 a_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 447 wait(SLEEP);
gidiana 22:ffb26af4d5d8 448 a_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 449 wait(SLEEP);
gidiana 22:ffb26af4d5d8 450 a_1.setGoal(0-dxl_offset[3]);
gidiana 22:ffb26af4d5d8 451 wait(SLEEP);
gidiana 22:ffb26af4d5d8 452 #if VERBOSE
gidiana 22:ffb26af4d5d8 453 printf("Dynamixel %d : RESET", joint_id[3] );
gidiana 22:ffb26af4d5d8 454 #endif
gidiana 22:ffb26af4d5d8 455 }
gidiana 23:47b8c7f9813e 456 */
gidiana 23:47b8c7f9813e 457
gidiana 22:ffb26af4d5d8 458
gidiana 21:43740448011a 459 }
gidiana 22:ffb26af4d5d8 460 }