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