├── .gitignore ├── Arduino-UBTECH.cpp ├── Arduino-UBTECH.h ├── build.sh ├── examples └── UBTECH-Query │ └── UBTECH-Query.ino ├── images ├── devices.jpg └── servo.jpg ├── keywords.txt └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | .vscode 3 | *.zip 4 | Arduino-UBTECH.ino 5 | -------------------------------------------------------------------------------- /Arduino-UBTECH.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino-UBTECH.h" 2 | 3 | /* 4 | This library enables you to control the UBTECH servos, motors, and sensor that come 5 | with popular JIMU robotic sets directly from an Arduino. You can chain any number of 6 | these devices together on a single serial bus which uses only two data pins joined 7 | together on the Arduino board. The library will handle the serial communication 8 | protocol via a half-duplex SoftwareSerial interface. No other hardware or circuitry 9 | is needed to handle the serial communication. 10 | 11 | The UBTECH devices work with 3V logic from the main control unit of JIMU robot kits 12 | so I did all my testing using a bi-directional logic level shifter to convert the 13 | 5V logic from my Arduino Uno to the 3V logic for the UBTECH devices. I have not yet 14 | tested wether the UBTECH/JIMU servo/motor/sensors are 5V tolerant. 15 | 16 | The UBTECH devices will work powered from the Arduino's Vin pin when the Arduino (Uno) is 17 | connected to a USB power source. This will only provide 5V to the bus and the servos and 18 | motors will not have full torque/speed but is good enough for testing. A better approach 19 | is to power the Arduino from a DC 7-8V source or power the UBTECH devices directly from 20 | an external 7-8V power source and connect common grounds with the Arduino. 21 | 22 | Connect one of the Arduino ground pins to the ground wire of the first UBTECH device. 23 | Connect the Vin pin on the Arduino to the power wire of the first UBTECH device. 24 | 25 | Join together two digital pins on the Arduino (I used pins 2 and 3 on Arduino Uno) 26 | and connect the joined wire to the 5V side of bi-directional logic shifter. Connect 27 | the 3V side of the logic shifter to the data wire of the first UBTECH device. Connect 28 | 5V and 3.3V pins on Arduino to the respective sides on the logic shifter along with grounds. 29 | 30 | Make sure the data pins you use on the Arduino support change interrupts for SoftwareSerial. 31 | Refer to: https://www.arduino.cc/en/Reference/SoftwareSerial 32 | */ 33 | 34 | 35 | 36 | UbtechBus::UbtechBus(uint8_t rxPin, uint8_t txPin) 37 | : _serial(rxPin, txPin), _timeout(50) 38 | { 39 | } 40 | 41 | 42 | void UbtechBus::begin() 43 | { 44 | _serial.setTimeout(_timeout); 45 | _serial.begin(115200); 46 | } 47 | 48 | uint16_t UbtechBus::setTimeout(uint16_t timeout) 49 | { 50 | uint16_t prevTimeout = _timeout; 51 | _serial.setTimeout(_timeout = timeout); 52 | return prevTimeout; 53 | } 54 | 55 | void UbtechBus::queryDevices(DeviceCallback callback) 56 | { 57 | queryServo(1, 32, callback); 58 | queryMotor(1, 8, callback); 59 | queryInfrared(1, 8, callback); 60 | queryUltrasonic(1, 8, callback); 61 | queryEyeLight(1, 8, callback); 62 | } 63 | 64 | 65 | bool UbtechBus::queryServo(uint8_t id) 66 | { 67 | uint8_t buffer[] = { 0xFC, 0xCF, id, 0x01/*cmd*/, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED }; 68 | buffer[8] = checksum(buffer+2, 6); 69 | if (!sendAndReceive(buffer, 10, 10)) 70 | return false; 71 | return (buffer[0] == 0xFC && buffer[1] == 0xCF && buffer[2] == id && buffer[3] == 0xAA); 72 | } 73 | 74 | void UbtechBus::queryServo(uint8_t minId, uint8_t maxId, DeviceCallback callback) { 75 | queryDevice(DeviceType::SERVO_H04, [](uint8_t id, UbtechBus* pBus) { 76 | return pBus->queryServo(id); 77 | }, minId, maxId, callback, 10); 78 | } 79 | 80 | bool UbtechBus::setServoId(uint8_t oldId, uint8_t newId) 81 | { 82 | uint8_t buffer[] = { 0xFA, 0xAF, oldId, 0xCD/*cmd*/, 0x00, newId, 0x00, 0x00, 0x00, 0xED }; 83 | buffer[8] = checksum(buffer+2, 6); 84 | if (!sendAndReceive(buffer, 10, 10)) 85 | return false; 86 | return (buffer[0] == 0xFA && buffer[1] == 0xAF && buffer[2] == oldId && buffer[3] == 0xAA); 87 | } 88 | 89 | int8_t UbtechBus::getServoPosition(uint8_t id) 90 | { 91 | uint8_t buffer[] = { 0xFA, 0xAF, id, 0x03/*cmd*/, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED }; 92 | buffer[8] = checksum(buffer+2, 6); 93 | return sendAndReceive(buffer, 10, 10) ? buffer[7] - 120 : 0x80; 94 | } 95 | 96 | bool UbtechBus::setServoPosition(uint8_t id, int8_t position, uint16_t time) 97 | { 98 | position = (position < -118 ? 2 : position > 118 ? 138 : position + 120); 99 | time = (time < 300 ? 300 : time > 5000 ? 5000 : time) / 20; 100 | uint8_t buffer[] = { 0xFA, 0xAF, id, 0x01/*cmd*/, (uint8_t)position, (uint8_t)time, 0x00, 0x00, 0x00, 0xED }; 101 | buffer[8] = checksum(buffer+2, 6); 102 | return sendAndReceive(buffer, 10, 1) && buffer[0] - 0xAA == id; 103 | } 104 | 105 | bool UbtechBus::setServoTurn(uint8_t id, uint16_t speed, bool clockwise) 106 | { 107 | speed = speed == 1 ? 128 : speed == 2 ? 234 : speed == 3 ? 340 : speed == 4 ? 526 : speed == 5 ? 658 : 0; 108 | uint8_t dir = (speed == 0 ? 0xFF : clockwise ? 0xFD : 0xFE); 109 | uint8_t speedHi = (speed & 0xFF00) >> 8; 110 | uint8_t speedLo = speed & 0x00FF; 111 | uint8_t buffer[] = { 0xFA, 0xAF, id, 0x01/*cmd*/, dir, 0x00, speedHi, speedLo, 0x00, 0xED }; 112 | buffer[8] = checksum(buffer+2, 6); 113 | return sendAndReceive(buffer, 10, 1) && buffer[0] - 0xAA == id; 114 | } 115 | 116 | bool UbtechBus::stopServo(uint8_t id) { 117 | return setServoTurn(id, 0); 118 | } 119 | 120 | 121 | bool UbtechBus::queryMotor(uint8_t id) 122 | { 123 | uint8_t buffer[] = { 0xFB, 0x03, 0x06, 0x05/*cmd*/, id, 0x00, 0x09, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 124 | buffer[9] = crc8_itu(buffer+1, 8); 125 | return sendAndReceive(buffer, 10, 17) && buffer[4] == id; 126 | } 127 | 128 | void UbtechBus::queryMotor(uint8_t minId, uint8_t maxId, DeviceCallback callback) { 129 | queryDevice(DeviceType::MOTOR, [](uint8_t id, UbtechBus* pBus) { 130 | return pBus->queryMotor(id); 131 | }, minId, maxId, callback, 10); 132 | } 133 | 134 | bool UbtechBus::setMotorId(uint8_t oldId, uint8_t newId) 135 | { 136 | uint8_t buffer[] = { 0xFB, 0x03, 0x08/*len*/, 0x06/*cmd*/, oldId, 0x00, 0x03, 0x00, 0x01, 0x00, newId, 0x00 }; 137 | buffer[11] = crc8_itu(buffer+1, 10); 138 | if (!sendAndReceive(buffer, 12, 7)) 139 | return false; 140 | return (buffer[0] == 0xAC && buffer[4] == oldId); 141 | } 142 | 143 | bool UbtechBus::setMotorTurn(uint8_t id, int16_t speed, uint16_t time) 144 | { 145 | uint8_t speedHi = speed < 0 ? 0xFF : 0x00; 146 | uint8_t speedLo = speed < -140 ? 116 : speed > 140 ? 140 : speed < 0 ? 256 + speed : speed; 147 | uint8_t timeHi = (time & 0xFF00) >> 8; 148 | uint8_t timeLo = time & 0x00FF; 149 | uint8_t buffer[] = { 0xFB, 0x03, 0x0C/*len*/, 0x06/*cmd*/, id, 0x00, 0x04, 0x00, 0x03, speedHi, speedLo, timeHi, timeLo, 0x00, 0x01, 0x00 }; 150 | buffer[15] = crc8_itu(buffer+1, 14); 151 | return sendAndReceive(buffer, 16, 7) && buffer[4] == id; 152 | } 153 | 154 | bool UbtechBus::stopMotor(uint8_t id) 155 | { 156 | uint8_t buffer[] = { 0xFB, 0x03, 0x08/*len*/, 0x06/*cmd*/, id, 0x00, 0x06, 0x00, 0x01, 0x00, 0x00, 0x00 }; 157 | buffer[11] = crc8_itu(buffer+1, 10); 158 | return sendAndReceive(buffer, 12, 7) && buffer[4] == id; 159 | } 160 | 161 | int16_t UbtechBus::getMotorSpeed(uint8_t id) 162 | { 163 | uint8_t buffer[] = { 0xFB, 0x03, 0x06/*len*/, 0x05/*cmd*/, id, 0x00, 0x07, 0x00, 0x01, 0x00 }; 164 | buffer[9] = crc8_itu(buffer+1, 8); 165 | if (!sendAndReceive(buffer, 10, 9) || buffer[4] != id) 166 | return 0xFF; 167 | return buffer[6] == 0xFF ? -(256 - buffer[7]) : buffer[7]; 168 | } 169 | 170 | 171 | bool UbtechBus::queryInfrared(uint8_t id) 172 | { 173 | uint8_t buffer[] = { 0xF8, 0x8F, 0x06, 0x07/*cmd*/, id, 0x00, 0xED, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 174 | buffer[5] = checksum(buffer+2, 3); 175 | if (!sendAndReceive(buffer, 7, 15)) 176 | return false; 177 | return buffer[0] == 0xF8 && buffer[1] == 0x8F && buffer[4] - 0xAA == id; 178 | } 179 | 180 | void UbtechBus::queryInfrared(uint8_t minId, uint8_t maxId, DeviceCallback callback) { 181 | queryDevice(DeviceType::INFRARED, [](uint8_t id, UbtechBus* pBus) { 182 | return pBus->queryInfrared(id); 183 | }, minId, maxId, callback, 10); 184 | } 185 | 186 | bool UbtechBus::setInfraredId(uint8_t oldId, uint8_t newId) 187 | { 188 | uint8_t buffer[] = { 0xF8, 0x8F, 0x07/*len*/, 0x06/*cmd*/, oldId, newId, 0x00, 0xED }; 189 | buffer[6] = checksum(buffer+2, 4); 190 | if (!sendAndReceive(buffer, 8, 7)) 191 | return false; 192 | return ((buffer[0] == 0xF8) && (buffer[1] == 0x8F) && (buffer[4] - 0xAA == oldId)); 193 | } 194 | 195 | bool UbtechBus::enableInfrared(uint8_t id, bool enable) 196 | { 197 | uint8_t buffer[] = { 0xF8, 0x8F, 0x06/*len*/, (uint8_t)(enable ? 0x02 : 0x03)/*cmd*/, id, 0x00, 0xED }; 198 | buffer[5] = checksum(buffer+2, 3); 199 | if (!sendAndReceive(buffer, 7, 7)) 200 | return false; 201 | return ((buffer[0] == 0xF8) && (buffer[1] == 0x8F) && (buffer[4] - 0xAA == id)); 202 | } 203 | 204 | uint16_t UbtechBus::getInfraredDistance(uint8_t id) 205 | { 206 | uint8_t buffer[] = { 0xF8, 0x8F, 0x06/*len*/, 0x04/*cmd*/, id, 0x00, 0xED, 0x00, 0x00 }; 207 | buffer[5] = checksum(buffer+2, 3); 208 | if (!sendAndReceive(buffer, 7, 9)) 209 | return 0x0000; 210 | if (buffer[0] != 0xF8 || buffer[1] != 0x8F || buffer[4] - 0xAA != id) 211 | return 0x0000; 212 | return ((buffer[5] << 8) + buffer[6]); 213 | } 214 | 215 | 216 | bool UbtechBus::queryUltrasonic(uint8_t id) 217 | { 218 | uint8_t buffer[] = { 0xF5, 0x5F, 0x06/*len*/, 0x07/*cmd*/, id, 0x00, 0xED, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 219 | buffer[5] = checksum(buffer+2, 3); 220 | if (!sendAndReceive(buffer, 7, 15)) 221 | return false; 222 | return (buffer[0] == 0xF5 && buffer[1] == 0x5F && buffer[4] - 0xAA == id); 223 | } 224 | 225 | void UbtechBus::queryUltrasonic(uint8_t minId, uint8_t maxId, DeviceCallback callback) { 226 | queryDevice(DeviceType::ULTRASONIC, [](uint8_t id, UbtechBus* pBus) { 227 | return pBus->queryUltrasonic(id); 228 | }, minId, maxId, callback, 10); 229 | } 230 | 231 | bool UbtechBus::setUltrasonicId(uint8_t oldId, uint8_t newId) 232 | { 233 | uint8_t buffer[] = { 0xF5, 0x5F, 0x07/*len*/, 0x06/*cmd*/, oldId, newId, 0x00, 0xED }; 234 | buffer[6] = checksum(buffer+2, 4); 235 | if (!sendAndReceive(buffer, 8, 7)) 236 | return false; 237 | return ((buffer[0] == 0xF5) && (buffer[1] == 0x5F) && (buffer[4] - 0xAA == oldId)); 238 | } 239 | 240 | bool UbtechBus::enableUltrasonic(uint8_t id, bool enable) 241 | { 242 | uint8_t buffer[] = { 0xF5, 0x5F, 0x06/*len*/, (uint8_t)(enable ? 0x02 : 0x03)/*cmd*/, id, 0x00, 0xED }; 243 | buffer[5] = checksum(buffer+2, 3); 244 | if (!sendAndReceive(buffer, 7, 7)) 245 | return false; 246 | return (buffer[0] == 0xF5 && buffer[1] == 0x5F && buffer[4] - 0xAA == id); 247 | } 248 | 249 | uint16_t UbtechBus::getUltrasonicDistance(uint8_t id) 250 | { 251 | uint8_t buffer[] = { 0xF5, 0x5F, 0x06/*len*/, 0x04/*cmd*/, id, 0x00, 0xED, 0x00, 0x00 }; 252 | buffer[5] = checksum(buffer+2, 3); 253 | if (!sendAndReceive(buffer, 7, 9)) 254 | return 0x0000; 255 | if (buffer[0] != 0xF5 || buffer[1] != 0x5F || buffer[4] - 0xAA != id) 256 | return 0x0000; 257 | return ((buffer[5] << 8) + buffer[6]); 258 | } 259 | 260 | bool UbtechBus::setUltrasonicLed(uint8_t id, Color color) 261 | { 262 | bool off = (color.red == 0x00 && color.green == 0x00 && color.blue == 0x00); 263 | uint8_t buffer[] = { 0xF5, 0x5F, 0x0D/*len*/, 0x08/*cmd*/, id, color.red, color.green, color.blue, (uint8_t)(off ? 0x00 : 0x01), 0x00, (uint8_t)(off ? 0x00 : 0xFF), (uint8_t)(off ? 0x00 : 0xFF), 0x00, 0xED }; 264 | buffer[12] = checksum(buffer+2, 10); 265 | if (!sendAndReceive(buffer, 14, 7)) 266 | return false; 267 | return (buffer[0] == 0xF5 && buffer[1] == 0x5F && buffer[4] - 0xAA == id); 268 | } 269 | 270 | bool UbtechBus::setUltrasonicLedOff(uint8_t id) { 271 | return setUltrasonicLed(id, Color(0x00, 0x00, 0x00)); 272 | } 273 | 274 | 275 | bool UbtechBus::queryEyeLight(uint8_t id) 276 | { 277 | uint8_t buffer[] = { 0xF4, 0x4F, 0x06/*len*/, 0x07/*cmd*/, id, 0x00, 0xED, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 278 | buffer[5] = checksum(buffer+2, 3); 279 | if (!sendAndReceive(buffer, 7, 15)) 280 | return false; 281 | return (buffer[0] == 0xF4 && buffer[1] == 0x4F && buffer[4] - 0xAA == id); 282 | } 283 | 284 | void UbtechBus::queryEyeLight(uint8_t minId, uint8_t maxId, DeviceCallback callback) { 285 | queryDevice(DeviceType::EYE_LIGHT, [](uint8_t id, UbtechBus* pBus) { 286 | return pBus->queryEyeLight(id); 287 | }, minId, maxId, callback, 10); 288 | } 289 | 290 | bool UbtechBus::setEyeLightId(uint8_t oldId, uint8_t newId) 291 | { 292 | uint8_t buffer[] = { 0xF4, 0x4F, 0x07/*len*/, 0x06/*cmd*/, oldId, newId, 0x00, 0xED }; 293 | buffer[6] = checksum(buffer+2, 4); 294 | if (!sendAndReceive(buffer, 8, 7)) 295 | return false; 296 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == oldId)); 297 | } 298 | 299 | bool UbtechBus::enableEyeLight(uint8_t id, bool enable) 300 | { 301 | uint8_t buffer[] = { 0xF4, 0x4F, 0x06/*len*/, (uint8_t)(enable ? 0x02 : 0x03)/*cmd*/, id, 0x00, 0xED }; 302 | buffer[5] = checksum(buffer+2, 3); 303 | if (!sendAndReceive(buffer, 7, 7)) 304 | return false; 305 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == id)); 306 | } 307 | 308 | bool UbtechBus::setEyeLightColor(uint8_t id, Color color, LED led, float nSeconds) 309 | { 310 | uint8_t duration = nSeconds < 0.1 || nSeconds >= 25.5 ? 255 : (uint8_t)(nSeconds * 10.0); 311 | uint8_t buffer[] = { 0xF4, 0x4F, 0x0C/*len*/, 0x0B/*cmd*/, id, duration, 0x01, led, color.red, color.green, color.blue, 0x00, 0xED }; 312 | buffer[11] = checksum(buffer+2, 9); 313 | if (!sendAndReceive(buffer, 13, 7)) 314 | return false; 315 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == id)); 316 | } 317 | 318 | bool UbtechBus::setEyeLightColor(uint8_t id, Color c1, Color c2, Color c3, Color c4, Color c5, Color c6, Color c7, Color c8, float nSeconds) 319 | { 320 | uint8_t duration = nSeconds < 0.1 || nSeconds >= 25.5 ? 255 : (uint8_t)(nSeconds * 10.0); 321 | uint8_t idx = 7, buffer[] = { 0xF4, 0x4F, 0x28/*len*/, 0x0B/*cmd*/, id, duration, 0x08, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0x00, 0xED }; 322 | buffer[idx++] = 0x01; buffer[idx++] = c1.red; buffer[idx++] = c1.green; buffer[idx++] = c1.blue; 323 | buffer[idx++] = 0x02; buffer[idx++] = c2.red; buffer[idx++] = c2.green; buffer[idx++] = c2.blue; 324 | buffer[idx++] = 0x04; buffer[idx++] = c3.red; buffer[idx++] = c3.green; buffer[idx++] = c3.blue; 325 | buffer[idx++] = 0x08; buffer[idx++] = c4.red; buffer[idx++] = c4.green; buffer[idx++] = c4.blue; 326 | buffer[idx++] = 0x10; buffer[idx++] = c5.red; buffer[idx++] = c5.green; buffer[idx++] = c5.blue; 327 | buffer[idx++] = 0x20; buffer[idx++] = c6.red; buffer[idx++] = c6.green; buffer[idx++] = c6.blue; 328 | buffer[idx++] = 0x40; buffer[idx++] = c7.red; buffer[idx++] = c7.green; buffer[idx++] = c7.blue; 329 | buffer[idx++] = 0x80; buffer[idx++] = c8.red; buffer[idx++] = c8.green; buffer[idx++] = c8.blue; 330 | buffer[39] = checksum(buffer+2, 37); 331 | if (!sendAndReceive(buffer, 41, 7)) 332 | return false; 333 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == id)); 334 | } 335 | 336 | bool UbtechBus::setEyeLightScene(uint8_t id, Scene scene, uint8_t nTimes) 337 | { 338 | uint8_t buffer[] = { 0xF4, 0x4F, 0x0C/*len*/, 0x0A/*cmd*/, id, (uint8_t)(scene + 12), 0x00, nTimes, 0x00, 0x00, 0x00, 0x00, 0xED }; 339 | buffer[11] = checksum(buffer+2, 9); 340 | if (!sendAndReceive(buffer, 13, 7)) 341 | return false; 342 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == id)); 343 | } 344 | 345 | bool UbtechBus::setEyeLightFace(uint8_t id, Face face, Color color, uint8_t nTimes) 346 | { 347 | uint8_t buffer[] = { 0xF4, 0x4F, 0x0C/*len*/, 0x0A/*cmd*/, id, face, 0x00, nTimes, color.red, color.green, color.blue, 0x00, 0xED }; 348 | buffer[11] = checksum(buffer+2, 9); 349 | if (!sendAndReceive(buffer, 13, 7)) 350 | return false; 351 | return ((buffer[0] == 0xF4) && (buffer[1] == 0x4F) && (buffer[4] - 0xAA == id)); 352 | } 353 | 354 | 355 | //Private Members 356 | 357 | bool UbtechBus::sendAndReceive(uint8_t *buffer, uint16_t sendSize, uint16_t recvSize, bool printRecv) 358 | { 359 | _serial.stopListening(); 360 | _serial.enableTx(); 361 | //prevent framing errors 362 | _serial.write((uint8_t)0); 363 | if (_serial.write(buffer, sendSize) != sendSize) 364 | return false; 365 | if (recvSize > 0) { 366 | _serial.listen(); 367 | _serial.disableTx(); 368 | int recvCnt = _serial.readBytes(buffer, recvSize); 369 | if (printRecv) printBuffer(buffer, recvCnt); 370 | return recvCnt == recvSize; 371 | } 372 | return true; 373 | } 374 | 375 | void UbtechBus::queryDevice(DeviceType type, QueryFunction query, uint8_t minId, uint8_t maxId, DeviceCallback callback, uint16_t timeout) 376 | { 377 | uint16_t prevTimeout = setTimeout(timeout); 378 | for (uint8_t i = minId; i <= maxId; i++) { 379 | if (query(i, this)) { 380 | callback(type, i); 381 | } else if (query(i, this)) { 382 | callback(type, i); 383 | } 384 | } 385 | setTimeout(prevTimeout); 386 | } 387 | 388 | uint8_t UbtechBus::checksum(uint8_t *buf, uint8_t len) { 389 | uint32_t i, sum = 0; 390 | for (i = 0; i < len; i++) { 391 | sum += buf[i]; 392 | } 393 | return (uint8_t)(sum % 256); 394 | } 395 | 396 | uint8_t UbtechBus::_crc8(unsigned short data) { 397 | for (int i = 0; i < 8; i++) { 398 | if (data & 0x8000) { 399 | data = data ^ (0x1070U << 3); 400 | } 401 | data = data << 1; 402 | } 403 | return (uint8_t)(data >> 8); 404 | } 405 | 406 | uint32_t UbtechBus::crc8(uint32_t crc, const uint8_t *vptr, uint32_t len) { 407 | for (int i = 0; i < len; i++) { 408 | crc = _crc8((crc ^ vptr[i]) << 8); 409 | } 410 | return crc; 411 | } 412 | 413 | uint32_t UbtechBus::crc8_itu(const uint8_t *pBuf, uint32_t len) { 414 | uint32_t crc; 415 | crc = crc8(0, pBuf, len); 416 | crc ^= 0x55; 417 | return crc; 418 | } 419 | 420 | void UbtechBus::printBuffer(uint8_t* buffer, int size) { 421 | for (int i = 0; i < size; i++) { 422 | if (i > 0) Serial.print(" "); 423 | if (buffer[i] < 16) Serial.print("0"); 424 | Serial.print(buffer[i], HEX); 425 | } 426 | Serial.println(";"); 427 | } 428 | -------------------------------------------------------------------------------- /Arduino-UBTECH.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | /* 5 | This library enables you to control the UBTECH servos, motors, and sensor that come 6 | with popular JIMU robotic sets directly from an Arduino. You can chain any number of 7 | these devices together on a single serial bus which uses only two data pins joined 8 | together on the Arduino board. The library will handle the serial communication 9 | protocol via a half-duplex SoftwareSerial interface. No other hardware or circuitry 10 | is needed to handle the serial communication. 11 | 12 | The UBTECH devices work with 3V logic from the main control unit of JIMU robot kits 13 | so I did all my testing using a bi-directional logic level shifter to convert the 14 | 5V logic from my Arduino Uno to the 3V logic for the UBTECH devices. I have not yet 15 | tested wether the UBTECH/JIMU servo/motor/sensors are 5V tolerant. 16 | 17 | The UBTECH devices will work powered from the Arduino's Vin pin when the Arduino (Uno) is 18 | connected to a USB power source. This will only provide 5V to the bus and the servos and 19 | motors will not have full torque/speed but is good enough for testing. A better approach 20 | is to power the Arduino from a DC 7-8V source or power the UBTECH devices directly from 21 | an external 7-8V power source and connect common grounds with the Arduino. 22 | 23 | Connect one of the Arduino ground pins to the ground wire of the first UBTECH device. 24 | Connect the Vin pin on the Arduino to the power wire of the first UBTECH device. 25 | 26 | Join together two digital pins on the Arduino (I used pins 2 and 3 on Arduino Uno) 27 | and connect the joined wire to the 5V side of bi-directional logic shifter. Connect 28 | the 3V side of the logic shifter to the data wire of the first UBTECH device. Connect 29 | 5V and 3.3V pins on Arduino to the respective sides on the logic shifter along with grounds. 30 | 31 | Make sure the data pins you use on the Arduino support change interrupts for SoftwareSerial. 32 | Refer to: https://www.arduino.cc/en/Reference/SoftwareSerial 33 | */ 34 | 35 | 36 | #ifndef __UbtechBus__ 37 | #define __UbtechBus__ 38 | 39 | class UbtechBus 40 | { 41 | public: 42 | 43 | enum DeviceType : uint8_t { 44 | UNKNOWN = 0, 45 | SERVO_H04 = 1, 46 | MOTOR = 2, 47 | INFRARED = 3, 48 | ULTRASONIC = 4, 49 | TOUCH = 5, 50 | COLOR = 6, 51 | EYE_LIGHT = 7, 52 | }; 53 | 54 | typedef struct _Color { 55 | uint8_t red; 56 | uint8_t green; 57 | uint8_t blue; 58 | _Color() : red(0), green(0), blue(0) {} 59 | _Color(uint8_t red, uint8_t green, uint8_t blue) 60 | : red(red), green(green), blue(blue) {} 61 | } Color; 62 | 63 | enum LED : uint8_t { 64 | L1 = 1, 65 | L2 = 2, 66 | L3 = 4, 67 | L4 = 8, 68 | L5 = 16, 69 | L6 = 32, 70 | L7 = 64, 71 | L8 = 128, 72 | ALL = 255 73 | }; 74 | 75 | enum Scene : uint8_t { 76 | COLORED = 0, 77 | DISCO = 1, 78 | PRIMARY = 2, 79 | STACKING = 3 80 | }; 81 | 82 | enum Face : uint8_t { 83 | BLINK = 0, 84 | SHY = 1, 85 | TEARS = 2, 86 | TEARS_FLASH = 3, 87 | CRY = 4, 88 | DIZZY = 5, 89 | HAPPY = 6, 90 | SURPRISED = 7, 91 | BREATH = 8, 92 | FLASH = 9, 93 | FAN = 10, 94 | WIPERS = 11 95 | }; 96 | 97 | typedef void (*DeviceCallback)(DeviceType type, uint8_t id); 98 | 99 | // constructor 100 | UbtechBus(uint8_t rxPin, uint8_t txPin); 101 | 102 | void begin(); 103 | uint16_t setTimeout(uint16_t timeout); 104 | void queryDevices(DeviceCallback callback); 105 | 106 | bool queryServo(uint8_t id); 107 | void queryServo(uint8_t minId, uint8_t maxId, DeviceCallback callback); 108 | bool setServoId(uint8_t oldId, uint8_t newId); 109 | int8_t getServoPosition(uint8_t id); 110 | bool setServoPosition(uint8_t id, int8_t position, uint16_t time = 400); 111 | bool setServoTurn(uint8_t id, uint16_t speed, bool clockwise = true); 112 | bool stopServo(uint8_t id); 113 | 114 | bool queryMotor(uint8_t id); 115 | void queryMotor(uint8_t minId, uint8_t maxId, DeviceCallback callback); 116 | bool setMotorId(uint8_t oldId, uint8_t newId); 117 | bool setMotorTurn(uint8_t id, int16_t speed, uint16_t time = 0xFFFF); 118 | bool stopMotor(uint8_t id); 119 | int16_t getMotorSpeed(uint8_t id); 120 | 121 | bool queryInfrared(uint8_t id); 122 | void queryInfrared(uint8_t minId, uint8_t maxId, DeviceCallback callback); 123 | bool setInfraredId(uint8_t oldId, uint8_t newId); 124 | bool enableInfrared(uint8_t id, bool enable); 125 | uint16_t getInfraredDistance(uint8_t id); 126 | 127 | bool queryUltrasonic(uint8_t id); 128 | void queryUltrasonic(uint8_t minId, uint8_t maxId, DeviceCallback callback); 129 | bool setUltrasonicId(uint8_t oldId, uint8_t newId); 130 | bool enableUltrasonic(uint8_t id, bool enable); 131 | uint16_t getUltrasonicDistance(uint8_t id); 132 | bool setUltrasonicLed(uint8_t id, Color color); 133 | bool setUltrasonicLedOff(uint8_t id); 134 | 135 | bool queryEyeLight(uint8_t id); 136 | void queryEyeLight(uint8_t minId, uint8_t maxId, DeviceCallback callback); 137 | bool setEyeLightId(uint8_t oldId, uint8_t newId); 138 | bool enableEyeLight(uint8_t id, bool enable); 139 | bool setEyeLightColor(uint8_t id, Color color, LED led = LED::ALL, float nSeconds = -1); 140 | bool setEyeLightColor(uint8_t id, Color c1, Color c2, Color c3, Color c4, Color c5, Color c6, Color c7, Color c8, float nSeconds = -1); 141 | bool setEyeLightScene(uint8_t id, Scene scene, uint8_t nTimes = 1); 142 | bool setEyeLightFace(uint8_t id, Face face, Color color, uint8_t nTimes = 1); 143 | 144 | private: 145 | 146 | typedef bool (*QueryFunction)(uint8_t id, UbtechBus* pBus); 147 | 148 | bool sendAndReceive(uint8_t *buffer, uint16_t sendSize, uint16_t recvSize, bool printRecv = false); 149 | void queryDevice(DeviceType type, QueryFunction query, uint8_t minId, uint8_t maxId, DeviceCallback callback, uint16_t timeout); 150 | 151 | static uint8_t checksum(uint8_t *buf, uint8_t len); 152 | static uint8_t _crc8(unsigned short data); 153 | static uint32_t crc8(uint32_t crc, const uint8_t *vptr, uint32_t len); 154 | static uint32_t crc8_itu(const uint8_t *pBuf, uint32_t len); 155 | static void printBuffer(uint8_t* buffer, int size); 156 | 157 | class HalfDuplexSoftwareSerial : public SoftwareSerial 158 | { 159 | public: 160 | HalfDuplexSoftwareSerial(uint8_t rxPin, uint8_t txPin): 161 | SoftwareSerial(rxPin, txPin), _txPin(txPin) { 162 | } 163 | void enableTx() { pinMode(_txPin, OUTPUT); } 164 | void disableTx() { pinMode(_txPin, INPUT); } 165 | private: 166 | uint8_t _txPin; 167 | }; 168 | 169 | uint16_t _timeout; 170 | HalfDuplexSoftwareSerial _serial; 171 | }; 172 | 173 | static UbtechBus::LED operator |(const UbtechBus::LED& L1, const UbtechBus::LED& L2) { 174 | return static_cast(static_cast(L1) | static_cast(L2)); 175 | } 176 | 177 | #endif 178 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rm ./Arduino-UBTECH.zip 4 | zip -r Arduino-UBTECH.zip . -x "*.git*" -x "*.vscode*" -x "*.DS_Store*" -x "build.sh" -x "Arduino-UBTECH.ino" 5 | -------------------------------------------------------------------------------- /examples/UBTECH-Query/UBTECH-Query.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MAX_DEVICES 32 4 | 5 | typedef struct { 6 | UbtechBus::DeviceType type; 7 | uint8_t id; 8 | } DeviceInfo; 9 | 10 | //UBTECH serial bus instance 11 | //Connected pins 2 and 3 (Arduino Uno) to data line 12 | // of first UBTECH device (servo/motor/sensor) 13 | UbtechBus _ubtBus(2, 3); 14 | 15 | //nNumber of devices connected 16 | uint16_t deviceCount = 0; 17 | 18 | //Store info for each connected device 19 | DeviceInfo deviceInfo[MAX_DEVICES]; 20 | 21 | //Callback to store each discovered device 22 | void deviceCallback(UbtechBus::DeviceType type, uint8_t id) { 23 | if (deviceCount < MAX_DEVICES) { 24 | deviceInfo[deviceCount++] = { type, id }; 25 | } 26 | } 27 | 28 | //Print info for each connected device 29 | void printDevices() 30 | { 31 | if (deviceCount < 1) { 32 | Serial.println("No Devices Found."); 33 | } else { 34 | char msg[30]; const char* types[] = { 35 | "UNKNOWN", "SERVO_H04", "MOTOR", "INFRARED", "ULTRASONIC", "TOUCH", "COLOR", "EYE_LIGHT" }; 36 | for (int i = 0; i < deviceCount; i++) { 37 | DeviceInfo& d = deviceInfo[i]; 38 | sprintf(msg, "Device: %s, Id: %d", types[d.type], d.id); 39 | Serial.println(msg); 40 | } 41 | } 42 | } 43 | 44 | void setup() 45 | { 46 | Serial.begin(9600); 47 | while (!Serial) { }; 48 | 49 | _ubtBus.begin(); 50 | 51 | Serial.println("Querying Devices.."); 52 | 53 | //query all connected devices on the bus 54 | _ubtBus.queryDevices(deviceCallback); 55 | 56 | printDevices(); 57 | } 58 | 59 | void loop() 60 | { 61 | } 62 | -------------------------------------------------------------------------------- /images/devices.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clopresti/Arduino-UBTECH/ad9b67b6dee215d085115f7944e6f41776be0e4d/images/devices.jpg -------------------------------------------------------------------------------- /images/servo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clopresti/Arduino-UBTECH/ad9b67b6dee215d085115f7944e6f41776be0e4d/images/servo.jpg -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | # class (KEYWORD1) 2 | 3 | UbtechBus KEYWORD1 4 | 5 | 6 | # function and method (KEYWORD2) 7 | 8 | begin KEYWORD2 9 | setTimeout KEYWORD2 10 | queryDevices KEYWORD2 11 | 12 | queryServo KEYWORD2 13 | setServoId KEYWORD2 14 | getServoPosition KEYWORD2 15 | setServoPosition KEYWORD2 16 | setServoTurn KEYWORD2 17 | stopServo KEYWORD2 18 | 19 | queryMotor KEYWORD2 20 | setMotorId KEYWORD2 21 | setMotorTurn KEYWORD2 22 | stopMotor KEYWORD2 23 | getMotorSpeed KEYWORD2 24 | 25 | queryInfrared KEYWORD2 26 | setInfraredId KEYWORD2 27 | enableInfrared KEYWORD2 28 | getInfraredDistance KEYWORD2 29 | 30 | queryUltrasonic KEYWORD2 31 | setUltrasonicId KEYWORD2 32 | enableUltrasonic KEYWORD2 33 | getUltrasonicDistance KEYWORD2 34 | setUltrasonicLed KEYWORD2 35 | setUltrasonicLedOff KEYWORD2 36 | 37 | queryEyeLight KEYWORD2 38 | setEyeLightId KEYWORD2 39 | enableEyeLight KEYWORD2 40 | setEyeLightColor KEYWORD2 41 | setEyeLightScene KEYWORD2 42 | setEyeLightFace KEYWORD2 43 | 44 | DeviceType KEYWORD2 45 | Color KEYWORD2 46 | LED KEYWORD2 47 | Scene KEYWORD2 48 | Face KEYWORD2 49 | 50 | 51 | # constant (LITERAL1) 52 | 53 | SERVO_H04 LITERAL1 54 | MOTOR LITERAL1 55 | INFRARED LITERAL1 56 | ULTRASONIC LITERAL1 57 | TOUCH LITERAL1 58 | COLOR LITERAL1 59 | EYE_LIGHT LITERAL1 60 | 61 | L1 LITERAL1 62 | L2 LITERAL1 63 | L3 LITERAL1 64 | L4 LITERAL1 65 | L5 LITERAL1 66 | L6 LITERAL1 67 | L7 LITERAL1 68 | L8 LITERAL1 69 | ALL LITERAL1 70 | 71 | COLORED LITERAL1 72 | DISCO LITERAL1 73 | PRIMARY LITERAL1 74 | STACKING LITERAL1 75 | 76 | BLINK LITERAL1 77 | SHY LITERAL1 78 | TEARS LITERAL1 79 | TEARS_FLASH LITERAL1 80 | CRY LITERAL1 81 | DIZZY LITERAL1 82 | HAPPY LITERAL1 83 | SURPRISED LITERAL1 84 | BREATH LITERAL1 85 | FLASH LITERAL1 86 | FAN LITERAL1 87 | WIPERS LITERAL1 88 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ## Arduino UBTECH/JIMU Library 2 | 3 | This library enables you to control the UBTECH servos, motors, and sensor that come 4 | with popular JIMU robotic sets directly from an Arduino. You can chain any number of 5 | these devices together on a single serial bus which uses only two data pins joined 6 | together on the Arduino board. The library will handle the serial communication 7 | protocol via a half-duplex SoftwareSerial interface. No other hardware or circuitry 8 | is needed to handle the serial communication. 9 | 10 | ![XL-320 Servo](/images/devices.jpg) 11 | 12 | ## Library Installation 13 | To use this library install the UBTECH-Arduino.zip archive from github releases to the 14 | Arduino library folder. 15 | ``` 16 | Arduino -> Sketch -> Include Library -> Add Zip Library 17 | ``` 18 | 19 | ## Configuration 20 | 21 | ![XL-320 Servo](/images/servo.jpg) 22 | 23 | The UBTECH devices work with 3V logic from the main control unit of JIMU robot kits 24 | so I did all my testing using a bi-directional logic level shifter to convert the 25 | 5V logic from my Arduino Uno to the 3V logic for the UBTECH devices. I have not yet 26 | tested wether the UBTECH/JIMU servo/motor/sensors are 5V tolerant. 27 | 28 | The UBTECH devices will work powered from the Arduino's Vin pin when the Arduino (Uno) is 29 | connected to a USB power source. This will only provide 5V to the bus and the servos and 30 | motors will not have full torque/speed but is good enough for testing. A better approach 31 | is to power the Arduino from a DC 7-8V source or power the UBTECH devices directly from 32 | an external 7-8V power source and connect common grounds with the Arduino. 33 | 34 | Connect one of the Arduino ground pins to the ground wire of the first UBTECH device. 35 | Connect the Vin pin on the Arduino to the power wire of the first UBTECH device. 36 | 37 | Join together two digital pins on the Arduino (I used pins 2 and 3 on Arduino Uno) 38 | and connect the joined wire to the 5V side of bi-directional logic shifter. Connect 39 | the 3V side of the logic shifter to the data wire of the first UBTECH device. Connect 40 | 5V and 3.3V pins on Arduino to the respective sides on the logic shifter along with grounds. 41 | 42 | Make sure the data pins you use on the Arduino support change interrupts for SoftwareSerial. Refer to: https://www.arduino.cc/en/Reference/SoftwareSerial 43 | 44 | 45 | ## Example Code 46 | ```c 47 | #include 48 | 49 | #define MAX_DEVICES 32 50 | 51 | typedef struct { 52 | UbtechBus::DeviceType type; 53 | uint8_t id; 54 | } DeviceInfo; 55 | 56 | //UBTECH serial bus instance 57 | //Connected pins 2 and 3 (Arduino Uno) to data line 58 | // of first UBTECH device (servo/motor/sensor) 59 | UbtechBus _ubtBus(2, 3); 60 | 61 | //nNumber of devices connected 62 | uint16_t deviceCount = 0; 63 | 64 | //Store info for each connected device 65 | DeviceInfo deviceInfo[MAX_DEVICES]; 66 | 67 | //Callback to store each discovered device 68 | void deviceCallback(UbtechBus::DeviceType type, uint8_t id) { 69 | if (deviceCount < MAX_DEVICES) { 70 | deviceInfo[deviceCount++] = { type, id }; 71 | } 72 | } 73 | 74 | //Print info for each connected device 75 | void printDevices() 76 | { 77 | if (deviceCount < 1) { 78 | Serial.println("No Devices Found."); 79 | } else { 80 | char msg[30]; const char* types[] = { 81 | "UNKNOWN", "SERVO_H04", "MOTOR", "INFRARED", "ULTRASONIC", "TOUCH", "COLOR", "EYE_LIGHT" }; 82 | for (int i = 0; i < deviceCount; i++) { 83 | DeviceInfo& d = deviceInfo[i]; 84 | sprintf(msg, "Device: %s, Id: %d", types[d.type], d.id); 85 | Serial.println(msg); 86 | } 87 | } 88 | } 89 | 90 | void setup() 91 | { 92 | Serial.begin(9600); 93 | while (!Serial) { }; 94 | 95 | _ubtBus.begin(); 96 | 97 | Serial.println("Querying Devices.."); 98 | 99 | //query all connected devices on the bus 100 | _ubtBus.queryDevices(deviceCallback); 101 | 102 | printDevices(); 103 | } 104 | 105 | void loop() 106 | { 107 | } 108 | ``` 109 | --------------------------------------------------------------------------------