From 2b55e730db8db5e8db8767574069d8cb9a0d7a0b Mon Sep 17 00:00:00 2001 From: aaaakshat <33050725+aaaakshat@users.noreply.github.com> Date: Sat, 20 Nov 2021 19:43:14 -0500 Subject: [PATCH 1/5] Gitignore vim swap files --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index 5a98412..f306b5b 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,10 @@ # Editor files .vscode/** +# Vim swapfiles +*.swo +*.swp + # Misc. /src/slam_mode_goal/.idea/** /src/sbg_to_imu/.idea/** From 5df3e51c97cc0706a610ab649fbd825ae12cb26d Mon Sep 17 00:00:00 2001 From: aaaakshat <33050725+aaaakshat@users.noreply.github.com> Date: Sat, 20 Nov 2021 21:04:56 -0500 Subject: [PATCH 2/5] Add read command from file functionality and delay flag to debugger --- .../new_lib/amp_serial_jetson_SERIAL_TEST.cpp | 75 +++++++++++++++++-- 1 file changed, 70 insertions(+), 5 deletions(-) diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp index 4e98b2c..a71a1d5 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp +++ b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp @@ -35,6 +35,9 @@ struct sp_port_config config; // Configuration of th // Global Variables Concerning State of Control amp_control_state_t amp_control_state = AMP_CONTROL_AUTONOMOUS; +// Initialise argparse declaration. +void argparse(int &argc, char *argv[], string &filename, float &delay); + //Debug Intialization #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) FILE * fptr1 = fopen("debug.txt", "w"); @@ -49,6 +52,36 @@ FILE * fptr3 = fopen("debug_rx.txt", "w"); #endif int main(int argc, char** argv) { + // Command pair structured as: + queue > commands; + // File to read commands from + string filename = "debug.txt"; + // Default delay value of 0.05 + float delay = 0.05; + string currSpeed; + string currAngle; + + argparse(argc, argv, filename, delay); + + // Handle invalid/corrupt debug file + ifstream fin(filename); + if (fin.fail()) { + cout << "Error: File " << filename << " doesn't exist or is incorrectly formatted" << endl; + return 1; + } + + // Read commands from file + string currSpeed; + string currAngle; + while (fin) { + getline(fin, currSpeed, ' '); + getline(fin, currAngle); + if (currAngle.empty() || currSpeed.empty()) break; + float speed = stof(currSpeed); + float angle = stof(currAngle); + commands.push(make_pair(speed, angle)); + } + int size = 0; int i = 0; //float speed[10] = {1.0, 2.0, 3.0, 5.5, 8.7, 10.0, 4.56, 3.3, 9.87, 2.343}; @@ -90,14 +123,19 @@ int main(int argc, char** argv) { sp_nonblocking_write(port, (const void *)s_data, s_pos * sizeof(uint8_t)); printf("Sending Packet: %s\n", s_data);*/ - while(i < 10) { + while(!commands.empty()) { + // Get speed and angle for current command + float speed, angle; + tie(speed, angle) = commands.front(); + commands.pop(); + // Declare & Initialize Local Variables amp_serial_pkt_t s_pkt; // Full Serial Packet amp_serial_pkt_control_t c_pkt; // Control Data Packet // Create Control Packet - c_pkt.v_speed = float_to_int(AMP_MAX_VEL, AMP_MIN_VEL, speed[i]); //msg->linear.x; - c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, angle[i]); //msg->angular.z; + c_pkt.v_speed = float_to_int(AMP_MAX_VEL, AMP_MIN_VEL, speed); //msg->linear.x; + c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, angle); //msg->angular.z; // Create Full Serial Packet s_pkt.id = AMP_SERIAL_CONTROL; @@ -110,12 +148,12 @@ int main(int argc, char** argv) { #ifdef DEBUG fprintf(fptr1, "Sending Packet...\n"); #endif - sleep(0.01); + sleep(delay); amp_serial_jetson_tx_pkt(&s_pkt, &size); #ifdef DEBUG fprintf(fptr1, "Receiving Packet...\n"); #endif - sleep(0.01); + sleep(delay); //amp_serial_jetson_rx_pkt(&s_pkt, size); i++; } @@ -731,3 +769,30 @@ void amp_serial_jetson_enable_default() { amp_serial_jetson_tx_pkt(&t_pkt, &size); } + +/* + * FUNCTION: + * + * void argparse(int &argc, char *argv[], string &filename, float &delay) { + * + * Parse input args + */ +void argparse(int &argc, char *argv[], string &filename, float &delay) { + vector args(argv + 1, argv + argc); + for (auto i = args.begin(); i != args.end(); ++i) { + if (*i == "-h" || *i == "--help") { + cout << "Help: " << endl; + cout << "-f or --file : Read commands from specified filename." << endl; + cout << " File must have speed and velocity separated by a space, each " + "command being on a new line" << endl; + cout << " If no filename is specified will try to read from 'debug.txt'" << endl; + cout << "-d or --delay : Set tx and rx delay" << endl; + cout << "-h or --help : Display this help menu" << endl; + } else if (*i == "-f" || *i == "--file") { + filename = *++i; + } else if (*i == "-d" || *i == "--delay") { + delay = stof(*++i); + } + } +} + From 2fb37c65e848e73460c4d5553734233734e20c77 Mon Sep 17 00:00:00 2001 From: aaaakshat <33050725+aaaakshat@users.noreply.github.com> Date: Sat, 20 Nov 2021 21:15:42 -0500 Subject: [PATCH 3/5] Improve help description and add exit after help menu --- src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp index a71a1d5..08bf0d5 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp +++ b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp @@ -786,8 +786,12 @@ void argparse(int &argc, char *argv[], string &filename, float &delay) { cout << " File must have speed and velocity separated by a space, each " "command being on a new line" << endl; cout << " If no filename is specified will try to read from 'debug.txt'" << endl; + cout << " Example file: \n" + " 5.4 2.4\n" + " 2.5 3.9" << endl; cout << "-d or --delay : Set tx and rx delay" << endl; cout << "-h or --help : Display this help menu" << endl; + exit(0); } else if (*i == "-f" || *i == "--file") { filename = *++i; } else if (*i == "-d" || *i == "--delay") { From dc38afc910a36c7f38a076b6d9410a8dffbbaf27 Mon Sep 17 00:00:00 2001 From: Akshat <33050725+aaaakshat@users.noreply.github.com> Date: Tue, 30 Nov 2021 21:22:10 -0500 Subject: [PATCH 4/5] Simplify speed and angle read function Co-authored-by: Zach Ghera --- .../src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp index 08bf0d5..e1f1c91 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp +++ b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp @@ -71,14 +71,9 @@ int main(int argc, char** argv) { } // Read commands from file - string currSpeed; - string currAngle; - while (fin) { - getline(fin, currSpeed, ' '); - getline(fin, currAngle); - if (currAngle.empty() || currSpeed.empty()) break; - float speed = stof(currSpeed); - float angle = stof(currAngle); + float speed; + float angle; + while (fin >> speed && fin >> angle) { commands.push(make_pair(speed, angle)); } From ceee7afe413330879268669d763e974d83a1b754 Mon Sep 17 00:00:00 2001 From: aaaakshat <33050725+aaaakshat@users.noreply.github.com> Date: Wed, 1 Dec 2021 15:58:31 -0500 Subject: [PATCH 5/5] Refactor to be more readable --- .../src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp index e1f1c91..e0dac89 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp +++ b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp @@ -1,8 +1,6 @@ /* - * amp_serial_jetson.cpp - * - * Created on: Apr 18, 2019 - * Author: David Pimley + * amp_serial_jetson_SERIAL_TEST.cpp + * Used for testing/debugging serial communications between the jetson and the hardware */ // Standard Defines @@ -54,12 +52,12 @@ FILE * fptr3 = fopen("debug_rx.txt", "w"); int main(int argc, char** argv) { // Command pair structured as: queue > commands; - // File to read commands from + // File to read debug commands from string filename = "debug.txt"; // Default delay value of 0.05 float delay = 0.05; - string currSpeed; - string currAngle; + float speed; + float angle; argparse(argc, argv, filename, delay); @@ -71,8 +69,6 @@ int main(int argc, char** argv) { } // Read commands from file - float speed; - float angle; while (fin >> speed && fin >> angle) { commands.push(make_pair(speed, angle)); } @@ -120,7 +116,6 @@ int main(int argc, char** argv) { while(!commands.empty()) { // Get speed and angle for current command - float speed, angle; tie(speed, angle) = commands.front(); commands.pop();