Team DIANA / Mbed OS Arm_dynamixel_DEVEL

To be tested

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@23:47b8c7f9813e, 2019-08-07 (annotated)

Committer:
gidiana
Date:
Wed Aug 07 12:54:09 2019 +0000
Revision:
23:47b8c7f9813e
Parent:
22:ffb26af4d5d8
Child:
24:adb6bac314d7
new version;

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