Team DIANA / Mbed OS Arm_dynamixel_can_procedurale

FOrk

Dependencies:   Servo AX12_final MX106_not_working comunication_1

main.cpp@22:ffb26af4d5d8, 2019-07-29 (annotated)

Committer:
gidiana
Date:
Mon Jul 29 22:34:34 2019 +0000
Revision:
22:ffb26af4d5d8
Parent:
21:43740448011a 
refactor complete

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 22:ffb26af4d5d8 7
gidiana 22:ffb26af4d5d8 8
gidiana 22:ffb26af4d5d8 9 #define VERBOSE 1
stebonicelli 18:841ab8c1bdbd 10
gidiana 22:ffb26af4d5d8 11 #define CAN_FREQUENCY 125000
gidiana 22:ffb26af4d5d8 12 #define BAUDRATE 57600
gidiana 22:ffb26af4d5d8 13 #define SLEEP 0.02
gidiana 22:ffb26af4d5d8 14 #define JOINT_SET_POSITION 20
gidiana 22:ffb26af4d5d8 15 #define JOINT_STATUS 30
gidiana 22:ffb26af4d5d8 16 #define JOINT_ERROR 40
gidiana 21:43740448011a 17 #define JOINT1_ID 4
gidiana 21:43740448011a 18 #define JOINT2_ID 5
gidiana 21:43740448011a 19 #define JOINT3_ID 6
gidiana 21:43740448011a 20 #define JOINT4_ID 7
gidiana 21:43740448011a 21
gidiana 22:ffb26af4d5d8 22
gidiana 22:ffb26af4d5d8 23 int dxl_id[]={1,2,3,4};
gidiana 22:ffb26af4d5d8 24 int joint_id[]={4,5,6,7};
gidiana 22:ffb26af4d5d8 25 int dxl_speed[]={50,25,50,50};
gidiana 22:ffb26af4d5d8 26 float dxl_gear[]={3,2,3,1};
gidiana 22:ffb26af4d5d8 27 float dxl_present_position[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 28 float dxl_goal_position[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 29 float dxl_offset[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 30 float dxl_current[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 31 float dxl_torque[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 32 float dxl_max_torque[] = {0,0,0,0};
gidiana 22:ffb26af4d5d8 33 float dxl_reset[]={0,0,0,0};
gidiana 22:ffb26af4d5d8 34 uint16_t dxl_temperature[]={0,0,0,0};
gidiana 22:ffb26af4d5d8 35 uint16_t dxl_limit[]={0,0,0,0};
gidiana 22:ffb26af4d5d8 36 int present, current, torque, temperature;
gidiana 22:ffb26af4d5d8 37 int status;
gidiana 22:ffb26af4d5d8 38
stebonicelli 13:698bd4df9702 39 DigitalOut led(LED1);
stebonicelli 13:698bd4df9702 40
gidiana 22:ffb26af4d5d8 41 communication_1 wire(PA_9, PA_10, BAUDRATE);
stebonicelli 18:841ab8c1bdbd 42
gidiana 22:ffb26af4d5d8 43 MX106 w_1(wire, dxl_id[0], dxl_gear[0]);
gidiana 22:ffb26af4d5d8 44 MX106 w_2(wire, dxl_id[1], dxl_gear[1]);
gidiana 22:ffb26af4d5d8 45 MX106 w_3(wire, dxl_id[2], dxl_gear[2]);
gidiana 22:ffb26af4d5d8 46 AX12 a_1(wire, dxl_id[3], dxl_gear[3]);
stebonicelli 18:841ab8c1bdbd 47
gidiana 21:43740448011a 48 CAN can1(PB_5, PB_6); // RX, TX
stebonicelli 13:698bd4df9702 49
stebonicelli 13:698bd4df9702 50 CANMessage messageIn;
stebonicelli 13:698bd4df9702 51 CANMessage messageOut;
stebonicelli 13:698bd4df9702 52
gidiana 22:ffb26af4d5d8 53
stebonicelli 18:841ab8c1bdbd 54
gidiana 22:ffb26af4d5d8 55 int main ()
stebonicelli 13:698bd4df9702 56 {
gidiana 22:ffb26af4d5d8 57 #if VERBOSE
gidiana 22:ffb26af4d5d8 58 printf("START \n\r");
gidiana 22:ffb26af4d5d8 59 #endif
gidiana 22:ffb26af4d5d8 60 can1.frequency(CAN_FREQUENCY);
gidiana 22:ffb26af4d5d8 61 messageIn.format=CANExtended;
gidiana 22:ffb26af4d5d8 62 messageOut.format=CANExtended;
gidiana 22:ffb26af4d5d8 63
gidiana 22:ffb26af4d5d8 64 wire.trigger();
gidiana 22:ffb26af4d5d8 65 wire.trigger();
gidiana 22:ffb26af4d5d8 66 wire.trigger();
gidiana 22:ffb26af4d5d8 67 wire.trigger();
gidiana 22:ffb26af4d5d8 68
gidiana 22:ffb26af4d5d8 69 #if VERBOSE
gidiana 22:ffb26af4d5d8 70 printf("DYNAMIXEL: Init START \n\r");
gidiana 22:ffb26af4d5d8 71 #endif
gidiana 22:ffb26af4d5d8 72 w_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 73 wait(SLEEP);
gidiana 22:ffb26af4d5d8 74 #if VERBOSE
gidiana 22:ffb26af4d5d8 75 printf("DYNAMIXEL: Init 1 \n\r");
gidiana 22:ffb26af4d5d8 76 #endif
gidiana 22:ffb26af4d5d8 77 w_2.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 78 wait(SLEEP);
gidiana 22:ffb26af4d5d8 79 #if VERBOSE
gidiana 22:ffb26af4d5d8 80 printf("DYNAMIXEL: Init 2 \n\r");
gidiana 22:ffb26af4d5d8 81 #endif
gidiana 22:ffb26af4d5d8 82 w_3.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 83 wait(SLEEP);
gidiana 22:ffb26af4d5d8 84 #if VERBOSE
gidiana 22:ffb26af4d5d8 85 printf("DYNAMIXEL: Init 3\n\r");
gidiana 22:ffb26af4d5d8 86 #endif
gidiana 22:ffb26af4d5d8 87 a_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 88 wait(SLEEP);
gidiana 22:ffb26af4d5d8 89 #if VERBOSE
gidiana 22:ffb26af4d5d8 90 printf("DYNAMIXEL: Init 4\n\r");
gidiana 22:ffb26af4d5d8 91 #endif
stebonicelli 19:7f8c174448d0 92
gidiana 22:ffb26af4d5d8 93 w_1.setMode(2);
gidiana 22:ffb26af4d5d8 94 wait(SLEEP);
gidiana 22:ffb26af4d5d8 95 w_1.setMaxSpeed(dxl_speed[0]);
gidiana 22:ffb26af4d5d8 96 wait(SLEEP);
gidiana 22:ffb26af4d5d8 97 #if VERBOSE
gidiana 22:ffb26af4d5d8 98 printf("DYNAMIXEL 1: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 99 #endif
gidiana 22:ffb26af4d5d8 100 w_2.setMode(2);
gidiana 22:ffb26af4d5d8 101 wait(SLEEP);
gidiana 22:ffb26af4d5d8 102 w_2.setMaxSpeed(dxl_speed[1]);
gidiana 22:ffb26af4d5d8 103 wait(SLEEP);
gidiana 22:ffb26af4d5d8 104 w_2.setCWLimitUnits(1900);
gidiana 22:ffb26af4d5d8 105 wait(SLEEP);
gidiana 22:ffb26af4d5d8 106 w_2.setCCWLimitUnits(-1900);
gidiana 22:ffb26af4d5d8 107 wait(SLEEP);
gidiana 22:ffb26af4d5d8 108 #if VERBOSE
gidiana 22:ffb26af4d5d8 109 printf("DYNAMIXEL 2: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 110 #endif
gidiana 22:ffb26af4d5d8 111 w_3.setMode(2);
gidiana 22:ffb26af4d5d8 112 wait(SLEEP);
gidiana 22:ffb26af4d5d8 113 w_3.setMaxSpeed(dxl_speed[2]);
gidiana 22:ffb26af4d5d8 114 wait(SLEEP);
gidiana 22:ffb26af4d5d8 115 #if VERBOSE
gidiana 22:ffb26af4d5d8 116 printf("DYNAMIXEL 3: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 117 #endif
gidiana 22:ffb26af4d5d8 118 a_1.setMode(1);
gidiana 22:ffb26af4d5d8 119 wait(SLEEP);
gidiana 22:ffb26af4d5d8 120 a_1.setMaxSpeed(dxl_speed[3]);
gidiana 22:ffb26af4d5d8 121 wait(SLEEP);
gidiana 22:ffb26af4d5d8 122 #if VERBOSE
gidiana 22:ffb26af4d5d8 123 printf("DYNAMIXEL 4: SET MODE\n\r");
gidiana 22:ffb26af4d5d8 124 #endif
gidiana 22:ffb26af4d5d8 125
gidiana 22:ffb26af4d5d8 126 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 22:ffb26af4d5d8 127 wait(SLEEP);
gidiana 22:ffb26af4d5d8 128 w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
gidiana 22:ffb26af4d5d8 129 wait(SLEEP);
gidiana 22:ffb26af4d5d8 130 w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
gidiana 22:ffb26af4d5d8 131 wait(SLEEP);
gidiana 22:ffb26af4d5d8 132 a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
gidiana 22:ffb26af4d5d8 133 wait(SLEEP);
gidiana 22:ffb26af4d5d8 134
gidiana 22:ffb26af4d5d8 135 wait(1);
stebonicelli 19:7f8c174448d0 136
gidiana 22:ffb26af4d5d8 137 while (1)
gidiana 22:ffb26af4d5d8 138 {
gidiana 22:ffb26af4d5d8 139 if(can1.read(messageIn))
gidiana 22:ffb26af4d5d8 140 {
gidiana 22:ffb26af4d5d8 141 #if VERBOSE
gidiana 22:ffb26af4d5d8 142 printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
gidiana 22:ffb26af4d5d8 143 #endif
gidiana 22:ffb26af4d5d8 144 led!=led;
gidiana 22:ffb26af4d5d8 145 if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT1_ID))
gidiana 22:ffb26af4d5d8 146 {
gidiana 22:ffb26af4d5d8 147 dxl_goal_position[0] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 148 }
gidiana 22:ffb26af4d5d8 149 if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT2_ID))
gidiana 22:ffb26af4d5d8 150 {
gidiana 22:ffb26af4d5d8 151 dxl_goal_position[1] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 152 }
gidiana 22:ffb26af4d5d8 153 if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT3_ID))
gidiana 22:ffb26af4d5d8 154 {
gidiana 22:ffb26af4d5d8 155 dxl_goal_position[2] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 156 }
gidiana 22:ffb26af4d5d8 157 if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT4_ID))
gidiana 22:ffb26af4d5d8 158 {
gidiana 22:ffb26af4d5d8 159 dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
gidiana 22:ffb26af4d5d8 160 }
clynamen 11:19e8022f60ea 161
gidiana 22:ffb26af4d5d8 162 if(messageIn.id == ((JOINT_ERROR << 8) + JOINT1_ID))
gidiana 22:ffb26af4d5d8 163 {
gidiana 22:ffb26af4d5d8 164 dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 165 }
gidiana 22:ffb26af4d5d8 166 if(messageIn.id == ((JOINT_ERROR << 8) + JOINT2_ID))
gidiana 22:ffb26af4d5d8 167 {
gidiana 22:ffb26af4d5d8 168 dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 169 }
gidiana 22:ffb26af4d5d8 170 if(messageIn.id == ((JOINT_ERROR << 8) + JOINT3_ID))
gidiana 22:ffb26af4d5d8 171 {
gidiana 22:ffb26af4d5d8 172 dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 173 }
gidiana 22:ffb26af4d5d8 174 if(messageIn.id == ((JOINT_ERROR << 8) + JOINT4_ID))
gidiana 22:ffb26af4d5d8 175 {
gidiana 22:ffb26af4d5d8 176 dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
gidiana 22:ffb26af4d5d8 177 }
gidiana 22:ffb26af4d5d8 178 }
stebonicelli 19:7f8c174448d0 179
gidiana 22:ffb26af4d5d8 180 w_1.setGoal((dxl_goal_position[0]-dxl_offset[0]));
gidiana 22:ffb26af4d5d8 181 wait(SLEEP);
gidiana 22:ffb26af4d5d8 182 w_2.setGoal((dxl_goal_position[1]-dxl_offset[1]));
gidiana 22:ffb26af4d5d8 183 wait(SLEEP);
gidiana 22:ffb26af4d5d8 184 w_3.setGoal((dxl_goal_position[2]-dxl_offset[2]));
gidiana 22:ffb26af4d5d8 185 wait(SLEEP);
gidiana 22:ffb26af4d5d8 186 a_1.setGoal((dxl_goal_position[3]-dxl_offset[3]));
gidiana 22:ffb26af4d5d8 187 wait(SLEEP);
stebonicelli 19:7f8c174448d0 188
gidiana 22:ffb26af4d5d8 189 dxl_present_position[0]=w_1.getPosition();
gidiana 22:ffb26af4d5d8 190 wait(SLEEP);
gidiana 22:ffb26af4d5d8 191 dxl_present_position[1]=w_2.getPosition();
gidiana 22:ffb26af4d5d8 192 wait(SLEEP);
gidiana 22:ffb26af4d5d8 193 dxl_present_position[2]=w_3.getPosition();
gidiana 22:ffb26af4d5d8 194 wait(SLEEP);
gidiana 22:ffb26af4d5d8 195 dxl_present_position[3]=a_1.getPosition();
gidiana 22:ffb26af4d5d8 196 wait(SLEEP);
stebonicelli 19:7f8c174448d0 197
gidiana 22:ffb26af4d5d8 198 #if VERBOSE
gidiana 22:ffb26af4d5d8 199 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 200 #endif
gidiana 22:ffb26af4d5d8 201
gidiana 22:ffb26af4d5d8 202 dxl_current[0]=w_1.getCurrent();
gidiana 22:ffb26af4d5d8 203 wait(SLEEP);
gidiana 22:ffb26af4d5d8 204 dxl_current[1]=w_2.getCurrent();
gidiana 22:ffb26af4d5d8 205 wait(SLEEP);
gidiana 22:ffb26af4d5d8 206 dxl_current[2]=w_3.getCurrent();
gidiana 22:ffb26af4d5d8 207 wait(SLEEP);
gidiana 22:ffb26af4d5d8 208 //dxl_current[3]=a_1.getCurrent();
gidiana 22:ffb26af4d5d8 209 //wait(SLEEP);
gidiana 22:ffb26af4d5d8 210 #if VERBOSE
gidiana 22:ffb26af4d5d8 211 printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] );
gidiana 22:ffb26af4d5d8 212 #endif
gidiana 22:ffb26af4d5d8 213
gidiana 22:ffb26af4d5d8 214 dxl_torque[0]=dxl_current[0]*0.875;
gidiana 22:ffb26af4d5d8 215 dxl_torque[1]=dxl_current[1]*0.875;
gidiana 22:ffb26af4d5d8 216 dxl_torque[2]=dxl_current[2]*0.875;
gidiana 22:ffb26af4d5d8 217 //dxl_torque[3]=dxl_current[3]*0.875;
gidiana 22:ffb26af4d5d8 218 #if VERBOSE
gidiana 22:ffb26af4d5d8 219 printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] );
gidiana 22:ffb26af4d5d8 220 #endif
gidiana 22:ffb26af4d5d8 221
gidiana 22:ffb26af4d5d8 222 dxl_temperature[0]=w_1.getTemp();
gidiana 22:ffb26af4d5d8 223 wait(SLEEP);
gidiana 22:ffb26af4d5d8 224 dxl_temperature[1]=w_2.getTemp();
gidiana 22:ffb26af4d5d8 225 wait(SLEEP);
gidiana 22:ffb26af4d5d8 226 dxl_temperature[2]=w_3.getTemp();
gidiana 22:ffb26af4d5d8 227 wait(SLEEP);
gidiana 22:ffb26af4d5d8 228 dxl_temperature[3]=a_1.getTemp();
gidiana 22:ffb26af4d5d8 229 wait(SLEEP);
gidiana 22:ffb26af4d5d8 230
gidiana 22:ffb26af4d5d8 231 #if VERBOSE
gidiana 22:ffb26af4d5d8 232 printf("DYNAMIXEL TEMPERATURE %d %d %d %d\n\r", dxl_temperature[0],dxl_temperature[1], dxl_temperature[2] ,dxl_temperature[3] );
gidiana 22:ffb26af4d5d8 233 #endif
gidiana 22:ffb26af4d5d8 234
gidiana 22:ffb26af4d5d8 235 for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++)
gidiana 22:ffb26af4d5d8 236 {
gidiana 22:ffb26af4d5d8 237 messageOut.len = 8;
gidiana 22:ffb26af4d5d8 238 present=(int)(dxl_present_position[i]*100);
gidiana 22:ffb26af4d5d8 239 current=(int)(dxl_current[i]*100);
gidiana 22:ffb26af4d5d8 240 torque=(int)(dxl_torque[i]*100);
gidiana 22:ffb26af4d5d8 241 temperature=(int)(dxl_temperature[i]*100);
gidiana 22:ffb26af4d5d8 242
gidiana 22:ffb26af4d5d8 243 messageOut.id = (JOINT_STATUS << 8) + joint_id[i];
gidiana 22:ffb26af4d5d8 244
gidiana 22:ffb26af4d5d8 245 messageOut.data[0] = (present >> 8);
gidiana 22:ffb26af4d5d8 246 messageOut.data[1] = (present);
gidiana 22:ffb26af4d5d8 247 messageOut.data[2] = (current >> 8);
gidiana 22:ffb26af4d5d8 248 messageOut.data[3] = (current);
gidiana 22:ffb26af4d5d8 249 messageOut.data[4] = (torque >> 8);
gidiana 22:ffb26af4d5d8 250 messageOut.data[5] = (torque);
gidiana 22:ffb26af4d5d8 251 messageOut.data[6] = (temperature >> 8);
gidiana 22:ffb26af4d5d8 252 messageOut.data[7] = (temperature);
gidiana 22:ffb26af4d5d8 253
gidiana 22:ffb26af4d5d8 254 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 255
gidiana 22:ffb26af4d5d8 256 #if VERBOSE
gidiana 22:ffb26af4d5d8 257 printf("CAN send status Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 258 #endif
gidiana 22:ffb26af4d5d8 259 }
gidiana 22:ffb26af4d5d8 260
gidiana 22:ffb26af4d5d8 261
gidiana 22:ffb26af4d5d8 262
gidiana 22:ffb26af4d5d8 263 dxl_max_torque[0]=w_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 264 wait(SLEEP);
gidiana 22:ffb26af4d5d8 265 dxl_max_torque[1]=w_2.getTorqueMax();
gidiana 22:ffb26af4d5d8 266 wait(SLEEP);
gidiana 22:ffb26af4d5d8 267 dxl_max_torque[2]=w_3.getTorqueMax();
gidiana 22:ffb26af4d5d8 268 wait(SLEEP);
gidiana 22:ffb26af4d5d8 269 dxl_max_torque[3]=a_1.getTorqueMax();
gidiana 22:ffb26af4d5d8 270 wait(SLEEP);
gidiana 22:ffb26af4d5d8 271
gidiana 22:ffb26af4d5d8 272 for (int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++ )
gidiana 22:ffb26af4d5d8 273 {
gidiana 22:ffb26af4d5d8 274
gidiana 22:ffb26af4d5d8 275 messageOut.len = 4;
gidiana 22:ffb26af4d5d8 276 messageOut.id= ( JOINT_ERROR << 8 ) + joint_id[i];
gidiana 22:ffb26af4d5d8 277
gidiana 22:ffb26af4d5d8 278 messageOut.data[0]=(((int)(dxl_max_torque[i]))>>8);
gidiana 22:ffb26af4d5d8 279 messageOut.data[1]= ((int)(dxl_max_torque[i]));
gidiana 22:ffb26af4d5d8 280 status = can1.write(messageOut);
gidiana 22:ffb26af4d5d8 281
gidiana 22:ffb26af4d5d8 282 #if VERBOSE
gidiana 22:ffb26af4d5d8 283 printf("CAN send status ERROR Joint %d : %d", joint_id[i] , status );
gidiana 22:ffb26af4d5d8 284 #endif
gidiana 22:ffb26af4d5d8 285
gidiana 22:ffb26af4d5d8 286 }
gidiana 22:ffb26af4d5d8 287 if(dxl_reset[0]!=0)
gidiana 22:ffb26af4d5d8 288 {
gidiana 22:ffb26af4d5d8 289 w_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 290 wait(SLEEP);
gidiana 22:ffb26af4d5d8 291 w_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 292 wait(SLEEP);
gidiana 22:ffb26af4d5d8 293 w_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 294 wait(SLEEP);
gidiana 22:ffb26af4d5d8 295 w_1.setGoal(0-dxl_offset[0]);
gidiana 22:ffb26af4d5d8 296 wait(SLEEP);
gidiana 22:ffb26af4d5d8 297 #if VERBOSE
gidiana 22:ffb26af4d5d8 298 printf("Dynamixel %d : RESET", joint_id[0] );
gidiana 22:ffb26af4d5d8 299 #endif
gidiana 22:ffb26af4d5d8 300 }
gidiana 22:ffb26af4d5d8 301 if(dxl_reset[1]!=0)
gidiana 22:ffb26af4d5d8 302 {
gidiana 22:ffb26af4d5d8 303 w_2.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 304 wait(SLEEP);
gidiana 22:ffb26af4d5d8 305 w_2.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 306 wait(SLEEP);
gidiana 22:ffb26af4d5d8 307 w_2.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 308 wait(SLEEP);
gidiana 22:ffb26af4d5d8 309 w_2.setGoal(0-dxl_offset[1]);
gidiana 22:ffb26af4d5d8 310 wait(SLEEP);
gidiana 22:ffb26af4d5d8 311 #if VERBOSE
gidiana 22:ffb26af4d5d8 312 printf("Dynamixel %d : RESET", joint_id[1] );
gidiana 22:ffb26af4d5d8 313 #endif
gidiana 22:ffb26af4d5d8 314 }
gidiana 22:ffb26af4d5d8 315 if(dxl_reset[2]!=0)
gidiana 22:ffb26af4d5d8 316 {
gidiana 22:ffb26af4d5d8 317 w_3.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 318 wait(SLEEP);
gidiana 22:ffb26af4d5d8 319 w_3.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 320 wait(SLEEP);
gidiana 22:ffb26af4d5d8 321 w_3.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 322 wait(SLEEP);
gidiana 22:ffb26af4d5d8 323 w_3.setGoal(0-dxl_offset[2]);
gidiana 22:ffb26af4d5d8 324 wait(SLEEP);
gidiana 22:ffb26af4d5d8 325 #if VERBOSE
gidiana 22:ffb26af4d5d8 326 printf("Dynamixel %d : RESET", joint_id[2] );
gidiana 22:ffb26af4d5d8 327 #endif
gidiana 22:ffb26af4d5d8 328 }
gidiana 22:ffb26af4d5d8 329 if(dxl_reset[3]!=0)
gidiana 22:ffb26af4d5d8 330 {
gidiana 22:ffb26af4d5d8 331 a_1.setMotorEnabled(0);
gidiana 22:ffb26af4d5d8 332 wait(SLEEP);
gidiana 22:ffb26af4d5d8 333 a_1.setMaxTorque(1);
gidiana 22:ffb26af4d5d8 334 wait(SLEEP);
gidiana 22:ffb26af4d5d8 335 a_1.setMotorEnabled(1);
gidiana 22:ffb26af4d5d8 336 wait(SLEEP);
gidiana 22:ffb26af4d5d8 337 a_1.setGoal(0-dxl_offset[3]);
gidiana 22:ffb26af4d5d8 338 wait(SLEEP);
gidiana 22:ffb26af4d5d8 339 #if VERBOSE
gidiana 22:ffb26af4d5d8 340 printf("Dynamixel %d : RESET", joint_id[3] );
gidiana 22:ffb26af4d5d8 341 #endif
gidiana 22:ffb26af4d5d8 342 }
gidiana 22:ffb26af4d5d8 343
gidiana 22:ffb26af4d5d8 344
gidiana 21:43740448011a 345 }
gidiana 22:ffb26af4d5d8 346 return 0;
gidiana 22:ffb26af4d5d8 347 }