int main(int argc, char** argv) { int connectError; CM730* board = new CM730(); cout << "Testing behaviour of CM730 Module" << endl << endl; cout << "Connect to CM730" << endl; connectError = board->connect(); cout << "Resumed with Error: " << connectError << endl << endl; cout << "Ping to CM730" << endl; int pingErr; int pingSucc; pingSucc = board->ping(200); cout << "Resumed with Err: " << pingSucc << endl << endl; cout << "Write on CM730 LED's" << endl; int writeErr1; writeErr1 = board->writeByte(CM730::ID_CM, CM730::P_LED_PANNEL, 0x03); cout << "Resumed with Error " << writeErr1 << endl << endl; cout << "Read LED status from CM730" << endl; int readErr1, readValue = 0; readErr1 = board->readByte(CM730::ID_CM, CM730::P_BAUD_RATE, &readValue); cout << "Resumed with Error: " << readErr1 << " and Value: " << readValue << endl << endl; sleep(1); cout << "Write LED's via SyncWrite" << endl; int syncWriteErr; uint8_t param[2]; param[0] = CM730::ID_CM; param[1] = 0x02; syncWriteErr = board->syncWrite(CM730::P_LED_PANNEL, 2, 1, param); cout << "Resumed with Error: " << syncWriteErr << endl << endl; board->writeByte(CM730::ID_CM, CM730::P_DXL_POWER, 1); cout << endl << endl; sleep(1); std::vector<int> servos; servos.push_back(1); servos.push_back(2); board->setQuerySet(servos); cout << "And now, try the BulkRead" << endl; int bulkReadErr; vector<BRData> brdata(2); BRBoard brboard; bulkReadErr = board->bulkRead(&brdata, &brboard); cout << "Resumed with Error: " << bulkReadErr << endl << endl; cout << "BulkRead Board Infos" << endl; cout << "Voltage: " << dec <<(int)brboard.voltage << endl; cout << "LED Pannel: " << (int)brboard.ledPannel << endl; cout << "AccelX: " << brboard.accelX << endl; cout << "AccelY: " << brboard.accelY << endl; cout << "AccelZ: " << brboard.accelZ << endl << endl; cout << "------------------------------------------------------" << endl; cout << "Ping servo 1" << endl; pingErr = board->ping(1); cout << "Resumed with Error: " << pingErr << endl << endl; cout << "Read Servo 1" << endl; int readErrS1, readValueS1; readErrS1 = board->readWord(1, DynamixelMX::P_PRESENT_POSITION_L, &readValueS1); cout << "Resumed with Error: " << readErrS1 << " and Value: " << dec << readValueS1 << endl << endl; board->writeByte(1, DynamixelMX::P_TORQUE_ENABLE, 1); if (pingSucc == 0 && writeErr1 == 0 && readErr1 == 0 && syncWriteErr == 0 && bulkReadErr == 0 && pingErr == 0) cout << "hooray!" << endl; return 0; }