From a79bb439b33d3163cbbd284f8594d12805c2e696 Mon Sep 17 00:00:00 2001 From: isaac hagedorn Date: Sat, 13 Nov 2021 07:26:17 -0500 Subject: [PATCH 1/6] ecu test working properly --- src/ti_comm/src/new_lib/amp_serial_jetson.h | 2 +- .../src/new_lib/amp_serial_jetson_ECU_TEST | Bin 31464 -> 31464 bytes .../new_lib/amp_serial_jetson_ECU_TEST.cpp | 15 ++++++++------- .../new_lib/amp_serial_jetson_SERIAL_TEST.cpp | 8 +++++--- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson.h b/src/ti_comm/src/new_lib/amp_serial_jetson.h index e9858fb..9e92c2e 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson.h +++ b/src/ti_comm/src/new_lib/amp_serial_jetson.h @@ -16,7 +16,7 @@ #define AMP_SERIAL_START_PKT 0x02 // start packet byte #define AMP_SERIAL_STOP_PKT 0x03 // end packet byte -#define AMP_SERIAL_CONFIG_BAUD 9600 // The Configuration Baud Rate +#define AMP_SERIAL_CONFIG_BAUD 4800 // The Configuration Baud Rate #define AMP_SERIAL_CONFIG_BITS 8 // The Configuration Bits per Byte #define AMP_SERIAL_CONFIG_PARY SP_PARITY_NONE // The Configuration Parity Bits #define AMP_SERIAL_CONFIG_STOP 1 // The Configuration Stop Bits diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST b/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST index 6bb730a34c39a45f12a988ca2ccd39fc94921419..cf6b3311b4f1c541d036f9f011477b4c37a51336 100755 GIT binary patch delta 3129 zcmZ8j3s98T6~6ahRzSdIS=N0aEV#guM8IeV4L%l?^3!P45ft+zxS$x+(x|D);F7?a ztSiOMHIqk8(@<^W6t$UcGPFr}6cCB2SsLRbwT-EgtP@8xpkmc-zx)6Hf^la!k9*Gf z&N=todw+u0Xz&`{9f&STA=0nJccw$zYLS&zi$fAx*#Y{`#FI1kHdv31hTQjk-O_ma z?e9)zwg1BZhVWYz?eWi^@MdIGr`bWozOH5dui~Pq(3!vnC#`VVDIvkr(S%fKhB}8e zTKDPyg+jxcc=f}vglMuQRkPy`@`N4EDh}DQ16MUW-iHICWOsW2zWp3z0(^q1C_-Lw z2jnofPidIm+W+Gn+rc5!++5j}muL`1jW4uwLHhaw4q{V&|E0<>g@Eo}c> zvg_Z3U2M1%r?Zh;zU@<2z`!BBg%`v%0eC*4BvP80z;j@Cc#c}s(x=X4A$<*f znbk}VJaXN1SB|Stc#f`XKgt2jQ`~Y-@h`wD4E!w$=u_`!I}HUBkD-xa&FZe;lk7`F z<^xf9Btqxf266enx3>*(mfAMLnYnG0GhN#q&SKjH3$q)OGTKKl2(Nm`h?Hnw1Nl6c zQP>wSc?)ktI>Xi*b63v-cJSF@z8J*f4s;nj0o7AD60G4cp2AVlTRQ439Pu3KD1&z! z3MIka@ZpDrV>-veixA%7;gD7~YHN+R;h3XV;R(Pty z?F!Ffsg_#TTMGzrEhMCVKDDUw|Aa_A{uW&cg>HiZw1!5IdP)j)vn%=6*@o$<^i_ZJ^z>+Yh<)(TLYmBOK9mh=&tH(~ ze$9Z-2_9ExyaQWE$dE({p?jxfPXs$?#CKx_^W|GK2Oq7QC`wDFGODRp#+CitrbHgbMZ&_F2NVb`txTt!;6b5-_bf_ zg_Z}ckkrmPQv#ytjTn4qRlaj|rjNOGl)XPEH8PDF;s`m;&djl_Ortzr@Rd!K{L|=a zNnK{E;^8)0i~NFby={gOYPzC6lU_||29+DCAsN3zVYGs7N7cMuqGvywYqnGrPt^$6dQdIhgU&h?U#YpqSHykQJ1Gn0c2(42Tecy;xY9Y~4m zAW@k1_F|N2(&h||rI0`d!rk%u%fARWASXN#ZjNNU#G6$Kdj3BOGR!hKB<%HL8EzrN z)Y1saOWrUv`4O+nCUj$CvSMqVq6q*6H4Pckq*?AVjOhG;;HN^f$EYhXID;VTlHfg@awN&7x3OXgd)Z?_hhdi`Wr~w3S2C@H* zm)9apgO|zVz^aRo$}#*J;M>ahq1Y!_qyreJvq{Huzl5mreO5>82YH!(;XejKwo4dy zLmVE1_~0Ih5ATA=oeYR>!4`>6?s=BwuxNYl@jl`&b&Lws<$q*hJdINsqkd4W{f+8* zu-X3gi#%rk$de_M_OPeR^{RLh05_Pz|KuwDNK8c%NV(rvzDc0V7+up$TbXZ74(()J zpy%1uHBZqMY`L?U&SKv4t}S`d0YwWix-~C4yc?~o b09tvH>{;%Aachx^Hn9F}d8mv&|K0xq9_sht delta 3113 zcmZ8j4Nz3q6~6Z^tKi@6vf#42xGb{3+E$2RtWo?~qJ&4sAFQC5XoZTX5&Vh5R6Dp) z#Q3t~=;kJk8pbw}X=-DtcFhc_8HI?VjbligjKP|yDVlY}_ya14cKe<8c8T%L>^g^ttsTceCf8vZsuU&to>*S&bz%J=eI({TDe!%X5(T8jvCbu?w0vCqHaOu@ladp@nuY)rdJQu@Qx#3n* zm%&u`_5VVlE`w2@J%lkshAJAib|Fs0;9}Uop&5{9*m@WS@TyZKcN=Wa5Xi&9K<^#M z*tWVh%}oRBw-~7Zl1$EvV}?BYI}&5ZsiI)zV>OBGkyus2X^is%aeE}aLb!Dqikl-T zU+)3__3yD1$;wsPr+>%kWXI$&B}`UYgAU*wdlU+Vz~4eT(4Qc^4#8ACbP@gMsy==n z^m1)yG=6Sbxh(sJdiT?o5)Jl!m6M5W&{!kbcQrP_H}sF-6xk^&=YtC=L~YA6vM+u% z>3;$_PxT;}BF&fBUu!HO&eB*Q&Tu-#L{l1#t<3ZSPZS@R#`06*ipjypy%Fo+vPi<( zE+%8OiG{I_jF%q%SW~ASN%(SaQ?0ew-wC0oOwNi*btAoX`rt9KKXyS}j>F07nySob z&t$MXQaS{3gw zw>wx(xHJ%7QI9XTTRC-HhP9eSa1U&#YZPQ_0fuXPGn zz?J>db(hXm`OchqIdgNAP)<=!;q>jPd(BQ(ByS&0bWXlm|DWr5vHkPjD3yQJC(6L&olPw6mHjLAA(;zeucL~p+}&U##vAs z5_jRm97CbgKtIIr%0MmmL!m98(Ks*-pl)1+7SI=aL!l1P^Po{UKo@YBT%d2@G*1J4 z1X>0vyYc?n0_JVd2GB1+TR_+1z10EwHE0wL!z7WPIF1*HO^KOU4=1MUf4Bn=@lB$g zzan}QlX#A>j!5D@F&^szF<)KRV_hkBWBp8gjP-A#9qS9C7wb-8btLgm#CWVX#C%7R zq<<(jI2`;nu^U@w#m87pfp&+*ATD#GxIMZA-;>!H2PJ-5@G*tbw!z{HK3r6eN#@%E zJI18O@CNbGgsD7E^iIgY>dt;Xeal-Gd_VABebMMa8yUN$u8i^e5gCI(4kX}Xk}v$( zcEf>}up#-EC5#+E3UM^sI-unodJqs7vhCg$O)YEcmb_>(X4caf7e6%*u#&&Px)$Mh zt0om%SCMpDlZve)2=>fXHAso|y;w;9v|vy2FJMv8gqPGqDRuk6QqB#r9yMDh2FWJj z$LnX{dJyPIFYTsm2-$Z+?F;KCC+bG1 zYZ2`e?Q@ek`Ah!d6xBYN&sW6)S2?+-@HL1p`CoQrB|vnA#TBW`8BgU}K@}`*9}?rG zd?T`^eDBAJUrn;vOT$>pSlUHuj+dV>YX)nDcaq)f?GN2e9r1?o3bk09!m*aR)XX>c zgQcevSR2nYGDg`|57)Y*-g31$Co#&AYIEB9L#QBak?JV_l7B@C&;tHDQXEU$OvOr4 zJpCy_gba%o;boxH!|VGHA*CovwbB}fB9BsDDbfiHlsdCZte6X)@?o?iE0?b% zwI&viTv=85qga?)U0Q*bN|BbP&!ur?Njm_x9$+2f;$*ueOVZ9s1#^EudCZiyVUSs3 z*c7{WrbMYqevIRHbb3mql5irG;rGmYaG{6Sj~a&b|BTxVs}ov!DQ{x#5H*luS05eA z#o>I)Tz^3vUM5w*9XEryKc)2El}5wLVD(_v%h(bSdU$;W@Lk1pDE3Kq^#HcjxYXm> zpd#x0XKR@ILrU}S(sveo|5c19ArAIK9C-@jqbDGq84id~CD%ZFz1zhokKNSrl=aC# zk*7!E8v`?^8u<_=MM=?jbQ_}Y>YfX%n5NhQUB4;f{D>$miPsrf6R5=+`BSS^qpdm? z%L)Sil2sD-2)=j+uNVHsnf#Dw#@a6KEMCO(#q6>jJVRV5%LJflNuG6A_}oknBhkOh zST+ndZ_N@54+v$+4!%xU%QLw_kkOD%Z8jbD1n zL@#=Jy2ExIV$`x*wzcsdbypYGUd4b31-J6q<<-1e>|dVAD+BGz4-S^@4iT?cWyBn@ wX)Z@ZbJe(*rm)l`c=foLNH^+54y1xa&9We{sX9-`H;Xgt$MMev-|+qa0i=`hPyhe` diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST.cpp b/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST.cpp index 4df6a4c..a96f796 100644 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST.cpp +++ b/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST.cpp @@ -79,11 +79,11 @@ int main(int argc, char** argv) { amp_serial_pkt_control_t c_pkt; // Control Data Packet - for(float i = AMP_MIN_ANG; i < AMP_MAX_ANG; i++) { - for(float j = AMP_MIN_VEL; j < AMP_MAX_VEL; j++) { + for(float i = AMP_MIN_VEL; i < AMP_MAX_VEL; i++) { + for(float j = AMP_MIN_ANG; j < AMP_MAX_ANG; j++) { // Create Control Packet - c_pkt.v_speed = float_to_int(AMP_MAX_VEL, AMP_MIN_VEL, j); //msg->linear.x; - c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, i); //msg->angular.z; + c_pkt.v_speed = float_to_int(AMP_MAX_VEL, AMP_MIN_VEL, i); //msg->linear.x; + c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, j); //msg->angular.z; // Create Full Serial Packet s_pkt.id = AMP_SERIAL_CONTROL; @@ -96,14 +96,15 @@ int main(int argc, char** argv) { #ifdef DEBUG fprintf(fptr1, "Sending Packet...\n"); #endif - sleep(0.00005); + sleep(0.5); amp_serial_jetson_tx_pkt(&s_pkt, &size); #ifdef DEBUG fprintf(fptr1, "Receiving Packet...\n"); #endif - sleep(0.00005); + sleep(0.5); //amp_serial_jetson_rx_pkt(&s_pkt, size); } + sleep(1); } } @@ -289,7 +290,7 @@ amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) { void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) { uint8_t s_pos = 1; // Current Position of Data Array - uint8_t c_crc = 0; // Used to calculate the current CRC + uint8_t c_crc = 2; // Used to calculate the current CRC int i; // Start Byte 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 110d8a6..4e98b2c 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 @@ -51,8 +51,10 @@ FILE * fptr3 = fopen("debug_rx.txt", "w"); int main(int argc, char** argv) { 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}; - float angle[10] = {1.0, 2.0, 3.0, 5.5, 8.7, 20.0, 4.56, 3.3, 9.87, 2.343}; + //float speed[10] = {1.0, 2.0, 3.0, 5.5, 8.7, 10.0, 4.56, 3.3, 9.87, 2.343}; + //float angle[10] = {1.0, 2.0, 3.0, 5.5, 8.7, 20.0, 4.56, 3.3, 9.87, 2.343}; + + //rptr = fopen(|"data.txt", 'r'); // Global Configuration Parameters config.baudrate = AMP_SERIAL_CONFIG_BAUD; @@ -87,7 +89,7 @@ int main(int argc, char** argv) { s_data[6] = '\n'; sp_nonblocking_write(port, (const void *)s_data, s_pos * sizeof(uint8_t)); printf("Sending Packet: %s\n", s_data);*/ - + while(i < 10) { // Declare & Initialize Local Variables amp_serial_pkt_t s_pkt; // Full Serial Packet 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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();