![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Code to set up an AX-12 or MX-28 (and to scan for unidentified servos) ready for use in the robot arm.
servo.cpp@0:03f84c95f73b, 2017-02-16 (annotated)
- Committer:
- jah128
- Date:
- Thu Feb 16 20:45:33 2017 +0000
- Revision:
- 0:03f84c95f73b
Initial commit
Who changed what in which revision?
User | Revision | Line number | New 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 | */ |