James Hilder / PR-RobotArmController

For YRL Robot Arm

servo.cpp@0:b14dfd8816da, 2017-03-03 (annotated)

Committer:
jah128
Date:
Fri Mar 03 13:28:54 2017 +0000
Revision:
0:b14dfd8816da
Updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:b14dfd8816da 1 /* University of York Robotics Laboratory Robot Arm Controller Board
jah128 0:b14dfd8816da 2 *
jah128 0:b14dfd8816da 3 * Dynamixel Servo Library for AX-12 and MX-28
jah128 0:b14dfd8816da 4 *
jah128 0:b14dfd8816da 5 * Based on library by Chris Styles (see copyright notice at end of file)
jah128 0:b14dfd8816da 6 *
jah128 0:b14dfd8816da 7 * File: servo.cpp
jah128 0:b14dfd8816da 8 *
jah128 0:b14dfd8816da 9 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:b14dfd8816da 10 * James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis
jah128 0:b14dfd8816da 11 *
jah128 0:b14dfd8816da 12 * February 2017, Version 1.0
jah128 0:b14dfd8816da 13 */
jah128 0:b14dfd8816da 14
jah128 0:b14dfd8816da 15 #include "robotarm.h"
jah128 0:b14dfd8816da 16
jah128 0:b14dfd8816da 17 int delay = RETURN_DELAY;
jah128 0:b14dfd8816da 18 char read_timeout_counter = 0;
jah128 0:b14dfd8816da 19 Servo::Servo(PinName tx, PinName rx)
jah128 0:b14dfd8816da 20 : _servo(tx,rx)
jah128 0:b14dfd8816da 21 {
jah128 0:b14dfd8816da 22 _servo.baud(57600);
jah128 0:b14dfd8816da 23 }
jah128 0:b14dfd8816da 24
jah128 0:b14dfd8816da 25 void Servo::ClearBuffer()
jah128 0:b14dfd8816da 26 {
jah128 0:b14dfd8816da 27 if (_servo.readable()) {
jah128 0:b14dfd8816da 28 pc.printf("\nBuffer error:");
jah128 0:b14dfd8816da 29 while(_servo.readable()) {
jah128 0:b14dfd8816da 30 pc.printf("%c",_servo.getc());
jah128 0:b14dfd8816da 31 }
jah128 0:b14dfd8816da 32 pc.printf("\n");
jah128 0:b14dfd8816da 33 }
jah128 0:b14dfd8816da 34 }
jah128 0:b14dfd8816da 35 void Servo::ScanForServos ()
jah128 0:b14dfd8816da 36 {
jah128 0:b14dfd8816da 37 pc.printf("SCANNING FOR ServoS...\n");
jah128 0:b14dfd8816da 38 pc.printf("Checking at 57600 baud\n");
jah128 0:b14dfd8816da 39 _servo.baud(57600);
jah128 0:b14dfd8816da 40 delay = 250;
jah128 0:b14dfd8816da 41 for(int k=0; k<2; k++) {
jah128 0:b14dfd8816da 42 if(k==1) {
jah128 0:b14dfd8816da 43 _servo.baud(1000000);
jah128 0:b14dfd8816da 44 pc.printf("\nChecking at 1000000 baud\n");
jah128 0:b14dfd8816da 45 }
jah128 0:b14dfd8816da 46 for(int id = 0; id<254; id++) {
jah128 0:b14dfd8816da 47 //pc.printf("ID %d: ",id);
jah128 0:b14dfd8816da 48 char TxBuf[8];
jah128 0:b14dfd8816da 49 TxBuf[0] = 0xff;
jah128 0:b14dfd8816da 50 TxBuf[1] = 0xff;
jah128 0:b14dfd8816da 51 TxBuf[2] = id;
jah128 0:b14dfd8816da 52 char sum = id + 7;
jah128 0:b14dfd8816da 53 TxBuf[3] = 4;
jah128 0:b14dfd8816da 54 TxBuf[4] = 2;
jah128 0:b14dfd8816da 55 TxBuf[5] = REG_MODEL_NUMBER;
jah128 0:b14dfd8816da 56 TxBuf[6] = 1;
jah128 0:b14dfd8816da 57 TxBuf[7] = 0xFF - sum;
jah128 0:b14dfd8816da 58 for (int i = 0; i<8 ; i++) {
jah128 0:b14dfd8816da 59 _servo.putc(TxBuf[i]);
jah128 0:b14dfd8816da 60 }
jah128 0:b14dfd8816da 61 // Wait for data to transmit
jah128 0:b14dfd8816da 62 int t_delay = 60;
jah128 0:b14dfd8816da 63 wait_us(t_delay);
jah128 0:b14dfd8816da 64 if(_servo.readable()) {
jah128 0:b14dfd8816da 65 pc.printf("ID %d: ",id);
jah128 0:b14dfd8816da 66 // Receive the Status packet 6+ number of bytes read
jah128 0:b14dfd8816da 67 char status[8];
jah128 0:b14dfd8816da 68 for (int i=0; i<(7) ; i++) {
jah128 0:b14dfd8816da 69 status[i] = _servo.getc();
jah128 0:b14dfd8816da 70 }
jah128 0:b14dfd8816da 71 if(status[2] == id) {
jah128 0:b14dfd8816da 72 pc.printf(" FOUND [");
jah128 0:b14dfd8816da 73 char modelnumber = status[5];
jah128 0:b14dfd8816da 74 switch(modelnumber) {
jah128 0:b14dfd8816da 75 case (AX12_MODEL):
jah128 0:b14dfd8816da 76 pc.printf("AX12]\n");
jah128 0:b14dfd8816da 77 break;
jah128 0:b14dfd8816da 78 case (MX28_MODEL):
jah128 0:b14dfd8816da 79 pc.printf("MX28]\n");
jah128 0:b14dfd8816da 80 break;
jah128 0:b14dfd8816da 81 default:
jah128 0:b14dfd8816da 82 pc.printf("UNKNOWN MODEL]\n");
jah128 0:b14dfd8816da 83 break;
jah128 0:b14dfd8816da 84 }
jah128 0:b14dfd8816da 85 } else pc.printf(" ID ERROR\n");
jah128 0:b14dfd8816da 86 } else {
jah128 0:b14dfd8816da 87 //pc.printf(" NOT FOUND\n");
jah128 0:b14dfd8816da 88 }
jah128 0:b14dfd8816da 89 }
jah128 0:b14dfd8816da 90 }
jah128 0:b14dfd8816da 91 pc.printf("\nScan complete.\n");
jah128 0:b14dfd8816da 92 delay = RETURN_DELAY;
jah128 0:b14dfd8816da 93 }
jah128 0:b14dfd8816da 94
jah128 0:b14dfd8816da 95 // Get the soft lower limit for servo
jah128 0:b14dfd8816da 96 short Servo::GetLowerLimit(int ID)
jah128 0:b14dfd8816da 97 {
jah128 0:b14dfd8816da 98 if(USE_SOFT_LIMITS==1){
jah128 0:b14dfd8816da 99 switch(ID){
jah128 0:b14dfd8816da 100 case BASE: return BASE_LIMIT_LOW;
jah128 0:b14dfd8816da 101 case SHOULDER: return SHOULDER_LIMIT_LOW;
jah128 0:b14dfd8816da 102 case ELBOW: return ELBOW_LIMIT_LOW;
jah128 0:b14dfd8816da 103 //case WRIST: return WRIST_LIMIT_LOW;
jah128 0:b14dfd8816da 104 }
jah128 0:b14dfd8816da 105 }
jah128 0:b14dfd8816da 106 return 0;
jah128 0:b14dfd8816da 107 }
jah128 0:b14dfd8816da 108
jah128 0:b14dfd8816da 109 // Get the soft upper limit for servo
jah128 0:b14dfd8816da 110 short Servo::GetUpperLimit(int ID)
jah128 0:b14dfd8816da 111 {
jah128 0:b14dfd8816da 112 if(USE_SOFT_LIMITS==1){
jah128 0:b14dfd8816da 113 switch(ID){
jah128 0:b14dfd8816da 114 case BASE: return BASE_LIMIT_HIGH;
jah128 0:b14dfd8816da 115 case SHOULDER: return SHOULDER_LIMIT_HIGH;
jah128 0:b14dfd8816da 116 case ELBOW: return ELBOW_LIMIT_HIGH;
jah128 0:b14dfd8816da 117 // case WRIST: return WRIST_LIMIT_HIGH;
jah128 0:b14dfd8816da 118 }
jah128 0:b14dfd8816da 119 }
jah128 0:b14dfd8816da 120 return 4095;
jah128 0:b14dfd8816da 121 }
jah128 0:b14dfd8816da 122
jah128 0:b14dfd8816da 123 // Get detailed data for servo
jah128 0:b14dfd8816da 124 void Servo::DebugData(int ID)
jah128 0:b14dfd8816da 125 {
jah128 0:b14dfd8816da 126 pc.printf("\nGetting Current Data for Servo %d",ID);
jah128 0:b14dfd8816da 127
jah128 0:b14dfd8816da 128
jah128 0:b14dfd8816da 129 char data[49];
jah128 0:b14dfd8816da 130 for(int i=0; i<12; i++) {
jah128 0:b14dfd8816da 131 int offset = i*4;
jah128 0:b14dfd8816da 132 int ErrorCode = read(ID, offset, 4, data+offset);
jah128 0:b14dfd8816da 133 pc.printf(".");
jah128 0:b14dfd8816da 134 }
jah128 0:b14dfd8816da 135 pc.printf("\n");
jah128 0:b14dfd8816da 136
jah128 0:b14dfd8816da 137
jah128 0:b14dfd8816da 138 pc.printf("\nEEPROM VALUES\n");
jah128 0:b14dfd8816da 139
jah128 0:b14dfd8816da 140 int modelnumber = data[0] + (data[1] << 8);
jah128 0:b14dfd8816da 141 pc.printf("Model Number : %x [",modelnumber);
jah128 0:b14dfd8816da 142 switch(modelnumber) {
jah128 0:b14dfd8816da 143 case (AX12_MODEL):
jah128 0:b14dfd8816da 144 pc.printf("AX12]\n");
jah128 0:b14dfd8816da 145 break;
jah128 0:b14dfd8816da 146 case (MX28_MODEL):
jah128 0:b14dfd8816da 147 pc.printf("MX28]\n");
jah128 0:b14dfd8816da 148 break;
jah128 0:b14dfd8816da 149 default:
jah128 0:b14dfd8816da 150 pc.printf("UNKNOWN]\n");
jah128 0:b14dfd8816da 151 break;
jah128 0:b14dfd8816da 152 }
jah128 0:b14dfd8816da 153 pc.printf("Firmware Version : %x\n",data[2]);
jah128 0:b14dfd8816da 154 pc.printf("ID : %x\n",data[3]);
jah128 0:b14dfd8816da 155 int baudrate = 2000000 / (data[4] + 1);
jah128 0:b14dfd8816da 156 //Special high-speed baudrates [for MX28 only]
jah128 0:b14dfd8816da 157 if(data[4] == 250) baudrate = 2250000;
jah128 0:b14dfd8816da 158 if(data[4] == 251) baudrate = 2500000;
jah128 0:b14dfd8816da 159 if(data[4] == 252) baudrate = 3000000;
jah128 0:b14dfd8816da 160 pc.printf("Baud Rate : %x [%d]\n",data[4],baudrate);
jah128 0:b14dfd8816da 161 pc.printf("Return Delay Time : %x [%duS]\n",data[5],(data[5] * 2));
jah128 0:b14dfd8816da 162 short cw_angle_limit = data[6] + (data[7] << 8);
jah128 0:b14dfd8816da 163 short ccw_angle_limit = data[8] + (data[9] << 8);
jah128 0:b14dfd8816da 164 pc.printf("CW Angle Limit : %x [%d",cw_angle_limit,cw_angle_limit);
jah128 0:b14dfd8816da 165 if(cw_angle_limit ==0 && ccw_angle_limit == 0)pc.printf(" - Wheel Mode]\n");
jah128 0:b14dfd8816da 166 else {
jah128 0:b14dfd8816da 167 if(cw_angle_limit == 4095 && ccw_angle_limit == 4095)pc.printf(" - Multiturn Mode]\n");
jah128 0:b14dfd8816da 168 else pc.printf("- Joint Mode]\n");
jah128 0:b14dfd8816da 169 }
jah128 0:b14dfd8816da 170 pc.printf("CCW Angle Limit : %x [%d",ccw_angle_limit,ccw_angle_limit);
jah128 0:b14dfd8816da 171 if(cw_angle_limit ==0 && ccw_angle_limit == 0)pc.printf(" - Wheel Mode]\n");
jah128 0:b14dfd8816da 172 else {
jah128 0:b14dfd8816da 173 if(cw_angle_limit == 4095 && ccw_angle_limit == 4095)pc.printf(" - Multiturn Mode]\n");
jah128 0:b14dfd8816da 174 else pc.printf("- Joint Mode]\n");
jah128 0:b14dfd8816da 175 }
jah128 0:b14dfd8816da 176 //Fill in blanks
jah128 0:b14dfd8816da 177 pc.printf("High Temp Limit : %x [%dC]\n",data[11],data[11]);
jah128 0:b14dfd8816da 178 pc.printf("Low Voltage Limit : %x [%2.1fV]\n",data[12],(float) (data[12]*0.1f));
jah128 0:b14dfd8816da 179 pc.printf("High Voltage Limit: %x [%2.1fV]\n",data[13],(float) (data[13]*0.1f));
jah128 0:b14dfd8816da 180 short max_torque = data[14] + (data[15] << 8);
jah128 0:b14dfd8816da 181 float pct_max_torque = (float) (max_torque / 10.23f);
jah128 0:b14dfd8816da 182 pc.printf("Preset Max Torque : %x [%3.2f%%]\n",max_torque,pct_max_torque);
jah128 0:b14dfd8816da 183 pc.printf("Status Return Lev.: %x [%d]\n",data[16]);
jah128 0:b14dfd8816da 184 pc.printf("Alarm LED : %x [%d]\n",data[17]);
jah128 0:b14dfd8816da 185 pc.printf("Alarm Shutdown : %x [%d]\n",data[18]);
jah128 0:b14dfd8816da 186 short multiturn_offset = data[20] + (data[21] << 8);
jah128 0:b14dfd8816da 187 pc.printf("Multiturn Offset : %x [%d]\n",multiturn_offset,multiturn_offset);
jah128 0:b14dfd8816da 188 pc.printf("\nRAM VALUES\n");
jah128 0:b14dfd8816da 189 pc.printf("Torque Enable : %x\n",data[24]);
jah128 0:b14dfd8816da 190 pc.printf("LED : %x\n",data[25]);
jah128 0:b14dfd8816da 191 pc.printf("D Gain : %x [%d]\n",data[26],data[26]);
jah128 0:b14dfd8816da 192 pc.printf("I Gain : %x [%d]\n",data[27],data[27]);
jah128 0:b14dfd8816da 193 pc.printf("P Gain : %x [%d]\n",data[28],data[28]);
jah128 0:b14dfd8816da 194 short goal_position = data[30] + (data[31] << 8);
jah128 0:b14dfd8816da 195 float gp_degrees = (goal_position - 2048) * 0.087890625;
jah128 0:b14dfd8816da 196 pc.printf("Goal Position : %x [%d: %3.2f degrees]\n",goal_position,goal_position,gp_degrees);
jah128 0:b14dfd8816da 197 short moving_speed = data[32] + (data[33] << 8);
jah128 0:b14dfd8816da 198 float mv_rpm = moving_speed * 0.114;
jah128 0:b14dfd8816da 199 pc.printf("Moving Speed : %x [%d: %4.2 rpm]\n",moving_speed,moving_speed,mv_rpm);
jah128 0:b14dfd8816da 200 short c_max_torque = data[34] + (data[35] << 8);
jah128 0:b14dfd8816da 201 float cpct_max_torque = (float) (c_max_torque / 10.23f);
jah128 0:b14dfd8816da 202 pc.printf("Current Max Torque: %x [%3.2f%%]\n",c_max_torque,cpct_max_torque);
jah128 0:b14dfd8816da 203 short present_position = data[36] + (data[37] << 8);
jah128 0:b14dfd8816da 204 float pp_degrees = present_position * 0.088f;
jah128 0:b14dfd8816da 205 pc.printf("Present Position : %x [%d: %3.2f degrees]\n",present_position,present_position,pp_degrees);
jah128 0:b14dfd8816da 206 short present_speed = data[38] + (data[39] << 8);
jah128 0:b14dfd8816da 207 float p_rpm = present_speed * 0.114;
jah128 0:b14dfd8816da 208 pc.printf("Present Speed : %x [%d: %4.2 rpm]\n",present_speed,present_speed,p_rpm);
jah128 0:b14dfd8816da 209 short present_load = data[40] + (data[41] << 8);
jah128 0:b14dfd8816da 210 if(present_load < 1024) {
jah128 0:b14dfd8816da 211 float present_loadpct = (1024 - present_load) / 10.23f;
jah128 0:b14dfd8816da 212 pc.printf("Present Load : %x [%3.2f%% CCW]\n",present_load,present_loadpct);
jah128 0:b14dfd8816da 213 } else {
jah128 0:b14dfd8816da 214 if(present_load > 1024) {
jah128 0:b14dfd8816da 215 float present_loadpct_cw = (present_load - 1024) / 10.23f;
jah128 0:b14dfd8816da 216 pc.printf("Present Load : %x [%3.2f%% CW]\n",present_load,present_loadpct_cw);
jah128 0:b14dfd8816da 217 } else pc.printf("Present Load : %x [NONE]\n",present_load);
jah128 0:b14dfd8816da 218 }
jah128 0:b14dfd8816da 219 pc.printf("Voltage : %x [%fV]\n",data[42],(data[42] * 0.1f));
jah128 0:b14dfd8816da 220 pc.printf("Temperature : %x [%dC]\n",data[43],data[43]);
jah128 0:b14dfd8816da 221
jah128 0:b14dfd8816da 222
jah128 0:b14dfd8816da 223
jah128 0:b14dfd8816da 224
jah128 0:b14dfd8816da 225 }
jah128 0:b14dfd8816da 226
jah128 0:b14dfd8816da 227 // Set the mode of the servo
jah128 0:b14dfd8816da 228 // 0 = Positional (0-300 degrees)
jah128 0:b14dfd8816da 229 // 1 = Rotational -1 to 1 speed
jah128 0:b14dfd8816da 230 int Servo::SetMode(int ID, int mode)
jah128 0:b14dfd8816da 231 {
jah128 0:b14dfd8816da 232
jah128 0:b14dfd8816da 233 if (mode == 1) { // set CR
jah128 0:b14dfd8816da 234 SetCWLimit(ID, 0);
jah128 0:b14dfd8816da 235 SetCCWLimit(ID, 0);
jah128 0:b14dfd8816da 236 SetCRSpeed(ID, 0.0);
jah128 0:b14dfd8816da 237 } else {
jah128 0:b14dfd8816da 238 SetCWLimit(ID, 0);
jah128 0:b14dfd8816da 239 SetCCWLimit(ID, 300);
jah128 0:b14dfd8816da 240 SetCRSpeed(ID, 0.0);
jah128 0:b14dfd8816da 241 }
jah128 0:b14dfd8816da 242 return(0);
jah128 0:b14dfd8816da 243 }
jah128 0:b14dfd8816da 244
jah128 0:b14dfd8816da 245 // if flag[0] is set, were blocking
jah128 0:b14dfd8816da 246 // if flag[1] is set, we're registering
jah128 0:b14dfd8816da 247 // they are mutually exclusive operations
jah128 0:b14dfd8816da 248 int Servo::SetGoal(int ID, short goal, int flags)
jah128 0:b14dfd8816da 249 {
jah128 0:b14dfd8816da 250
jah128 0:b14dfd8816da 251 char reg_flag = 0;
jah128 0:b14dfd8816da 252 char data[2];
jah128 0:b14dfd8816da 253
jah128 0:b14dfd8816da 254 // set the flag is only the register bit is set in the flag
jah128 0:b14dfd8816da 255 if (flags == 0x2) {
jah128 0:b14dfd8816da 256 reg_flag = 1;
jah128 0:b14dfd8816da 257 }
jah128 0:b14dfd8816da 258 if(GetLowerLimit(ID) > goal){
jah128 0:b14dfd8816da 259 goal=GetLowerLimit(ID);
jah128 0:b14dfd8816da 260 if(USE_LIMIT_WARNING == 1){
jah128 0:b14dfd8816da 261 display.clear_display();
jah128 0:b14dfd8816da 262 display.set_position(0,0);
jah128 0:b14dfd8816da 263 display.write_string("RANGE ERROR");
jah128 0:b14dfd8816da 264 }
jah128 0:b14dfd8816da 265 }
jah128 0:b14dfd8816da 266 if(GetUpperLimit(ID) < goal){
jah128 0:b14dfd8816da 267 goal=GetUpperLimit(ID);
jah128 0:b14dfd8816da 268 if(USE_LIMIT_WARNING == 1){
jah128 0:b14dfd8816da 269 display.clear_display();
jah128 0:b14dfd8816da 270 display.set_position(0,0);
jah128 0:b14dfd8816da 271 display.write_string("RANGE ERROR");
jah128 0:b14dfd8816da 272 }
jah128 0:b14dfd8816da 273 }
jah128 0:b14dfd8816da 274 if (DEBUG) {
jah128 0:b14dfd8816da 275 pc.printf("SetGoal to 0x%x ",goal);
jah128 0:b14dfd8816da 276 }
jah128 0:b14dfd8816da 277
jah128 0:b14dfd8816da 278 // Apply inversions if set
jah128 0:b14dfd8816da 279 switch(ID){
jah128 0:b14dfd8816da 280 case(BASE):if(INVERT_BASE == 1)goal=4095-goal;break;
jah128 0:b14dfd8816da 281 case(SHOULDER):if(INVERT_SHOULDER == 1)goal=4095-goal;break;
jah128 0:b14dfd8816da 282 case(ELBOW):if(INVERT_ELBOW == 1)goal=4095-goal;break;
jah128 0:b14dfd8816da 283 case(WRIST):if(INVERT_BASE == 1)goal=4095-goal;break;
jah128 0:b14dfd8816da 284
jah128 0:b14dfd8816da 285 }
jah128 0:b14dfd8816da 286
jah128 0:b14dfd8816da 287 data[0] = goal & 0xff; // bottom 8 bits
jah128 0:b14dfd8816da 288 data[1] = goal >> 8; // top 8 bits
jah128 0:b14dfd8816da 289
jah128 0:b14dfd8816da 290 // write the packet, return the error code
jah128 0:b14dfd8816da 291 int rVal = write(ID, REG_GOAL_POSITION, 2, data, reg_flag);
jah128 0:b14dfd8816da 292
jah128 0:b14dfd8816da 293 if (flags == 1) {
jah128 0:b14dfd8816da 294 // block until it comes to a halt
jah128 0:b14dfd8816da 295 if (DEBUG) pc.printf(" [WAITING]");
jah128 0:b14dfd8816da 296 while (isMoving(ID)) {}
jah128 0:b14dfd8816da 297 }
jah128 0:b14dfd8816da 298 if (DEBUG) pc.printf("\n");
jah128 0:b14dfd8816da 299 return(rVal);
jah128 0:b14dfd8816da 300 }
jah128 0:b14dfd8816da 301
jah128 0:b14dfd8816da 302 // if flag[0] is set, were blocking
jah128 0:b14dfd8816da 303 // if flag[1] is set, we're registering
jah128 0:b14dfd8816da 304 // they are mutually exclusive operations
jah128 0:b14dfd8816da 305 int Servo::SetGoalDegrees(int ID, int degrees, int flags)
jah128 0:b14dfd8816da 306 {
jah128 0:b14dfd8816da 307 short goal = (degrees * 11.377778) + 2048;
jah128 0:b14dfd8816da 308 return SetGoal(ID,goal,flags);
jah128 0:b14dfd8816da 309 }
jah128 0:b14dfd8816da 310
jah128 0:b14dfd8816da 311
jah128 0:b14dfd8816da 312 // Set continuous rotation speed from -1 to 1
jah128 0:b14dfd8816da 313 int Servo::SetCRSpeed(int ID, float speed)
jah128 0:b14dfd8816da 314 {
jah128 0:b14dfd8816da 315
jah128 0:b14dfd8816da 316 // bit 10 = direction, 0 = CCW, 1=CW
jah128 0:b14dfd8816da 317 // bits 9-0 = Speed
jah128 0:b14dfd8816da 318 char data[2];
jah128 0:b14dfd8816da 319
jah128 0:b14dfd8816da 320 int goal = (0x3ff * abs(speed));
jah128 0:b14dfd8816da 321
jah128 0:b14dfd8816da 322 // Set direction CW if we have a negative speed
jah128 0:b14dfd8816da 323 if (speed < 0) {
jah128 0:b14dfd8816da 324 goal |= (0x1 << 10);
jah128 0:b14dfd8816da 325 }
jah128 0:b14dfd8816da 326
jah128 0:b14dfd8816da 327 data[0] = goal & 0xff; // bottom 8 bits
jah128 0:b14dfd8816da 328 data[1] = goal >> 8; // top 8 bits
jah128 0:b14dfd8816da 329
jah128 0:b14dfd8816da 330 // write the packet, return the error code
jah128 0:b14dfd8816da 331 int rVal = write(ID, 0x20, 2, data);
jah128 0:b14dfd8816da 332
jah128 0:b14dfd8816da 333 return(rVal);
jah128 0:b14dfd8816da 334 }
jah128 0:b14dfd8816da 335
jah128 0:b14dfd8816da 336
jah128 0:b14dfd8816da 337 int Servo::SetCWLimit (int ID, int degrees)
jah128 0:b14dfd8816da 338 {
jah128 0:b14dfd8816da 339
jah128 0:b14dfd8816da 340 char data[2];
jah128 0:b14dfd8816da 341
jah128 0:b14dfd8816da 342 // 1023 / 300 * degrees
jah128 0:b14dfd8816da 343 short limit = (1023 * degrees) / 300;
jah128 0:b14dfd8816da 344
jah128 0:b14dfd8816da 345 if (DEBUG) {
jah128 0:b14dfd8816da 346 pc.printf("SetCWLimit to 0x%x\n",limit);
jah128 0:b14dfd8816da 347 }
jah128 0:b14dfd8816da 348
jah128 0:b14dfd8816da 349 data[0] = limit & 0xff; // bottom 8 bits
jah128 0:b14dfd8816da 350 data[1] = limit >> 8; // top 8 bits
jah128 0:b14dfd8816da 351
jah128 0:b14dfd8816da 352 // write the packet, return the error code
jah128 0:b14dfd8816da 353 return (write(ID, REG_CW_LIMIT, 2, data));
jah128 0:b14dfd8816da 354
jah128 0:b14dfd8816da 355 }
jah128 0:b14dfd8816da 356
jah128 0:b14dfd8816da 357 int Servo::SetCCWLimit (int ID, int degrees)
jah128 0:b14dfd8816da 358 {
jah128 0:b14dfd8816da 359
jah128 0:b14dfd8816da 360 char data[2];
jah128 0:b14dfd8816da 361
jah128 0:b14dfd8816da 362 // 1023 / 300 * degrees
jah128 0:b14dfd8816da 363 short limit = (1023 * degrees) / 300;
jah128 0:b14dfd8816da 364
jah128 0:b14dfd8816da 365 if (DEBUG) {
jah128 0:b14dfd8816da 366 pc.printf("SetCCWLimit to 0x%x\n",limit);
jah128 0:b14dfd8816da 367 }
jah128 0:b14dfd8816da 368
jah128 0:b14dfd8816da 369 data[0] = limit & 0xff; // bottom 8 bits
jah128 0:b14dfd8816da 370 data[1] = limit >> 8; // top 8 bits
jah128 0:b14dfd8816da 371
jah128 0:b14dfd8816da 372 // write the packet, return the error code
jah128 0:b14dfd8816da 373 return (write(ID, REG_CCW_LIMIT, 2, data));
jah128 0:b14dfd8816da 374 }
jah128 0:b14dfd8816da 375
jah128 0:b14dfd8816da 376 int Servo::SetTorqueEnable (int ID, int enable)
jah128 0:b14dfd8816da 377 {
jah128 0:b14dfd8816da 378 char data[1];
jah128 0:b14dfd8816da 379 data[0]=enable;
jah128 0:b14dfd8816da 380 if (DEBUG) {
jah128 0:b14dfd8816da 381 pc.printf("SetTorqueEnable to %d\n",enable);
jah128 0:b14dfd8816da 382 }
jah128 0:b14dfd8816da 383
jah128 0:b14dfd8816da 384
jah128 0:b14dfd8816da 385 // write the packet, return the error code
jah128 0:b14dfd8816da 386 return (write(ID, REG_TORQUE_ENABLE, 1, data));
jah128 0:b14dfd8816da 387 }
jah128 0:b14dfd8816da 388
jah128 0:b14dfd8816da 389 int Servo::SetLowVoltageLimit (int ID, char lv_limit)
jah128 0:b14dfd8816da 390 {
jah128 0:b14dfd8816da 391
jah128 0:b14dfd8816da 392 char data[1];
jah128 0:b14dfd8816da 393 data[0] = lv_limit;
jah128 0:b14dfd8816da 394 if (DEBUG) {
jah128 0:b14dfd8816da 395 pc.printf("Setting low voltage limit to %2.1f\n",(float) lv_limit / 10.0);
jah128 0:b14dfd8816da 396 }
jah128 0:b14dfd8816da 397 return (write(ID, REG_LOW_VOLTAGE_LIMIT, 1, data));
jah128 0:b14dfd8816da 398 }
jah128 0:b14dfd8816da 399
jah128 0:b14dfd8816da 400 int Servo::LockEeprom (int ID)
jah128 0:b14dfd8816da 401 {
jah128 0:b14dfd8816da 402 char data[1];
jah128 0:b14dfd8816da 403 data[0]=1;
jah128 0:b14dfd8816da 404 if (DEBUG) {
jah128 0:b14dfd8816da 405 pc.printf("Locking EEPROM\n");
jah128 0:b14dfd8816da 406 }
jah128 0:b14dfd8816da 407 return (write(ID, REG_EEPROM_LOCK, 1, data));
jah128 0:b14dfd8816da 408 }
jah128 0:b14dfd8816da 409
jah128 0:b14dfd8816da 410 int Servo::SetHighVoltageLimit (int ID, char hv_limit)
jah128 0:b14dfd8816da 411 {
jah128 0:b14dfd8816da 412
jah128 0:b14dfd8816da 413 char data[1];
jah128 0:b14dfd8816da 414 data[0] = hv_limit;
jah128 0:b14dfd8816da 415 if (DEBUG) {
jah128 0:b14dfd8816da 416 pc.printf("Setting high voltage limit to %2.1f\n",(float) hv_limit / 10.0);
jah128 0:b14dfd8816da 417 }
jah128 0:b14dfd8816da 418 return (write(ID, REG_HIGH_VOLTAGE_LIMIT, 1, data));
jah128 0:b14dfd8816da 419 }
jah128 0:b14dfd8816da 420
jah128 0:b14dfd8816da 421 int Servo::SetDelayTime (int ID, char delay)
jah128 0:b14dfd8816da 422 {
jah128 0:b14dfd8816da 423 char data[1];
jah128 0:b14dfd8816da 424 data[0] = delay;
jah128 0:b14dfd8816da 425 if (DEBUG) {
jah128 0:b14dfd8816da 426 pc.printf("Setting delay time to %dus\n",delay+delay);
jah128 0:b14dfd8816da 427 }
jah128 0:b14dfd8816da 428 return (write(ID, REG_RETURN_DELAY, 1, data));
jah128 0:b14dfd8816da 429 }
jah128 0:b14dfd8816da 430
jah128 0:b14dfd8816da 431
jah128 0:b14dfd8816da 432
jah128 0:b14dfd8816da 433 int Servo::SetTemperatureLimit (int ID, char temp_limit)
jah128 0:b14dfd8816da 434 {
jah128 0:b14dfd8816da 435
jah128 0:b14dfd8816da 436 char data[1];
jah128 0:b14dfd8816da 437 data[0] = temp_limit;
jah128 0:b14dfd8816da 438 if (DEBUG) {
jah128 0:b14dfd8816da 439 pc.printf("Setting temperature limit to %dC\n",temp_limit);
jah128 0:b14dfd8816da 440 }
jah128 0:b14dfd8816da 441 return (write(ID, REG_HIGHTEMP_LIMIT, 1, data));
jah128 0:b14dfd8816da 442 }
jah128 0:b14dfd8816da 443
jah128 0:b14dfd8816da 444 int Servo::SetID (int CurrentID, int NewID)
jah128 0:b14dfd8816da 445 {
jah128 0:b14dfd8816da 446
jah128 0:b14dfd8816da 447 char data[1];
jah128 0:b14dfd8816da 448 data[0] = NewID;
jah128 0:b14dfd8816da 449 if (DEBUG) {
jah128 0:b14dfd8816da 450 pc.printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
jah128 0:b14dfd8816da 451 }
jah128 0:b14dfd8816da 452 return (write(CurrentID, REG_ID, 1, data));
jah128 0:b14dfd8816da 453
jah128 0:b14dfd8816da 454 }
jah128 0:b14dfd8816da 455
jah128 0:b14dfd8816da 456 int Servo::SetBaud (int ID, int baud)
jah128 0:b14dfd8816da 457 {
jah128 0:b14dfd8816da 458
jah128 0:b14dfd8816da 459 char data[1];
jah128 0:b14dfd8816da 460 data[0] = baud;
jah128 0:b14dfd8816da 461 if (DEBUG) {
jah128 0:b14dfd8816da 462 pc.printf("Setting baud to %d\n",(2000000 / baud));
jah128 0:b14dfd8816da 463 }
jah128 0:b14dfd8816da 464 return (write(ID, REG_BAUDRATE, 1, data));
jah128 0:b14dfd8816da 465
jah128 0:b14dfd8816da 466 }
jah128 0:b14dfd8816da 467
jah128 0:b14dfd8816da 468
jah128 0:b14dfd8816da 469 // return 1 is the servo is still in flight
jah128 0:b14dfd8816da 470 int Servo::isMoving(int ID)
jah128 0:b14dfd8816da 471 {
jah128 0:b14dfd8816da 472
jah128 0:b14dfd8816da 473 char data[1];
jah128 0:b14dfd8816da 474 read(ID,REG_MOVING,1,data);
jah128 0:b14dfd8816da 475 return(data[0]);
jah128 0:b14dfd8816da 476 }
jah128 0:b14dfd8816da 477
jah128 0:b14dfd8816da 478
jah128 0:b14dfd8816da 479 void Servo::trigger(void)
jah128 0:b14dfd8816da 480 {
jah128 0:b14dfd8816da 481
jah128 0:b14dfd8816da 482 char TxBuf[16];
jah128 0:b14dfd8816da 483 char sum = 0;
jah128 0:b14dfd8816da 484
jah128 0:b14dfd8816da 485 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 486 pc.printf("\nTriggered\n");
jah128 0:b14dfd8816da 487 }
jah128 0:b14dfd8816da 488
jah128 0:b14dfd8816da 489 // Build the TxPacket first in RAM, then we'll send in one go
jah128 0:b14dfd8816da 490 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 491 pc.printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
jah128 0:b14dfd8816da 492 }
jah128 0:b14dfd8816da 493
jah128 0:b14dfd8816da 494 TxBuf[0] = 0xFF;
jah128 0:b14dfd8816da 495 TxBuf[1] = 0xFF;
jah128 0:b14dfd8816da 496
jah128 0:b14dfd8816da 497 // ID - Broadcast
jah128 0:b14dfd8816da 498 TxBuf[2] = 0xFE;
jah128 0:b14dfd8816da 499 sum += TxBuf[2];
jah128 0:b14dfd8816da 500
jah128 0:b14dfd8816da 501 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 502 pc.printf(" ID : %d\n",TxBuf[2]);
jah128 0:b14dfd8816da 503 }
jah128 0:b14dfd8816da 504
jah128 0:b14dfd8816da 505 // Length
jah128 0:b14dfd8816da 506 TxBuf[3] = 0x02;
jah128 0:b14dfd8816da 507 sum += TxBuf[3];
jah128 0:b14dfd8816da 508 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 509 pc.printf(" Length %d\n",TxBuf[3]);
jah128 0:b14dfd8816da 510 }
jah128 0:b14dfd8816da 511
jah128 0:b14dfd8816da 512 // Instruction - ACTION
jah128 0:b14dfd8816da 513 TxBuf[4] = 0x04;
jah128 0:b14dfd8816da 514 sum += TxBuf[4];
jah128 0:b14dfd8816da 515 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 516 pc.printf(" Instruction 0x%X\n",TxBuf[5]);
jah128 0:b14dfd8816da 517 }
jah128 0:b14dfd8816da 518
jah128 0:b14dfd8816da 519 // Checksum
jah128 0:b14dfd8816da 520 TxBuf[5] = 0xFF - sum;
jah128 0:b14dfd8816da 521 if (TRIGGER_DEBUG) {
jah128 0:b14dfd8816da 522 pc.printf(" Checksum 0x%X\n",TxBuf[5]);
jah128 0:b14dfd8816da 523 }
jah128 0:b14dfd8816da 524
jah128 0:b14dfd8816da 525 // Transmit the packet in one burst with no pausing
jah128 0:b14dfd8816da 526 for (int i = 0; i < 6 ; i++) {
jah128 0:b14dfd8816da 527 _servo.putc(TxBuf[i]);
jah128 0:b14dfd8816da 528 }
jah128 0:b14dfd8816da 529
jah128 0:b14dfd8816da 530 // This is a broadcast packet, so there will be no reply
jah128 0:b14dfd8816da 531
jah128 0:b14dfd8816da 532 return;
jah128 0:b14dfd8816da 533 }
jah128 0:b14dfd8816da 534
jah128 0:b14dfd8816da 535 int Servo::GetModelNumber(int ID)
jah128 0:b14dfd8816da 536 {
jah128 0:b14dfd8816da 537 if (DEBUG) {
jah128 0:b14dfd8816da 538 pc.printf("\nGetModelNumber(%d)",ID);
jah128 0:b14dfd8816da 539 }
jah128 0:b14dfd8816da 540 char data[2];
jah128 0:b14dfd8816da 541 int ErrorCode = read(ID, REG_MODEL_NUMBER, 2, data);
jah128 0:b14dfd8816da 542 int modelnumber = data[0] + (data[1] << 8);
jah128 0:b14dfd8816da 543 return (modelnumber);
jah128 0:b14dfd8816da 544 }
jah128 0:b14dfd8816da 545
jah128 0:b14dfd8816da 546 float Servo::GetPositionDegrees(int ID)
jah128 0:b14dfd8816da 547 {
jah128 0:b14dfd8816da 548 short position = GetPosition(ID);
jah128 0:b14dfd8816da 549 //float angle = (position * 300)/1024; FOR AX-12
jah128 0:b14dfd8816da 550 float angle = (position - 2048) * 0.087890625;
jah128 0:b14dfd8816da 551
jah128 0:b14dfd8816da 552 return (angle);
jah128 0:b14dfd8816da 553 }
jah128 0:b14dfd8816da 554
jah128 0:b14dfd8816da 555 short Servo::GetPosition(int ID)
jah128 0:b14dfd8816da 556 {
jah128 0:b14dfd8816da 557
jah128 0:b14dfd8816da 558 if (DEBUG) {
jah128 0:b14dfd8816da 559 pc.printf("\nGetPosition(%d)",ID);
jah128 0:b14dfd8816da 560 }
jah128 0:b14dfd8816da 561
jah128 0:b14dfd8816da 562 char data[2];
jah128 0:b14dfd8816da 563
jah128 0:b14dfd8816da 564 int ErrorCode = read(ID, REG_POSITION, 2, data);
jah128 0:b14dfd8816da 565 if (DEBUG) {
jah128 0:b14dfd8816da 566 pc.printf("[EC=%d]",ErrorCode);
jah128 0:b14dfd8816da 567 }
jah128 0:b14dfd8816da 568 short position = data[0] + (data[1] << 8);
jah128 0:b14dfd8816da 569
jah128 0:b14dfd8816da 570 // Apply inversions if set
jah128 0:b14dfd8816da 571 switch(ID){
jah128 0:b14dfd8816da 572 case(BASE):if(INVERT_BASE == 1)position=4095-position;break;
jah128 0:b14dfd8816da 573 case(SHOULDER):if(INVERT_SHOULDER == 1)position=4095-position;break;
jah128 0:b14dfd8816da 574 case(ELBOW):if(INVERT_ELBOW == 1)position=4095-position;break;
jah128 0:b14dfd8816da 575 case(WRIST):if(INVERT_BASE == 1)position=4095-position;break;
jah128 0:b14dfd8816da 576
jah128 0:b14dfd8816da 577 }
jah128 0:b14dfd8816da 578 return (position);
jah128 0:b14dfd8816da 579 }
jah128 0:b14dfd8816da 580
jah128 0:b14dfd8816da 581
jah128 0:b14dfd8816da 582 float Servo::GetTemp (int ID)
jah128 0:b14dfd8816da 583 {
jah128 0:b14dfd8816da 584
jah128 0:b14dfd8816da 585 if (DEBUG) {
jah128 0:b14dfd8816da 586 pc.printf("\nGetTemp(%d)",ID);
jah128 0:b14dfd8816da 587 }
jah128 0:b14dfd8816da 588 char data[1];
jah128 0:b14dfd8816da 589 int ErrorCode = read(ID, REG_TEMP, 1, data);
jah128 0:b14dfd8816da 590 float temp = data[0];
jah128 0:b14dfd8816da 591 return(temp);
jah128 0:b14dfd8816da 592 }
jah128 0:b14dfd8816da 593
jah128 0:b14dfd8816da 594 short Servo::GetTemperature(int ID)
jah128 0:b14dfd8816da 595 {
jah128 0:b14dfd8816da 596 if (DEBUG) {
jah128 0:b14dfd8816da 597 pc.printf("\nGetTemperature(%d)",ID);
jah128 0:b14dfd8816da 598 }
jah128 0:b14dfd8816da 599 char data[1];
jah128 0:b14dfd8816da 600 int ErrorCode = read(ID, REG_TEMP, 1, data);
jah128 0:b14dfd8816da 601 return (short) (data[0]);
jah128 0:b14dfd8816da 602 }
jah128 0:b14dfd8816da 603
jah128 0:b14dfd8816da 604 float Servo::GetVolts (int ID)
jah128 0:b14dfd8816da 605 {
jah128 0:b14dfd8816da 606 if (DEBUG) {
jah128 0:b14dfd8816da 607 pc.printf("\nGetVolts(%d)",ID);
jah128 0:b14dfd8816da 608 }
jah128 0:b14dfd8816da 609 char data[1];
jah128 0:b14dfd8816da 610 int ErrorCode = read(ID, REG_VOLTS, 1, data);
jah128 0:b14dfd8816da 611 float volts = data[0]/10.0;
jah128 0:b14dfd8816da 612 return(volts);
jah128 0:b14dfd8816da 613 }
jah128 0:b14dfd8816da 614
jah128 0:b14dfd8816da 615 short Servo::GetVoltage(int ID)
jah128 0:b14dfd8816da 616 {
jah128 0:b14dfd8816da 617 if (DEBUG) {
jah128 0:b14dfd8816da 618 pc.printf("\nGetVoltage(%d)",ID);
jah128 0:b14dfd8816da 619 }
jah128 0:b14dfd8816da 620 char data[1];
jah128 0:b14dfd8816da 621 int ErrorCode = read(ID, REG_VOLTS, 1, data);
jah128 0:b14dfd8816da 622 return (short) (data[0]);
jah128 0:b14dfd8816da 623 }
jah128 0:b14dfd8816da 624
jah128 0:b14dfd8816da 625 short Servo::GetLoad(int ID)
jah128 0:b14dfd8816da 626 {
jah128 0:b14dfd8816da 627 if (DEBUG) {
jah128 0:b14dfd8816da 628 pc.printf("\nGetLoad(%d)",ID);
jah128 0:b14dfd8816da 629 }
jah128 0:b14dfd8816da 630 char data[2];
jah128 0:b14dfd8816da 631 int ErrorCode = read(ID, REG_LOAD, 2, data);
jah128 0:b14dfd8816da 632 return (short) (data[0] + (data[1]<<8));
jah128 0:b14dfd8816da 633 }
jah128 0:b14dfd8816da 634
jah128 0:b14dfd8816da 635 short Servo::GetSpeed(int ID)
jah128 0:b14dfd8816da 636 {
jah128 0:b14dfd8816da 637 if (DEBUG) {
jah128 0:b14dfd8816da 638 pc.printf("\nGetSpeed(%d)",ID);
jah128 0:b14dfd8816da 639 }
jah128 0:b14dfd8816da 640 char data[2];
jah128 0:b14dfd8816da 641 int ErrorCode = read(ID, REG_SPEED, 2, data);
jah128 0:b14dfd8816da 642 return (short) (data[0] + (data[1]<<8));
jah128 0:b14dfd8816da 643 }
jah128 0:b14dfd8816da 644
jah128 0:b14dfd8816da 645 int Servo::read(int ID, int start, int bytes, char* data)
jah128 0:b14dfd8816da 646 {
jah128 0:b14dfd8816da 647
jah128 0:b14dfd8816da 648 char PacketLength = 0x4;
jah128 0:b14dfd8816da 649 char TxBuf[16];
jah128 0:b14dfd8816da 650 char sum = 0;
jah128 0:b14dfd8816da 651 char Status[16];
jah128 0:b14dfd8816da 652
jah128 0:b14dfd8816da 653 Status[4] = 0xFE; // return code
jah128 0:b14dfd8816da 654
jah128 0:b14dfd8816da 655 if (READ_DEBUG) {
jah128 0:b14dfd8816da 656 pc.printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
jah128 0:b14dfd8816da 657 }
jah128 0:b14dfd8816da 658
jah128 0:b14dfd8816da 659 // Build the TxPacket first in RAM, then we'll send in one go
jah128 0:b14dfd8816da 660 if (READ_DEBUG) {
jah128 0:b14dfd8816da 661 pc.printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
jah128 0:b14dfd8816da 662 }
jah128 0:b14dfd8816da 663
jah128 0:b14dfd8816da 664 TxBuf[0] = 0xff;
jah128 0:b14dfd8816da 665 TxBuf[1] = 0xff;
jah128 0:b14dfd8816da 666
jah128 0:b14dfd8816da 667 // ID
jah128 0:b14dfd8816da 668 TxBuf[2] = ID;
jah128 0:b14dfd8816da 669 sum += TxBuf[2];
jah128 0:b14dfd8816da 670 if (READ_DEBUG) {
jah128 0:b14dfd8816da 671 pc.printf(" ID : %d\n",TxBuf[2]);
jah128 0:b14dfd8816da 672 }
jah128 0:b14dfd8816da 673
jah128 0:b14dfd8816da 674 // Packet Length
jah128 0:b14dfd8816da 675 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
jah128 0:b14dfd8816da 676 sum += TxBuf[3]; // Accululate the packet sum
jah128 0:b14dfd8816da 677 if (READ_DEBUG) {
jah128 0:b14dfd8816da 678 pc.printf(" Length : 0x%x\n",TxBuf[3]);
jah128 0:b14dfd8816da 679 }
jah128 0:b14dfd8816da 680
jah128 0:b14dfd8816da 681 // Instruction - Read
jah128 0:b14dfd8816da 682 TxBuf[4] = 0x2;
jah128 0:b14dfd8816da 683 sum += TxBuf[4];
jah128 0:b14dfd8816da 684 if (READ_DEBUG) {
jah128 0:b14dfd8816da 685 pc.printf(" Instruction : 0x%x\n",TxBuf[4]);
jah128 0:b14dfd8816da 686 }
jah128 0:b14dfd8816da 687
jah128 0:b14dfd8816da 688 // Start Address
jah128 0:b14dfd8816da 689 TxBuf[5] = start;
jah128 0:b14dfd8816da 690 sum += TxBuf[5];
jah128 0:b14dfd8816da 691 if (READ_DEBUG) {
jah128 0:b14dfd8816da 692 pc.printf(" Start Address : 0x%x\n",TxBuf[5]);
jah128 0:b14dfd8816da 693 }
jah128 0:b14dfd8816da 694
jah128 0:b14dfd8816da 695 // Bytes to read
jah128 0:b14dfd8816da 696 TxBuf[6] = bytes;
jah128 0:b14dfd8816da 697 sum += TxBuf[6];
jah128 0:b14dfd8816da 698 if (READ_DEBUG) {
jah128 0:b14dfd8816da 699 pc.printf(" No bytes : 0x%x\n",TxBuf[6]);
jah128 0:b14dfd8816da 700 }
jah128 0:b14dfd8816da 701
jah128 0:b14dfd8816da 702 // Checksum
jah128 0:b14dfd8816da 703 TxBuf[7] = 0xFF - sum;
jah128 0:b14dfd8816da 704 if (READ_DEBUG) {
jah128 0:b14dfd8816da 705 pc.printf(" Checksum : 0x%x\n",TxBuf[7]);
jah128 0:b14dfd8816da 706 }
jah128 0:b14dfd8816da 707
jah128 0:b14dfd8816da 708 // Transmit the packet in one burst with no pausing
jah128 0:b14dfd8816da 709 for (int i = 0; i<8 ; i++) {
jah128 0:b14dfd8816da 710 _servo.putc(TxBuf[i]);
jah128 0:b14dfd8816da 711 }
jah128 0:b14dfd8816da 712
jah128 0:b14dfd8816da 713 // Wait for data to transmit
jah128 0:b14dfd8816da 714 wait_us(60); //was 60
jah128 0:b14dfd8816da 715
jah128 0:b14dfd8816da 716
jah128 0:b14dfd8816da 717 // Skip if the read was to the broadcast address
jah128 0:b14dfd8816da 718 if (ID != 0xFE) {
jah128 0:b14dfd8816da 719 int timedout = 0;
jah128 0:b14dfd8816da 720 int timeout_count = 0;
jah128 0:b14dfd8816da 721 while(!_servo.readable()) {
jah128 0:b14dfd8816da 722 timeout_count++;
jah128 0:b14dfd8816da 723 if(timeout_count % 10000 == 0) {
jah128 0:b14dfd8816da 724 timedout=1;
jah128 0:b14dfd8816da 725 break;
jah128 0:b14dfd8816da 726 }
jah128 0:b14dfd8816da 727 }
jah128 0:b14dfd8816da 728 if(timedout==1) {
jah128 0:b14dfd8816da 729 read_timeout_counter++;
jah128 0:b14dfd8816da 730 if(DEBUG)pc.printf(" Read timed out [%d of %d]\n",read_timeout_counter,READ_TIMEOUT_LIMIT);
jah128 0:b14dfd8816da 731 if(read_timeout_counter == READ_TIMEOUT_LIMIT){
jah128 0:b14dfd8816da 732 display.clear_display();
jah128 0:b14dfd8816da 733 display.set_position(0,0);
jah128 0:b14dfd8816da 734 display.write_string("SERVO ERROR");
jah128 0:b14dfd8816da 735 read_timeout_counter = 0;
jah128 0:b14dfd8816da 736 return 255;
jah128 0:b14dfd8816da 737 }
jah128 0:b14dfd8816da 738 return read(ID,start,bytes,data);
jah128 0:b14dfd8816da 739 } else {
jah128 0:b14dfd8816da 740 read_timeout_counter = 0;
jah128 0:b14dfd8816da 741 // Receive the Status packet 6+ number of bytes read
jah128 0:b14dfd8816da 742 for (int i=0; i<(6+bytes) ; i++) {
jah128 0:b14dfd8816da 743 Status[i] = _servo.getc();
jah128 0:b14dfd8816da 744 }
jah128 0:b14dfd8816da 745
jah128 0:b14dfd8816da 746 // Copy the data from Status into data for return
jah128 0:b14dfd8816da 747 for (int i=0; i < Status[3]-2 ; i++) {
jah128 0:b14dfd8816da 748 data[i] = Status[5+i];
jah128 0:b14dfd8816da 749 }
jah128 0:b14dfd8816da 750
jah128 0:b14dfd8816da 751 if (READ_DEBUG) {
jah128 0:b14dfd8816da 752 pc.printf("\nStatus Packet\n");
jah128 0:b14dfd8816da 753 pc.printf(" Header : 0x%x\n",Status[0]);
jah128 0:b14dfd8816da 754 pc.printf(" Header : 0x%x\n",Status[1]);
jah128 0:b14dfd8816da 755 pc.printf(" ID : 0x%x\n",Status[2]);
jah128 0:b14dfd8816da 756 pc.printf(" Length : 0x%x\n",Status[3]);
jah128 0:b14dfd8816da 757 pc.printf(" Error Code : 0x%x\n",Status[4]);
jah128 0:b14dfd8816da 758
jah128 0:b14dfd8816da 759 for (int i=0; i < Status[3]-2 ; i++) {
jah128 0:b14dfd8816da 760 pc.printf(" Data : 0x%x\n",Status[5+i]);
jah128 0:b14dfd8816da 761 }
jah128 0:b14dfd8816da 762
jah128 0:b14dfd8816da 763 pc.printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
jah128 0:b14dfd8816da 764 }
jah128 0:b14dfd8816da 765
jah128 0:b14dfd8816da 766 } // if (ID!=0xFE)
jah128 0:b14dfd8816da 767 wait_us(5);
jah128 0:b14dfd8816da 768 }
jah128 0:b14dfd8816da 769 return(Status[4]);
jah128 0:b14dfd8816da 770 }
jah128 0:b14dfd8816da 771
jah128 0:b14dfd8816da 772
jah128 0:b14dfd8816da 773 int Servo:: write(int ID, int start, int bytes, char* data, int flag)
jah128 0:b14dfd8816da 774 {
jah128 0:b14dfd8816da 775 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
jah128 0:b14dfd8816da 776
jah128 0:b14dfd8816da 777 char TxBuf[16];
jah128 0:b14dfd8816da 778 char sum = 0;
jah128 0:b14dfd8816da 779 char Status[6];
jah128 0:b14dfd8816da 780
jah128 0:b14dfd8816da 781 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 782 pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
jah128 0:b14dfd8816da 783 }
jah128 0:b14dfd8816da 784
jah128 0:b14dfd8816da 785 // Build the TxPacket first in RAM, then we'll send in one go
jah128 0:b14dfd8816da 786 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 787 pc.printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
jah128 0:b14dfd8816da 788 }
jah128 0:b14dfd8816da 789
jah128 0:b14dfd8816da 790 TxBuf[0] = 0xff;
jah128 0:b14dfd8816da 791 TxBuf[1] = 0xff;
jah128 0:b14dfd8816da 792
jah128 0:b14dfd8816da 793 // ID
jah128 0:b14dfd8816da 794 TxBuf[2] = ID;
jah128 0:b14dfd8816da 795 sum += TxBuf[2];
jah128 0:b14dfd8816da 796
jah128 0:b14dfd8816da 797 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 798 pc.printf(" ID : %d\n",TxBuf[2]);
jah128 0:b14dfd8816da 799 }
jah128 0:b14dfd8816da 800
jah128 0:b14dfd8816da 801 // packet Length
jah128 0:b14dfd8816da 802 TxBuf[3] = 3+bytes;
jah128 0:b14dfd8816da 803 sum += TxBuf[3];
jah128 0:b14dfd8816da 804
jah128 0:b14dfd8816da 805 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 806 pc.printf(" Length : %d\n",TxBuf[3]);
jah128 0:b14dfd8816da 807 }
jah128 0:b14dfd8816da 808
jah128 0:b14dfd8816da 809 // Instruction
jah128 0:b14dfd8816da 810 if (flag == 1) {
jah128 0:b14dfd8816da 811 TxBuf[4]=0x04;
jah128 0:b14dfd8816da 812 sum += TxBuf[4];
jah128 0:b14dfd8816da 813 } else {
jah128 0:b14dfd8816da 814 TxBuf[4]=0x03;
jah128 0:b14dfd8816da 815 sum += TxBuf[4];
jah128 0:b14dfd8816da 816 }
jah128 0:b14dfd8816da 817
jah128 0:b14dfd8816da 818 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 819 pc.printf(" Instruction : 0x%x\n",TxBuf[4]);
jah128 0:b14dfd8816da 820 }
jah128 0:b14dfd8816da 821
jah128 0:b14dfd8816da 822 // Start Address
jah128 0:b14dfd8816da 823 TxBuf[5] = start;
jah128 0:b14dfd8816da 824 sum += TxBuf[5];
jah128 0:b14dfd8816da 825 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 826 pc.printf(" Start : 0x%x\n",TxBuf[5]);
jah128 0:b14dfd8816da 827 }
jah128 0:b14dfd8816da 828
jah128 0:b14dfd8816da 829 // data
jah128 0:b14dfd8816da 830 for (char i=0; i<bytes ; i++) {
jah128 0:b14dfd8816da 831 TxBuf[6+i] = data[i];
jah128 0:b14dfd8816da 832 sum += TxBuf[6+i];
jah128 0:b14dfd8816da 833 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 834 pc.printf(" Data : 0x%x\n",TxBuf[6+i]);
jah128 0:b14dfd8816da 835 }
jah128 0:b14dfd8816da 836 }
jah128 0:b14dfd8816da 837
jah128 0:b14dfd8816da 838 // checksum
jah128 0:b14dfd8816da 839 TxBuf[6+bytes] = 0xFF - sum;
jah128 0:b14dfd8816da 840 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 841 pc.printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
jah128 0:b14dfd8816da 842 }
jah128 0:b14dfd8816da 843
jah128 0:b14dfd8816da 844 // Transmit the packet in one burst with no pausing
jah128 0:b14dfd8816da 845 for (int i = 0; i < (7 + bytes) ; i++) {
jah128 0:b14dfd8816da 846 _servo.putc(TxBuf[i]);
jah128 0:b14dfd8816da 847 }
jah128 0:b14dfd8816da 848
jah128 0:b14dfd8816da 849 // Wait for data to transmit
jah128 0:b14dfd8816da 850 wait_us(60);
jah128 0:b14dfd8816da 851
jah128 0:b14dfd8816da 852 // make sure we have a valid return
jah128 0:b14dfd8816da 853 Status[4]=0x00;
jah128 0:b14dfd8816da 854
jah128 0:b14dfd8816da 855 // we'll only get a reply if it was not broadcast
jah128 0:b14dfd8816da 856 if (ID!=0xFE) {
jah128 0:b14dfd8816da 857 int timedout = 0;
jah128 0:b14dfd8816da 858 int timeout_count = 0;
jah128 0:b14dfd8816da 859 while(!_servo.readable()) {
jah128 0:b14dfd8816da 860 timeout_count++;
jah128 0:b14dfd8816da 861 if(timeout_count % 10000 == 0) {
jah128 0:b14dfd8816da 862 timedout=1;
jah128 0:b14dfd8816da 863 break;
jah128 0:b14dfd8816da 864 }
jah128 0:b14dfd8816da 865 }
jah128 0:b14dfd8816da 866 if(timedout==1) {
jah128 0:b14dfd8816da 867 read_timeout_counter++;
jah128 0:b14dfd8816da 868 if(DEBUG)pc.printf(" Write ack. timed out [%d of %d]\n",read_timeout_counter,READ_TIMEOUT_LIMIT);
jah128 0:b14dfd8816da 869 if(read_timeout_counter == READ_TIMEOUT_LIMIT){
jah128 0:b14dfd8816da 870 display.clear_display();
jah128 0:b14dfd8816da 871 display.set_position(0,0);
jah128 0:b14dfd8816da 872 display.write_string("SERVO ERROR");
jah128 0:b14dfd8816da 873 read_timeout_counter = 0;
jah128 0:b14dfd8816da 874 return 255;
jah128 0:b14dfd8816da 875 }
jah128 0:b14dfd8816da 876 return write(ID,start,bytes,data,flag);
jah128 0:b14dfd8816da 877 } else {
jah128 0:b14dfd8816da 878 read_timeout_counter = 0;
jah128 0:b14dfd8816da 879 // response is always 6 bytes
jah128 0:b14dfd8816da 880 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
jah128 0:b14dfd8816da 881 for (int i=0; i < 6 ; i++) {
jah128 0:b14dfd8816da 882 Status[i] = _servo.getc();
jah128 0:b14dfd8816da 883 }
jah128 0:b14dfd8816da 884 }
jah128 0:b14dfd8816da 885 // Build the TxPacket first in RAM, then we'll send in one go
jah128 0:b14dfd8816da 886 if (WRITE_DEBUG) {
jah128 0:b14dfd8816da 887 pc.printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
jah128 0:b14dfd8816da 888 pc.printf(" ID : %d\n",Status[2]);
jah128 0:b14dfd8816da 889 pc.printf(" Length : %d\n",Status[3]);
jah128 0:b14dfd8816da 890 pc.printf(" Error : 0x%x\n",Status[4]);
jah128 0:b14dfd8816da 891 pc.printf(" Checksum : 0x%x\n",Status[5]);
jah128 0:b14dfd8816da 892 }
jah128 0:b14dfd8816da 893
jah128 0:b14dfd8816da 894
jah128 0:b14dfd8816da 895 }
jah128 0:b14dfd8816da 896
jah128 0:b14dfd8816da 897 return(Status[4]); // return error code
jah128 0:b14dfd8816da 898 }
jah128 0:b14dfd8816da 899
jah128 0:b14dfd8816da 900 //Set the baud rate for serial connection to something other than default(1000000)
jah128 0:b14dfd8816da 901 void Servo::SetInitBaud(int baud, int delaytime)
jah128 0:b14dfd8816da 902 {
jah128 0:b14dfd8816da 903 pc.printf("Setting serial baud rate to %d\n",baud);
jah128 0:b14dfd8816da 904 _servo.baud(baud);
jah128 0:b14dfd8816da 905 delay = delaytime;
jah128 0:b14dfd8816da 906 }
jah128 0:b14dfd8816da 907
jah128 0:b14dfd8816da 908 /* Additional copyright notice */
jah128 0:b14dfd8816da 909
jah128 0:b14dfd8816da 910 /*
jah128 0:b14dfd8816da 911 * Copyright 2017 University of York
jah128 0:b14dfd8816da 912 *
jah128 0:b14dfd8816da 913 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 0:b14dfd8816da 914 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 0:b14dfd8816da 915 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 0:b14dfd8816da 916 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 0:b14dfd8816da 917 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:b14dfd8816da 918 *
jah128 0:b14dfd8816da 919 */
jah128 0:b14dfd8816da 920
jah128 0:b14dfd8816da 921 /*
jah128 0:b14dfd8816da 922 * Copyright (c) 2010, Chris Styles (http://mbed.org)
jah128 0:b14dfd8816da 923 *
jah128 0:b14dfd8816da 924 * Permission is hereby granted, free of charge, to any person obtaining a copy
jah128 0:b14dfd8816da 925 * of this software and associated documentation files (the "Software"), to deal
jah128 0:b14dfd8816da 926 * in the Software without restriction, including without limitation the rights
jah128 0:b14dfd8816da 927 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jah128 0:b14dfd8816da 928 * copies of the Software, and to permit persons to whom the Software is
jah128 0:b14dfd8816da 929 * furnished to do so, subject to the following conditions:
jah128 0:b14dfd8816da 930 *
jah128 0:b14dfd8816da 931 * The above copyright notice and this permission notice shall be included in
jah128 0:b14dfd8816da 932 * all copies or substantial portions of the Software.
jah128 0:b14dfd8816da 933 *
jah128 0:b14dfd8816da 934 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jah128 0:b14dfd8816da 935 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jah128 0:b14dfd8816da 936 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jah128 0:b14dfd8816da 937 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jah128 0:b14dfd8816da 938 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jah128 0:b14dfd8816da 939 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jah128 0:b14dfd8816da 940 * THE SOFTWARE.
jah128 0:b14dfd8816da 941 */
jah128 0:b14dfd8816da 942 /*
jah128 0:b14dfd8816da 943 * Copyright (c) 2010, Chris Styles (http://mbed.org)
jah128 0:b14dfd8816da 944 *
jah128 0:b14dfd8816da 945 * Permission is hereby granted, free of charge, to any person obtaining a copy
jah128 0:b14dfd8816da 946 * of this software and associated documentation files (the "Software"), to deal
jah128 0:b14dfd8816da 947 * in the Software without restriction, including without limitation the rights
jah128 0:b14dfd8816da 948 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jah128 0:b14dfd8816da 949 * copies of the Software, and to permit persons to whom the Software is
jah128 0:b14dfd8816da 950 * furnished to do so, subject to the following conditions:
jah128 0:b14dfd8816da 951 *
jah128 0:b14dfd8816da 952 * The above copyright notice and this permission notice shall be included in
jah128 0:b14dfd8816da 953 * all copies or substantial portions of the Software.
jah128 0:b14dfd8816da 954 *
jah128 0:b14dfd8816da 955 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jah128 0:b14dfd8816da 956 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jah128 0:b14dfd8816da 957 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jah128 0:b14dfd8816da 958 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jah128 0:b14dfd8816da 959 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jah128 0:b14dfd8816da 960 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jah128 0:b14dfd8816da 961 * THE SOFTWARE.
jah128 0:b14dfd8816da 962 */