James Hilder / RobotArmController

Robot Arm Controller Library

Dependents:   PR_RobotArm lighthouse_2

servo.cpp@0:9f7b70e0186e, 2017-02-16 (annotated)

Committer:
jah128
Date:
Thu Feb 16 23:57:45 2017 +0000
Revision:
0:9f7b70e0186e
First commit

Who changed what in which revision?

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