int main() { SerialPort sp; cout << sp.Connect("COM3") << endl; cout << sp.Setup() << endl; // Create a message RobotCommand msg; UnitStatus stat; SetStatus(stat, 0, 0, 0, 1, 1); // Write it to device while (1) { char c = getch(); switch (c) { case 'w': SetMessage(msg, CMD_FORWARD, 50); // Forward with 50/255 speed break; case 'a': SetMessage(msg, CMD_LEFT, 10); // Left with 10/255 of max break; case 's': SetMessage(msg, CMD_BACKWARD, 20); // Backward with 20/255 break; case 'd': SetMessage(msg, CMD_RIGHT, 5); break; case 'q': SetMessage(msg, CMD_STOP, 0); // Stop with no braking case 'e': SetMessage(msg, CMD_STOP, 255); // Stop with full braking break; default: cout << "## Command not recognized ##" << endl << endl; continue; } cout << "Sending message: " << endl << "------------------------" << endl << msg << endl << endl; sp.WriteMessage(msg); cout << "Fetching robot status:" << endl << "------------------------" << endl; SetMessage(msg, CMD_GET_CURRENT_STATE); sp.WriteMessage(msg); int res = sp.ReadMessage(stat); if (res) cout << "Failed to read message: " << res << endl; else cout << stat << endl; } sp.Close(); cout << endl << "End" << endl; return 0; }