James Hilder / Mbed 2 deprecated PR_QuickServoDemo

Very simple program to demonstrate MX-28 servo; turns from -90 to +90 degrees and back.

Dependencies:   mbed

Fork of PR_RobotArmSetup by James Hilder

servo.cpp@1:311dbbc3aaa3, 2017-02-16 (annotated)

Committer:
jah128
Date:
Thu Feb 16 21:21:09 2017 +0000
Revision:
1:311dbbc3aaa3
Parent:
0:03f84c95f73b 
Initial commit

Who changed what in which revision?

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