diff --git a/src/kart_2dnav/launch/sim_autocross_track.launch b/src/kart_2dnav/launch/sim_autocross_track.launch index 2949727..77f89e7 100644 --- a/src/kart_2dnav/launch/sim_autocross_track.launch +++ b/src/kart_2dnav/launch/sim_autocross_track.launch @@ -13,13 +13,13 @@ - - + Remap the scan topic to top/scan to match naming used for output + scan topic from pointcloud_to_laserscan - + --> - + diff --git a/src/kart_2dnav/launch/sim_autocross_track_only_goal_setting.launch b/src/kart_2dnav/launch/sim_autocross_track_only_goal_setting.launch index b9e4133..a5d1c70 100644 --- a/src/kart_2dnav/launch/sim_autocross_track_only_goal_setting.launch +++ b/src/kart_2dnav/launch/sim_autocross_track_only_goal_setting.launch @@ -25,7 +25,7 @@ - + @@ -74,6 +74,6 @@ - + diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libserialport.h.swp b/src/ti_comm/src/Ignore/libserialport-0.1.0/.libserialport.h.swp deleted file mode 100644 index a0182cc..0000000 Binary files a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libserialport.h.swp and /dev/null differ diff --git a/src/ti_comm/src/Makefile b/src/ti_comm/src/Makefile index fbdd019..e7fb32b 100644 --- a/src/ti_comm/src/Makefile +++ b/src/ti_comm/src/Makefile @@ -1,20 +1,22 @@ -# Compiler Option -CC = g++ +WARNING = -Wall -Wshadow --pedantic +ERROR = -Wvla -Werror +GCC = g++ -std=c++11 -lstdc++ -lm +PKG_CONFIG = pkg-config +CFLAGS = -g $(WARNING) $(ERROR) $(shell $(PKG_CONFIG) --cflags libserialport) +LIBS = $(shell $(PKG_CONFIG) --libs libserialport) -# Compiler Flags -CFLAGS = -g -Wall -TESTFLAGS = -DDEBUG=1 +TESTFLAGS = -DDEBUG=1 -DDEBUG_TX=1 -DDEBUG_RX=1 +SOURCES = $(wildcard *.cpp) -default: all +BINARIES = $(SOURCES:.cpp=) -all: amp_serial_jetson.o libserialport-0.1.0/serialport.o libserialport-0.1.0/linux_termios.o - $(CC) $(CFLAGS) $(TESTFLAGS) -o amp_serial_jetson amp_serial_jetson.o libserialport-0.1.0/serialport.o libserialport-0.1.0/linux_termios.o -ludev +%: %.cpp + $(GCC) $(CFLAGS) $< $(LIBS) -o $@ -amp_serial_jetson.o: amp_serial_jetson.cpp amp_serial_jetson.h util.h amp_err.h - $(CC) $(CFLAGS) $(TESTFLAGS) -c amp_serial_jetson.cpp +debug_%: %.cpp + $(GCC) $(CFLAGS) $(TESTFLAGS) $< $(LIBS) -o $@ -sample_publisher: sample_publisher.o - $(CC) $(CFLAGS) -o sample_publisher sample_publisher.o +all: $(BINARIES) -sample_publisher.o: sample_publisher.cpp - $(CC) $(CFLAGS) -c sample_publisher.cpp +clean: + rm $(BINARIES) diff --git a/src/ti_comm/src/a.out b/src/ti_comm/src/a.out deleted file mode 100755 index 86f41df..0000000 Binary files a/src/ti_comm/src/a.out and /dev/null differ diff --git a/src/ti_comm/src/amp_serial_jetson.cpp b/src/ti_comm/src/amp_serial_jetson.cpp index e8c8e45..5f1a646 100644 --- a/src/ti_comm/src/amp_serial_jetson.cpp +++ b/src/ti_comm/src/amp_serial_jetson.cpp @@ -17,7 +17,7 @@ #include "geometry_msgs/Twist.h" // External Libraries -#include "libserialport/libserialport.h" +#include // User Defined Libraries / Headers #include "amp_err.h" @@ -65,7 +65,7 @@ int main(int argc, char** argv) { config.xon_xoff = AMP_SERIAL_CONFIG_XST; // Initialize the Serial Port - amp_serial_jetson_initialize(); + amp_serial_jetson_initialize(port); //amp_serial_jetson_enable_default(); @@ -155,7 +155,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { * * initializes any given port */ - amp_err_code_t amp_serial_jetson_initialize() { + amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) { // Declare & Initialize Local Variables #ifdef DEBUG @@ -173,8 +173,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { // Open the Serial Port based on the Previous Handle fprintf(fptr1, "Opening port...\n"); #endif - - check(sp_open(port, (enum sp_mode)(SP_MODE_READ | SP_MODE_WRITE)), AMP_SERIAL_ERROR_INIT); + check(sp_open(port, SP_MODE_READ_WRITE), AMP_SERIAL_ERROR_INIT); #ifdef DEBUG fprintf(fptr1, "Setting configurations...\n"); @@ -612,7 +611,7 @@ void end_program(amp_err_code_t amp_err) { /* Free any structures we allocated. */ if (&config != NULL) - //sp_free_config(&config); + sp_free_config(&config); if (port != NULL) sp_free_port(port); diff --git a/src/ti_comm/src/amp_serial_jetson.h b/src/ti_comm/src/amp_serial_jetson.h index 52a04b6..088291e 100644 --- a/src/ti_comm/src/amp_serial_jetson.h +++ b/src/ti_comm/src/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 4800 // The Configuration Baud Rate +#define AMP_SERIAL_CONFIG_BAUD 9600 // 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 @@ -29,9 +29,9 @@ #define AMP_SERIAL_RC_CTRL_MAX_VEL 10.0f // Max Remote Control Velocity in (m/s) #define AMP_SERIAL_RC_CTRL_MAX_ANG 20.0f // Max Remote Control Steering Angle (rads) -#define AMP_MAX_VEL 0.4f +#define AMP_MAX_VEL 10.0f #define AMP_MIN_VEL 0.0f -#define AMP_MAX_ANG 1.4f +#define AMP_MAX_ANG 5.0f #define AMP_MIN_ANG 0.0f #define PS3_BUTTON_SELECT 0 @@ -113,8 +113,8 @@ typedef enum amp_control_state_t { // DECLARE PACKET DATA STRUCTURES typedef struct amp_serial_pkt_control_t { - uint8_t v_angle; // vehicle steering angle - uint8_t v_speed; // vehicle speed + int v_angle; // vehicle steering angle + int v_speed; // vehicle speed } amp_serial_pkt_control_t; typedef struct amp_serial_pkt_dac_t { @@ -160,15 +160,15 @@ typedef struct amp_serial_pkt_t { // FUNCTION DECLARATIONS -------------------------------------------------- -amp_err_code_t amp_serial_jetson_initialize(); +amp_err_code_t amp_serial_jetson_initialize(sp_port * _port); void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config); void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config); -//void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg); +void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg); -//void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg); +void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg); amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size); @@ -176,22 +176,16 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data); amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte); -int send(amp_serial_pkt_t * pkt); - amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes); uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf); amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte); -int receive(amp_serial_pkt_t * pkt, int size); - int float_to_int(float max, float min, float num); void amp_serial_jetson_enable_kart(); -void amp_serial_jetson_kill_kart(); - void amp_serial_jetson_enable_drive(); void amp_serial_jetson_enable_default(); diff --git a/src/ti_comm/src/amp_serial_jetson_ECU_TEST b/src/ti_comm/src/amp_serial_jetson_ECU_TEST deleted file mode 100755 index d9cc5c1..0000000 Binary files a/src/ti_comm/src/amp_serial_jetson_ECU_TEST and /dev/null differ diff --git a/src/ti_comm/src/amp_serial_jetson_ECU_TEST.cpp b/src/ti_comm/src/amp_serial_jetson_ECU_TEST.cpp index bb04f84..a96f796 100644 --- a/src/ti_comm/src/amp_serial_jetson_ECU_TEST.cpp +++ b/src/ti_comm/src/amp_serial_jetson_ECU_TEST.cpp @@ -49,127 +49,80 @@ FILE * fptr3 = fopen("debug_rx.txt", "w"); int main(int argc, char** argv) { - int size; - - // Global Configuration Parameters - config.baudrate = AMP_SERIAL_CONFIG_BAUD; - config.bits = AMP_SERIAL_CONFIG_BITS; - config.parity = AMP_SERIAL_CONFIG_PARY; - config.stopbits = AMP_SERIAL_CONFIG_STOP; - config.cts = AMP_SERIAL_CONFIG_CTS; - config.dsr = AMP_SERIAL_CONFIG_DSR; - config.dtr = AMP_SERIAL_CONFIG_DTR; - config.rts = AMP_SERIAL_CONFIG_RTS; - config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - // Initialize the Serial Port - amp_serial_jetson_initialize(port); - - //amp_serial_jetson_enable_default(); - - // Set the kart to the enable state - amp_serial_jetson_enable_kart(); - - // Set the kart to the drive state - //amp_serial_jetson_enable_drive(); - - while(true) { - // 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, 8.0); //msg->linear.x; - c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, 2.0); //msg->angular.z; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = 0xE2; - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - #ifdef DEBUG - fprintf(fptr1, "Sending Packet...\n"); - #endif - sleep(15); - amp_serial_jetson_tx_pkt(&s_pkt, &size); - #ifdef DEBUG - fprintf(fptr1, "Receiving Packet...\n"); - #endif - sleep(15); - amp_serial_jetson_rx_pkt(&s_pkt, size); - } + int size; + + // Global Configuration Parameters + config.baudrate = AMP_SERIAL_CONFIG_BAUD; + config.bits = AMP_SERIAL_CONFIG_BITS; + config.parity = AMP_SERIAL_CONFIG_PARY; + config.stopbits = AMP_SERIAL_CONFIG_STOP; + config.cts = AMP_SERIAL_CONFIG_CTS; + config.dsr = AMP_SERIAL_CONFIG_DSR; + config.dtr = AMP_SERIAL_CONFIG_DTR; + config.rts = AMP_SERIAL_CONFIG_RTS; + config.xon_xoff = AMP_SERIAL_CONFIG_XST; + + // Initialize the Serial Port + amp_serial_jetson_initialize(); + + //amp_serial_jetson_enable_default(); + + // Set the kart to the enable state + amp_serial_jetson_enable_kart(); + + // Set the kart to the drive state + //amp_serial_jetson_enable_drive(); + + while(true) { + // Declare & Initialize Local Variables + amp_serial_pkt_t s_pkt; // Full Serial Packet + amp_serial_pkt_control_t c_pkt; // Control Data Packet + + + 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, 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; + s_pkt.size = 2; + + // Copy From the Control Packet to the Serial Packet + memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); + + // Send the Packet + #ifdef DEBUG + fprintf(fptr1, "Sending Packet...\n"); + #endif + sleep(0.5); + amp_serial_jetson_tx_pkt(&s_pkt, &size); + #ifdef DEBUG + fprintf(fptr1, "Receiving Packet...\n"); + #endif + sleep(0.5); + //amp_serial_jetson_rx_pkt(&s_pkt, size); + } + sleep(1); + } + } - #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) - fclose(fptr1); - #endif - - #ifdef DEBUG_TX - fclose(fptr2); - #endif - - #ifdef DEBUG_RX - fclose(fptr3); - #endif - - return EXIT_SUCCESS; -} - -/*void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // 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 = (unsigned char)msg->linear.x; - c_pkt.v_angle = (unsigned char)msg->angular.z; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); + #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) + fclose(fptr1); + #endif - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt); + #ifdef DEBUG_TX + fclose(fptr2); + #endif - printf("Sending Packet...\n"); + #ifdef DEBUG_RX + fclose(fptr3); + #endif - return; + return EXIT_SUCCESS; } -void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // Declare & Initialize Local Variables - uint8_t translational_velocity = msg->linear.x; // Translational Velocity Command - uint8_t drive_angle = msg->angular.z; // Steering Angle Command - amp_serial_pkt_t s_pkt; // Full Serial Packet - amp_serial_pkt_control_t c_pkt; // Control Data Format - - // Check Current Status of the Car's Control (RC / Autonomous) - if (AMP_CONTROL_REMOTE == amp_control_state) { - return; - } - - // Create Control Packet - c_pkt.v_speed = translational_velocity; - c_pkt.v_angle = drive_angle; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt); - - return; -}*/ - /* * FUNCTION: * @@ -177,7 +130,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { * * initializes any given port */ - amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) { + amp_err_code_t amp_serial_jetson_initialize() { // Declare & Initialize Local Variables #ifdef DEBUG @@ -337,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 @@ -346,6 +299,7 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) // ID Byte s_data[s_pos++] = (uint8_t)pkt->id; c_crc += pkt->id & 0xFF; + #ifdef DEBUG_TX fprintf(fptr1, "Packet contents\n"); fprintf(fptr2, "Packet contents\n"); @@ -354,21 +308,26 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) #endif // Size Byte - s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif - - // Data Byte - for (i = 0; i < pkt->size; i++) { - s_data[s_pos++] = (uint8_t)pkt->msg[i]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif + if(pkt->size > 0) + { + s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; + c_crc += (0xE0 + (uint8_t)pkt->size) & 0xFF; + + #ifdef DEBUG_TX + fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); + fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); + #endif + + // Data Byte + for (i = 0; i < pkt->size; i++) { + s_data[s_pos++] = (uint8_t)pkt->msg[i]; + c_crc += pkt->msg[i] & 0xFF; + + #ifdef DEBUG_TX + fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); + fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); + #endif + } } // Send the CRC of the Previous Packet @@ -486,8 +445,8 @@ amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { int i; for(i = 0; i < bytes; i++) { - fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); + fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); + fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); } #endif @@ -590,7 +549,7 @@ int float_to_int(float max, float min, float num) { int val; - val = (int)roundf((num - min)/(max-min)*255); + val = (int)roundf((num-min)/(max-min)*255); return val; } @@ -723,8 +682,8 @@ void amp_serial_jetson_enable_kart() { // Enable the kart t_pkt.id = AMP_SERIAL_ENABLE; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; + t_pkt.size = 0; + size = 0; amp_serial_jetson_tx_pkt(&t_pkt, &size); } @@ -739,8 +698,8 @@ void amp_serial_jetson_enable_drive() { #endif t_pkt.id = AMP_SERIAL_DRIVE; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; + t_pkt.size = 0; + size = 0; amp_serial_jetson_tx_pkt(&t_pkt, &size); } @@ -755,8 +714,8 @@ void amp_serial_jetson_enable_default() { #endif t_pkt.id = AMP_SERIAL_DEFAULT; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; + t_pkt.size = 0; + size = 0; amp_serial_jetson_tx_pkt(&t_pkt, &size); } diff --git a/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST b/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST deleted file mode 100755 index 937e62d..0000000 Binary files a/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST and /dev/null differ diff --git a/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST.cpp b/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST.cpp index 97a8700..e0dac89 100644 --- a/src/ti_comm/src/amp_serial_jetson_SERIAL_TEST.cpp +++ b/src/ti_comm/src/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 @@ -35,6 +33,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,9 +50,35 @@ FILE * fptr3 = fopen("debug_rx.txt", "w"); #endif int main(int argc, char** argv) { - int size, i; - 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}; + // Command pair structured as: + queue > commands; + // File to read debug commands from + string filename = "debug.txt"; + // Default delay value of 0.05 + float delay = 0.05; + float speed; + float angle; + + 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 + while (fin >> speed && fin >> angle) { + 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}; + //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; @@ -65,7 +92,7 @@ int main(int argc, char** argv) { config.xon_xoff = AMP_SERIAL_CONFIG_XST; // Initialize the Serial Port - amp_serial_jetson_initialize(port); + amp_serial_jetson_initialize(); //amp_serial_jetson_enable_default(); @@ -86,19 +113,23 @@ 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(!commands.empty()) { + // Get speed and angle for current command + tie(speed, angle) = commands.front(); + commands.pop(); - while(i < 10) { // 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; - s_pkt.size = sizeof(amp_serial_pkt_control_t); + s_pkt.size = 2; // Copy From the Control Packet to the Serial Packet memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); @@ -107,65 +138,16 @@ int main(int argc, char** argv) { #ifdef DEBUG fprintf(fptr1, "Sending Packet...\n"); #endif - sleep(100); + sleep(delay); amp_serial_jetson_tx_pkt(&s_pkt, &size); #ifdef DEBUG fprintf(fptr1, "Receiving Packet...\n"); #endif - i++; - } - - #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) - fclose(fptr1); - #endif - - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - //printf("%d\n", c_pkt.v_speed); - //printf("%d\n", c_pkt.v_angle); - - sp_flush(port, SP_BUF_BOTH); - - // Send the Packet - #ifdef DEBUG - fprintf(fptr1, "Sending Packet...\n"); - #endif - size = send(&s_pkt); - //amp_serial_jetson_tx_pkt(&s_pkt, &size); - #ifdef DEBUG - fprintf(fptr1, "Receiving Packet...\n"); - #endif - printf("hello\n"); - usleep(50000); - printf("goodbye\n"); - error = receive(&s_pkt, size); - if(error==42) - { - printf("RX Error\n"); - #ifdef DEBUG - fprintf(fptr1, "RX Error\n"); - #endif - } - else if(error!=0) - { - printf("Mismatch\n"); - #ifdef DEBUG - fprintf(fptr1, "Mismatch\n"); - #endif - } + sleep(delay); //amp_serial_jetson_rx_pkt(&s_pkt, size); - //i++; + i++; } - printf("done\n"); - fclose(fptr); - //amp_serial_jetson_kill_kart(); - #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) fclose(fptr1); #endif @@ -181,59 +163,6 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } -/*void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // 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 = msg->linear.x; - c_pkt.v_angle = msg->angular.z; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt); - - printf("Sending Packet...\n"); - - return; -} - -void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // Declare & Initialize Local Variables - float translational_velocity = msg->linear.x; // Translational Velocity Command - float drive_angle = msg->angular.z; // Steering Angle Command - amp_serial_pkt_t s_pkt; // Full Serial Packet - amp_serial_pkt_control_t c_pkt; // Control Data Format - - // Check Current Status of the Car's Control (RC / Autonomous) - if (AMP_CONTROL_REMOTE == amp_control_state) { - return; - } - - // Create Control Packet - c_pkt.v_speed = translational_velocity; - c_pkt.v_angle = drive_angle; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt); - - return; -}*/ - /* * FUNCTION: * @@ -241,7 +170,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { * * initializes any given port */ - amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) { + amp_err_code_t amp_serial_jetson_initialize() { // Declare & Initialize Local Variables #ifdef DEBUG @@ -401,7 +330,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 - unsigned int c_crc = 0; // Used to calculate the current CRC + uint8_t c_crc = 0; // Used to calculate the current CRC int i; // Start Byte @@ -410,6 +339,7 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) // ID Byte s_data[s_pos++] = (uint8_t)pkt->id; c_crc += pkt->id & 0xFF; + #ifdef DEBUG_TX fprintf(fptr1, "Packet contents\n"); fprintf(fptr2, "Packet contents\n"); @@ -418,21 +348,26 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) #endif // Size Byte - s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif + if(pkt->size > 0) + { + s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; + c_crc += (0xE0 + (uint8_t)pkt->size) & 0xFF; - // Data Byte - for (i = 0; i < pkt->size; i++) { - s_data[s_pos++] = (uint8_t)pkt->msg[i]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif + #ifdef DEBUG_TX + fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); + fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); + #endif + + // Data Byte + for (i = 0; i < pkt->size; i++) { + s_data[s_pos++] = (uint8_t)pkt->msg[i]; + c_crc += pkt->msg[i] & 0xFF; + + #ifdef DEBUG_TX + fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); + fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); + #endif + } } // Send the CRC of the Previous Packet @@ -458,8 +393,6 @@ void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) */ amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) { - sp_flush(port, SP_BUF_INPUT); - // Wait for data to be sent out of the port while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { } @@ -481,81 +414,6 @@ amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) { return AMP_ERROR_NONE; } -int send(amp_serial_pkt_t * pkt) -{ - uint8_t * start_byte = new (uint8_t); - uint8_t * stop_byte = new (uint8_t); - uint8_t * pkt_id_byte = new (uint8_t); - uint8_t * pkt_size_byte = new (uint8_t); - uint8_t * data_byte = new (uint8_t); - uint8_t * crc_byte = new (uint8_t); - uint8_t c_crc = 0; - int size = 0; - - *start_byte = (uint8_t)AMP_SERIAL_START_PKT; - *stop_byte = (uint8_t)AMP_SERIAL_STOP_PKT; - - // Start Byte - amp_serial_jetson_tx_byte(start_byte); - size++; - - // ID Byte - #ifdef DEBUG_TX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - *pkt_id_byte = (uint8_t)pkt->id; - amp_serial_jetson_tx_byte(pkt_id_byte); - size++; - c_crc += pkt->id & 0xFF; - - if(pkt->size!=0) - { - // Size Byte - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif - *pkt_size_byte = 0xE0 + (uint8_t)pkt->size; - amp_serial_jetson_tx_byte(pkt_size_byte); - size++; - c_crc += pkt->size & 0xFF; - - // Data Byte - for (int i = 0; i < pkt->size; i++) - { - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - *data_byte = (uint8_t)pkt->msg[i]; - amp_serial_jetson_tx_byte(data_byte); - size++; - c_crc += pkt->msg[i] & 0xFF; - } - } - - // Send the CRC of the Previous Packet - pkt->crc = c_crc; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr1, "End packet\n"); - fprintf(fptr2, "End packet\n"); - #endif - *crc_byte = (uint8_t)pkt->crc; - amp_serial_jetson_tx_byte(crc_byte); - size++; - - // Finish off by sending the ETX - amp_serial_jetson_tx_byte(stop_byte); - size++; - - return size; -} - /* * FUNCTION: * @@ -565,7 +423,7 @@ int send(amp_serial_pkt_t * pkt) */ amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { // Declare & Initialize Local Variables - unsigned int c_crc = 0; // Used to calculate the current CRC + uint8_t c_crc; // Used to calculate the current CRC uint8_t * s_buf = static_cast(malloc(bytes)); // Used for single byte reads i.e. STX, ETX @@ -613,15 +471,143 @@ amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { #endif return AMP_SERIAL_ERROR_RX_NO_STOP; } - case SP_PARITY_ODD: - return "Odd"; - case SP_PARITY_EVEN: - return "Even"; - case SP_PARITY_MARK: - return "Mark"; - case SP_PARITY_SPACE: - return "Space"; - default: + + c_crc = amp_serial_jetson_rebuild_packet(pkt, s_buf); + if(c_crc != pkt->crc) { + #ifdef DEBUG_RX + fprintf(fptr1, "ERROR: CRC Mismatch\n"); + fprintf(fptr3, "ERROR: CRC Mismatch\n"); + #endif + return AMP_SERIAL_ERROR_CRC; + } + + #ifdef DEBUG_RX + int i; + for(i = 0; i < bytes; i++) + { + fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); + fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); + } + #endif + + // Free all allocated space + free(s_buf); + return AMP_ERROR_NONE; +} + +/* + * FUNCTION: + * + * uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) + * + * rebuilds packet from received buffer and returns crc for confirmation + */ +uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) { + + int i; + uint8_t c_crc = 0; + // ID from the Serial Port + pkt->id = (amp_serial_pkt_id_t)s_buf[1]; + c_crc += pkt->id & 0xFF; + #ifdef DEBUG_RX + fprintf(fptr1, "Packet contents\n"); + fprintf(fptr2, "Packet contents\n"); + fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); + fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); + #endif + + // Size from the Serial Port + pkt->size = s_buf[2]; + c_crc += pkt->size & 0xFF; + #ifdef DEBUG_TX + fprintf(fptr1, "size: %u\n", (uint8_t)pkt->size); + fprintf(fptr2, "size: %u\n", (uint8_t)pkt->size); + #endif + + // Data from the Incoming Packet + for (i = 0; i < pkt->size; i++) + { + pkt->msg[i] = s_buf[i+3]; + c_crc += pkt->msg[i] & 0xFF; + #ifdef DEBUG_TX + fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); + fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); + #endif + } + + // CRC from the Incoming Packet + pkt->crc = s_buf[i+3]; + #ifdef DEBUG_TX + fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); + fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); + #endif + + return c_crc; +} + +/* + * FUNCTION: + * + * amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) + * + * byte by byte reciever for debugging + */ +amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) { + + // Verify the Current State of the Serial Port + while (port_state != AMP_SERIAL_STATE_IDLE) { + } + + // Verify that there is Data to read + while(sp_input_waiting(port) == 0) { + #ifdef DEBUG_RX + fprintf(fptr1, "Waiting...\n"); + fprintf(fptr3, "Waiting...\n"); + #endif + } + + // Read in the data + check(sp_nonblocking_read(port, s_byte, sizeof(uint8)), AMP_SERIAL_ERROR_RX); + + port_state = AMP_SERIAL_STATE_IDLE; + + #ifdef DEBUG_RX + fprintf(fptr1, "rx: %u\n", *s_byte); + fprintf(fptr3, "rx: %u\n", *s_byte); + #endif + + return AMP_ERROR_NONE; +} + +/* FUNCTION: + * + * int float_to_int(float, max, float min, float num) + * + * returns converted int from 0 to 255 + */ +int float_to_int(float max, float min, float num) +{ + int val; + + val = (int)roundf((num-min)/(max-min)*255); + + return val; +} + +/* + * FUNCTION: + * + * const char *parity_name(enum sp_parity parity) + * + * returns the parity state of the buffer + */ +const char *parity_name(enum sp_parity parity) +{ + switch (parity) { + case SP_PARITY_INVALID: + return "(Invalid)"; + case SP_PARITY_NONE: + return "None"; case SP_PARITY_ODD: return "Odd"; case SP_PARITY_EVEN: @@ -738,6 +724,7 @@ void amp_serial_jetson_enable_kart() { t_pkt.id = AMP_SERIAL_ENABLE; t_pkt.size = 0; size = 0; + amp_serial_jetson_tx_pkt(&t_pkt, &size); } @@ -751,8 +738,8 @@ void amp_serial_jetson_enable_drive() { #endif t_pkt.id = AMP_SERIAL_DRIVE; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; + t_pkt.size = 0; + size = 0; amp_serial_jetson_tx_pkt(&t_pkt, &size); } @@ -767,8 +754,39 @@ void amp_serial_jetson_enable_default() { #endif t_pkt.id = AMP_SERIAL_DEFAULT; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; + t_pkt.size = 0; + size = 0; 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 << " 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") { + delay = stof(*++i); + } + } +} + diff --git a/src/ti_comm/src/Ignore/amp_serial_jetson.cpp b/src/ti_comm/src/forbidden/Ignore/amp_serial_jetson.cpp similarity index 100% rename from src/ti_comm/src/Ignore/amp_serial_jetson.cpp rename to src/ti_comm/src/forbidden/Ignore/amp_serial_jetson.cpp diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.deps/linux_termios.Plo b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.deps/linux_termios.Plo similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.deps/linux_termios.Plo rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.deps/linux_termios.Plo diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.deps/serialport.Plo b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.deps/serialport.Plo similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.deps/serialport.Plo rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.deps/serialport.Plo diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.a b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.a similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.a rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.a diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.la b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.la similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.la rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.la diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.lai b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.lai similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.lai rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.lai diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so.0 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so.0 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so.0 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so.0 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so.0.0.0 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so.0.0.0 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/libserialport.so.0.0.0 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/libserialport.so.0.0.0 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/linux_termios.o b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/linux_termios.o similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/linux_termios.o rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/linux_termios.o diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/serialport.o b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/serialport.o similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/.libs/serialport.o rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/.libs/serialport.o diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/AUTHORS b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/AUTHORS similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/AUTHORS rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/AUTHORS diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/COPYING b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/COPYING similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/COPYING rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/COPYING diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/ChangeLog b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/ChangeLog similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/ChangeLog rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/ChangeLog diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/Doxyfile b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Doxyfile similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/Doxyfile rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Doxyfile diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/INSTALL b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/INSTALL similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/INSTALL rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/INSTALL diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile.am b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile.am similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile.am rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile.am diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile.in b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile.in similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/Makefile.in rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/Makefile.in diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/NEWS b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/NEWS similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/NEWS rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/NEWS diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/README b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/README similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/README rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/README diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/aclocal-1.14 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/aclocal-1.14 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/aclocal-1.14 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/aclocal-1.14 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/aclocal.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/aclocal.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/aclocal.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/aclocal.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.0 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.0 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.0 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.0 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.1 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.1 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.1 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.1 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.2 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.2 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/output.2 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/output.2 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/requests b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/requests similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/requests rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/requests diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.0 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.0 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.0 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.0 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.1 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.1 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.1 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.1 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.2 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.2 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autom4te.cache/traces.2 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autom4te.cache/traces.2 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/automake-1.14 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/automake-1.14 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/automake-1.14 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/automake-1.14 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ar-lib b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ar-lib similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ar-lib rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ar-lib diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/compile b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/compile similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/compile rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/compile diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/config.guess b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/config.guess similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/config.guess rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/config.guess diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/config.sub b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/config.sub similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/config.sub rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/config.sub diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/depcomp b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/depcomp similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/depcomp rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/depcomp diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/install-sh b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/install-sh similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/install-sh rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/install-sh diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/libtool.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/libtool.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/libtool.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/libtool.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltmain.sh b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltmain.sh similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltmain.sh rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltmain.sh diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltoptions.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltoptions.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltoptions.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltoptions.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltsugar.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltsugar.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltsugar.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltsugar.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltversion.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltversion.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/ltversion.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/ltversion.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/lt~obsolete.m4 b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/lt~obsolete.m4 similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/lt~obsolete.m4 rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/lt~obsolete.m4 diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/missing b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/missing similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/autostuff/missing rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/autostuff/missing diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/config.log b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/config.log similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/config.log rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/config.log diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/config.status b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/config.status similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/config.status rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/config.status diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/configure b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/configure similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/configure rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/configure diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/configure.ac b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/configure.ac similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/configure.ac rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/configure.ac diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.h b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.h similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.h rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.h diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.h.in b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.h.in similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.h.in rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.h.in diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.la b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.la similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.la rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.la diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.pc b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.pc similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.pc rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.pc diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.pc.in b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.pc.in similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libserialport.pc.in rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libserialport.pc.in diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/libtool b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libtool similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/libtool rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/libtool diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.c b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.c similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.c rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.c diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.h b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.h similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.h rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.h diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.lo b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.lo similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.lo rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.lo diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.o b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.o similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/linux_termios.o rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/linux_termios.o diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.c b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.c similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.c rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.c diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.lo b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.lo similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.lo rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.lo diff --git a/src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.o b/src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.o similarity index 100% rename from src/ti_comm/src/Ignore/libserialport-0.1.0/serialport.o rename to src/ti_comm/src/forbidden/Ignore/libserialport-0.1.0/serialport.o diff --git a/src/ti_comm/src/forbidden/sim_launch_log.txt b/src/ti_comm/src/forbidden/sim_launch_log.txt new file mode 100644 index 0000000..902fad8 --- /dev/null +++ b/src/ti_comm/src/forbidden/sim_launch_log.txt @@ -0,0 +1,518 @@ +0.399993 -0.129131 +0.395507 -0.058358 +0.400000 -0.162196 +0.400000 -0.200549 +0.281664 -0.402751 +0.400000 -0.201077 +0.400000 -0.198908 +0.221914 -0.259458 +0.367433 -0.215623 +0.378678 0.025144 +0.385328 0.153559 +0.243422 0.458009 +0.204615 0.530410 +0.202096 0.535832 +0.213916 0.511225 +0.229456 0.481859 +0.133544 0.544661 +0.221624 0.496260 +0.331985 0.346849 +0.336172 0.342862 +0.357232 0.324073 +0.360683 0.321183 +0.360132 0.321641 +0.400000 0.283990 +0.400000 0.271273 +0.400000 0.262269 +0.233803 0.257663 +0.382283 0.034450 +0.400000 -0.082544 +0.400000 -0.107400 +0.400000 -0.199835 +0.400000 -0.198817 +0.400000 -0.270477 +0.400000 -0.250603 +0.400000 -0.240129 +0.228897 -0.261539 +0.377082 -0.213436 +0.400000 -0.200403 +0.400000 -0.198883 +0.238486 -0.256902 +0.377989 -0.046593 +0.400000 0.061902 +0.400000 0.169694 +0.400000 0.209163 +0.400000 0.206036 +0.400000 0.218339 +0.400000 0.207222 +0.400000 0.203835 +0.400000 0.218374 +0.400000 0.210424 +0.215801 0.261201 +0.363494 0.220926 +0.400000 0.203330 +0.400000 0.200300 +0.225603 0.262964 +0.370543 0.222283 +0.400000 0.253096 +0.400000 0.248166 +0.400000 0.241632 +0.343031 0.336517 +0.336253 0.342786 +0.355773 0.325311 +0.359570 0.322110 +0.361027 0.320897 +0.171064 0.324338 +0.320028 0.302448 +0.400000 0.251660 +0.400000 0.240588 +0.216413 0.261100 +0.360574 0.219170 +0.400000 0.184809 +0.400000 0.091054 +0.216266 -0.044249 +0.209882 -0.260476 +0.352615 -0.223940 +0.400000 -0.199687 +0.400000 -0.198474 +0.400000 -0.197842 +0.400000 -0.237125 +0.400000 -0.239626 +0.379062 -0.306589 +0.362940 -0.319318 +0.365091 -0.317561 +0.400000 -0.249767 +0.400000 -0.233926 +0.400000 -0.176118 +0.400000 -0.092494 +0.400000 0.023811 +0.131289 0.699893 +0.099018 0.699893 +0.051726 0.699893 +0.062552 0.699893 +0.078894 0.699893 +0.082304 0.699893 +0.095281 0.699893 +0.099264 0.699893 +0.101313 0.699893 +0.123918 0.699893 +0.135082 0.699893 +0.143881 0.695145 +0.288977 0.157500 +0.400000 0.008182 +0.400000 0.117345 +0.400000 0.086492 +0.400000 -0.025431 +0.208239 0.261038 +0.351522 0.277130 +0.393027 0.283201 +0.397534 0.275254 +0.181631 0.436426 +0.190285 0.413640 +0.332731 0.346133 +0.103418 0.620411 +0.129288 0.512387 +0.220872 0.468662 +0.137583 0.490269 +0.227831 0.459420 +0.335772 0.343239 +0.341592 0.337829 +0.355474 0.325566 +0.363505 0.318855 +0.400000 0.228275 +0.400000 0.212758 +0.400000 0.207878 +0.212715 0.159788 +0.362233 0.151621 +0.400000 0.023449 +0.400000 -0.088254 +0.225625 -0.262240 +0.370455 -0.219944 +0.230573 -0.288022 +0.206887 -0.345691 +0.357876 -0.323530 +0.377726 -0.307607 +0.383094 -0.303558 +0.359488 -0.322178 +0.363296 -0.319027 +0.362996 -0.319272 +0.200754 -0.326464 +0.346239 -0.286853 +0.400000 -0.250316 +0.400000 -0.239661 +0.221558 -0.259290 +0.209874 0.008883 +0.359214 0.002579 +0.209909 0.260370 +0.221091 0.259565 +0.365949 0.216528 +0.400000 0.201974 +0.400000 0.200032 +0.400000 0.198214 +0.399968 0.291479 +0.379787 0.306040 +0.178645 0.591494 +0.070273 0.699893 +-0.123380 -0.682497 +-0.180972 -0.557458 +-0.176537 -0.597001 +-0.052460 -0.699893 +0.167758 0.446238 +0.304024 0.258020 +0.400000 0.198188 +0.400000 0.187147 +0.205960 -0.110805 +0.351367 -0.180206 +0.206943 0.264524 +0.208919 0.262043 +0.348503 0.229705 +0.061139 0.699893 +0.213018 0.268209 +0.353459 0.249826 +0.400000 0.235447 +0.400000 0.228051 +0.400000 0.291457 +0.400000 0.200614 +0.400000 0.198681 +0.134598 0.699893 +-0.128557 -0.699893 +-0.200000 -0.529271 +-0.193038 -0.556180 +-0.188530 -0.566829 +0.093716 0.699893 +0.230547 0.446839 +0.349880 0.330403 +0.193500 0.384056 +0.199165 0.339072 +0.344295 0.327601 +0.382132 0.304276 +0.388462 0.299611 +0.400000 0.237607 +0.400000 0.222948 +0.400000 0.215182 +0.238395 0.257405 +0.384436 0.039075 +0.400000 -0.068627 +0.400000 -0.156099 +0.400000 -0.198116 +0.400000 -0.159324 +0.400000 -0.044178 +0.400000 0.067796 +0.400000 0.183067 +0.400000 0.254590 +0.400000 0.250554 +0.400000 0.242584 +0.104107 0.613652 +0.137564 0.689250 +0.167086 0.622832 +0.186011 0.572939 +0.144639 0.497382 +0.213546 0.505903 +0.316761 0.362129 +0.332116 0.346723 +0.187709 0.348452 +0.325876 0.289933 +0.400000 0.237044 +0.400000 0.227504 +0.400000 0.190034 +0.400000 0.101102 +0.222417 -0.260034 +0.369235 -0.216319 +0.400000 -0.212589 +0.400000 -0.204683 +0.400000 -0.208022 +0.400000 -0.205333 +0.400000 -0.201840 +0.235270 -0.259271 +0.226777 -0.261450 +0.372368 -0.218652 +0.191814 -0.263631 +0.336322 -0.242697 +0.060922 -0.699893 +0.072969 -0.699893 +0.067667 -0.699893 +0.077616 -0.699893 +0.077285 -0.699893 +0.070526 -0.699893 +0.077585 -0.699893 +0.095321 -0.699893 +0.083532 -0.699893 +0.083809 -0.699893 +0.133276 -0.690946 +0.184004 -0.392362 +0.300228 -0.355902 +0.400000 -0.184821 +0.400000 -0.035269 +0.237206 -0.176914 +0.387815 -0.104012 +0.340319 -0.312257 +0.257541 -0.375837 +0.154926 -0.659038 +0.143596 -0.696119 +0.136427 -0.699893 +0.137998 -0.699893 +0.140151 -0.699893 +0.105129 -0.699893 +0.095423 -0.699893 +0.097096 -0.699893 +0.235971 -0.374491 +0.364162 -0.249074 +0.374734 -0.309909 +0.376470 -0.308569 +0.375740 -0.309131 +0.160529 -0.462807 +0.182204 -0.401192 +0.320119 -0.358648 +0.189057 -0.375577 +0.329652 -0.349110 +0.181344 -0.337777 +0.323565 -0.313507 +0.395638 -0.273012 +0.400000 -0.259836 +0.220766 -0.259520 +0.223910 -0.238599 +0.372033 -0.142353 +0.400000 -0.016953 +0.400000 0.096326 +0.203079 0.294776 +0.346488 0.292967 +0.332845 0.346023 +0.337058 0.342029 +0.207041 0.525281 +0.205081 0.529418 +0.226512 0.487182 +0.216011 0.507076 +0.207035 0.525293 +0.296172 0.384955 +0.197477 0.348417 +0.345920 0.328746 +0.385154 0.302031 +0.392285 0.296861 +0.400000 0.198710 +0.400000 0.179561 +0.220033 0.234129 +0.367734 0.131948 +0.400000 0.130746 +0.400000 0.059498 +0.238147 0.257125 +0.389524 0.175192 +0.400000 0.198920 +0.400000 0.198163 +0.400000 0.200898 +0.400000 0.199171 +0.400000 0.183387 +0.238027 0.258618 +0.386962 0.218303 +0.400000 0.206715 +0.400000 0.204318 +0.400000 0.287874 +0.400000 0.289291 +0.101893 0.699893 +-0.139262 -0.699893 +-0.188884 -0.565979 +-0.064017 -0.699893 +0.168412 0.453861 +0.300898 0.266790 +0.206943 0.262060 +0.223797 0.260734 +0.372466 0.217441 +0.226291 0.259663 +0.375992 0.210594 +0.400000 0.179833 +0.400000 0.086030 +0.237767 0.208276 +0.387307 0.007967 +0.400000 -0.080845 +0.400000 -0.100043 +0.170545 0.390674 +0.310178 0.337658 +0.380589 0.223350 +0.394414 0.139182 +0.220210 0.498945 +0.142188 0.699893 +0.132852 0.699893 +0.077329 0.699893 +0.066315 0.699893 +0.258673 0.316290 +0.394130 0.208283 +0.400000 0.202895 +0.400000 0.218378 +0.400000 0.216695 +0.217602 0.260232 +0.364372 0.219607 +0.205946 0.262401 +0.350973 0.230061 +0.400000 0.201408 +0.400000 0.199995 +0.400000 0.206174 +0.400000 0.203084 +0.400000 0.198421 +0.400000 0.191201 +0.400000 0.121562 +0.400000 0.200131 +0.400000 0.199078 +0.400000 0.166444 +0.239339 0.048537 +0.389533 -0.085417 +0.400000 -0.195254 +0.400000 -0.198789 +0.199947 -0.262655 +0.185527 0.136874 +0.332674 0.018591 +0.210454 0.261031 +0.354420 0.228542 +0.400000 0.201929 +0.400000 0.199540 +0.400000 0.257324 +0.400000 0.248856 +0.400000 0.223319 +0.400000 0.207815 +0.400000 0.203104 +0.400000 0.206317 +0.400000 0.203105 +0.219287 -0.188665 +0.367868 -0.095949 +0.400000 -0.076553 +0.400000 0.025642 +0.400000 0.127597 +0.400000 0.011427 +0.400000 -0.100189 +0.400000 -0.216928 +0.400000 -0.223644 +0.400000 -0.198528 +0.194640 -0.368238 +0.342898 -0.336638 +0.374900 -0.309780 +0.381037 -0.305097 +0.384065 -0.302836 +0.387198 -0.291468 +0.400000 -0.209876 +0.400000 -0.205837 +0.400000 -0.203585 +0.400000 -0.204713 +0.400000 -0.167227 +0.400000 -0.160015 +0.400000 -0.052365 +0.400000 0.045806 +0.195059 0.261835 +0.337915 0.289629 +0.370586 0.313157 +0.373901 0.310556 +0.157558 0.521629 +-0.051552 -0.699893 +0.133479 0.661680 +0.264904 0.425342 +0.341590 0.337831 +0.344396 0.335281 +0.199120 0.320298 +0.343819 0.313570 +0.400000 0.291457 +0.400000 0.284011 +0.400000 0.201138 +0.231973 0.262010 +0.219894 0.145327 +0.370529 0.088682 +0.400000 -0.034064 +0.400000 -0.144235 +0.079627 -0.699893 +0.110544 -0.689835 +0.142019 -0.347959 +0.162095 -0.264332 +0.290364 -0.265670 +0.141894 -0.333095 +0.266040 -0.298655 +0.370844 -0.224216 +0.379391 -0.264403 +0.081066 0.699893 +0.036559 0.688667 +-0.145060 -0.553253 +-0.181181 -0.584989 +-0.126088 -0.699893 +0.192436 -0.294183 +0.339803 -0.111116 +0.377685 -0.140206 +0.290862 -0.336206 +0.115204 -0.240872 +-0.100487 -0.500279 +-0.194017 -0.446165 +-0.156115 -0.361232 +-0.174621 -0.503364 +-0.122755 -0.595234 +0.178896 0.116203 +0.313922 0.180373 +0.182971 0.264682 +0.320134 0.266411 +0.400000 0.207595 +0.400000 0.204247 +0.210235 0.326466 +0.362362 0.319794 +0.190164 0.424300 +0.203408 0.305862 +0.350515 0.302940 +0.195992 0.368624 +0.338201 0.340961 +0.357439 0.323898 +0.357070 0.324210 +0.196450 0.353008 +0.189178 0.308882 +0.333709 0.303012 +0.183082 0.266322 +0.334650 0.241840 +0.400000 0.198446 +0.400000 0.198100 +0.223475 0.216275 +0.240951 -0.257024 +0.393272 -0.201681 +0.400000 -0.201067 +0.400000 -0.199293 +0.400000 -0.198517 +0.238253 0.067650 +0.391153 0.157145 +0.400000 0.147893 +0.400000 0.069881 +0.236639 0.251274 +0.386312 0.186065 +0.400000 0.072285 +0.400000 -0.037499 +0.400000 0.083137 +0.400000 0.198652 +0.214404 0.294892 +0.361746 0.293630 +0.400000 0.290234 +0.400000 0.234290 +0.211384 0.305638 +0.360773 0.303236 +0.197547 0.338984 +0.341886 0.327048 +0.200486 0.303447 +0.349839 0.301497 +0.392060 0.297021 +0.399231 0.286306 +0.400000 0.208018 +0.400000 0.203378 +0.220297 0.259320 +0.367018 0.215336 +0.400000 0.124053 +0.400000 0.086026 +0.400000 -0.031237 +0.400000 -0.144306 +0.400000 -0.198409 +0.221640 -0.260203 +0.193210 -0.263134 +0.335050 -0.238944 +0.208405 -0.270945 +0.353777 -0.241243 +0.400000 -0.217953 +0.400000 -0.211955 +0.400000 -0.202163 +0.400000 -0.199833 +0.400000 -0.197627 +0.400000 -0.127751 +0.400000 -0.034567 +0.400000 0.089930 +0.400000 0.118239 +0.400000 0.064522 +0.400000 0.185835 +0.400000 0.199435 +0.237588 0.258466 +0.388743 0.206701 diff --git a/src/ti_comm/src/new_lib/Makefile b/src/ti_comm/src/new_lib/Makefile deleted file mode 100644 index e7fb32b..0000000 --- a/src/ti_comm/src/new_lib/Makefile +++ /dev/null @@ -1,22 +0,0 @@ -WARNING = -Wall -Wshadow --pedantic -ERROR = -Wvla -Werror -GCC = g++ -std=c++11 -lstdc++ -lm -PKG_CONFIG = pkg-config -CFLAGS = -g $(WARNING) $(ERROR) $(shell $(PKG_CONFIG) --cflags libserialport) -LIBS = $(shell $(PKG_CONFIG) --libs libserialport) - -TESTFLAGS = -DDEBUG=1 -DDEBUG_TX=1 -DDEBUG_RX=1 -SOURCES = $(wildcard *.cpp) - -BINARIES = $(SOURCES:.cpp=) - -%: %.cpp - $(GCC) $(CFLAGS) $< $(LIBS) -o $@ - -debug_%: %.cpp - $(GCC) $(CFLAGS) $(TESTFLAGS) $< $(LIBS) -o $@ - -all: $(BINARIES) - -clean: - rm $(BINARIES) diff --git a/src/ti_comm/src/new_lib/README.md b/src/ti_comm/src/new_lib/README.md deleted file mode 100644 index 8f74f6c..0000000 --- a/src/ti_comm/src/new_lib/README.md +++ /dev/null @@ -1,10 +0,0 @@ -How to Build Libserialport -1. Add Submodule From Root -* git submodule update --recursive -2. Build Library -* ./autogen.sh -* ./configure -* make -* sudo make install -3. Add Library to Path -* diff --git a/src/ti_comm/src/new_lib/amp_err.h b/src/ti_comm/src/new_lib/amp_err.h deleted file mode 100644 index 4e31624..0000000 --- a/src/ti_comm/src/new_lib/amp_err.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * amp_err.h - * - * Created on: Mar 15, 2019 - * Author: David - */ - -#ifndef SRC_AMP_ERR_H_ -#define SRC_AMP_ERR_H_ - -/* - * Contains the pertinent error codes regarding serial transmission. - */ -typedef enum amp_err_code_t { - AMP_ERROR_NONE = 1, // successful operation - - // Errors Regarding Serial Transmission (-1 -> -20) - AMP_SERIAL_ERROR_RX_TIMEOUT = -1, // timed out waiting for data - AMP_SERIAL_ERROR_CRC = -2, // indicates bad crc (invalid rx bytes) - AMP_SERIAL_ERROR_RX_NO_STOP = -3, // no stop byte received - AMP_SERIAL_ERROR_RX_NO_START = -3, // no start byte received - AMP_SERIAL_ERROR_INIT = -4, // initialization errors - AMP_SERIAL_ERROR_TX = -5, // transmission issues - AMP_SERIAL_ERROR_RX = -6, // receive issues - - // Error Regarding PWM Module (-21 -> -40) - AMP_PWM_ERROR_BOUNDS = -21, // PWM Value out of Bounds - - // Errors Regarding DAC Module (-41 - 60) - AMP_DAC_ERROR_BOUNDS = -41 // DAC Value out of Bounds (12 Bit) -} amp_err_code_t; - -#endif /* SRC_AMP_ERR_H_ */ diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson.h b/src/ti_comm/src/new_lib/amp_serial_jetson.h deleted file mode 100644 index 9e92c2e..0000000 --- a/src/ti_comm/src/new_lib/amp_serial_jetson.h +++ /dev/null @@ -1,199 +0,0 @@ -/* - * amp_serial.h - * - * Created on: Mar 13, 2019 - * Author: David Pimley - */ - -#ifndef SRC_AMP_SERIAL_H_ -#define SRC_AMP_SERIAL_H_ - -#include "util.h" -#include "amp_err.h" - -#define AMP_SERIAL_MAX_PKT_SIZE 0xFF // max packet size - -#define AMP_SERIAL_START_PKT 0x02 // start packet byte -#define AMP_SERIAL_STOP_PKT 0x03 // end packet byte - -#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 -#define AMP_SERIAL_CONFIG_CTS SP_CTS_INVALID // The Configuration CTS -#define AMP_SERIAL_CONFIG_DSR SP_DSR_INVALID // The Configuration DSR -#define AMP_SERIAL_CONFIG_DTR SP_DTR_INVALID // The Configuration DTR -#define AMP_SERIAL_CONFIG_RTS SP_RTS_INVALID // The Configuration RTS -#define AMP_SERIAL_CONFIG_XST SP_XONXOFF_INVALID // The Configuration XON/XOFF - - -#define AMP_SERIAL_RC_CTRL_MAX_VEL 10.0f // Max Remote Control Velocity in (m/s) -#define AMP_SERIAL_RC_CTRL_MAX_ANG 20.0f // Max Remote Control Steering Angle (rads) -#define AMP_MAX_VEL 77 -#define AMP_MIN_VEL 0 -#define AMP_MAX_ANG 255 -#define AMP_MIN_ANG 0 - -#define PS3_BUTTON_SELECT 0 -#define PS3_BUTTON_STICK_LEFT 1 -#define PS3_BUTTON_STICK_RIGHT 2 -#define PS3_BUTTON_START 3 -#define PS3_BUTTON_CROSS_UP 4 -#define PS3_BUTTON_CROSS_RIGHT 5 -#define PS3_BUTTON_CROSS_DOWN 6 -#define PS3_BUTTON_CROSS_LEFT 7 -#define PS3_BUTTON_REAR_LEFT_2 8 -#define PS3_BUTTON_REAR_RIGHT_2 9 -#define PS3_BUTTON_REAR_LEFT_1 10 -#define PS3_BUTTON_REAR_RIGHT_1 11 -#define PS3_BUTTON_ACTION_TRIANGLE 12 -#define PS3_BUTTON_ACTION_CIRCLE 13 -#define PS3_BUTTON_ACTION_CROSS 14 -#define PS3_BUTTON_ACTION_SQUARE 15 -#define PS3_BUTTON_PAIRING 16 - -#define PS3_AXIS_STICK_LEFT_LEFTWARDS 0 -#define PS3_AXIS_STICK_LEFT_UPWARDS 1 -#define PS3_AXIS_STICK_RIGHT_LEFTWARDS 2 -#define PS3_AXIS_STICK_RIGHT_UPWARDS 3 -#define PS3_AXIS_BUTTON_CROSS_UP 4 -#define PS3_AXIS_BUTTON_CROSS_RIGHT 5 -#define PS3_AXIS_BUTTON_CROSS_DOWN 6 -#define PS3_AXIS_BUTTON_CROSS_LEFT 7 -#define PS3_AXIS_BUTTON_REAR_LEFT_2 8 -#define PS3_AXIS_BUTTON_REAR_RIGHT_2 9 -#define PS3_AXIS_BUTTON_REAR_LEFT_1 10 -#define PS3_AXIS_BUTTON_REAR_RIGHT_1 11 -#define PS3_AXIS_BUTTON_ACTION_TRIANGLE 12 -#define PS3_AXIS_BUTTON_ACTION_CIRCLE 13 -#define PS3_AXIS_BUTTON_ACTION_CROSS 14 -#define PS3_AXIS_BUTTON_ACTION_SQUARE 15 -#define PS3_AXIS_ACCELEROMETER_LEFT 16 -#define PS3_AXIS_ACCELEROMETER_FORWARD 17 -#define PS3_AXIS_ACCELEROMETER_UP 18 -#define PS3_AXIS_GYRO_YAW 19 - - -/* - * Contains all packet ids possible from the NVIDIA Jetson TX2 On-Board - * Computer to the TI Micro. Any packet id not found in the below enumeration - * results in a kill kart function to stop the motion of the kart. - */ -typedef enum amp_serial_pkt_id_t { - AMP_SERIAL_CONTROL = 0xF1, // packet of steering & translational floats - //AMP_SERIAL_DAC_CONTROL, // packet for control of DAC - //AMP_SERIAL_PWM_CONTROL, // packet for control of PWM - AMP_SERIAL_DEFAULT = 0xF4, // packet for default mode of kart - AMP_SERIAL_ENABLE = 0xF0, // packet to enter enabled state (Power to the MC/servo) - AMP_SERIAL_DRIVE = 0xF5, // packet to enter drive forward state (will follow throttle/steering commands in FWD direction) - //AMP_SERIAL_DRIVE_REV, // packet to enter drive reverse state - AMP_SERIAL_KILL_KART = 0xF2 // packet for stopping all motion -} amp_serial_pkt_id_t; - -/* - * States of the Current Serial Port. The point of creating the states - * is to ensure that the software is only sending or receiving at one - * time. - */ -typedef enum amp_serial_state_t { - AMP_SERIAL_STATE_IDLE, - AMP_SERIAL_STATE_RX, - AMP_SERIAL_STATE_TX -} amp_serial_state_t; - -/* - * State of the current control of the system. Will be used as a hard toggle - * i.e. starts in autonomous and can be controlled by rc if desired but can't be - * switched back - */ -typedef enum amp_control_state_t { - AMP_CONTROL_AUTONOMOUS, - AMP_CONTROL_REMOTE -} amp_control_state_t; - -// DECLARE PACKET DATA STRUCTURES -typedef struct amp_serial_pkt_control_t { - uint8_t v_speed; // vehicle speed - uint8_t v_angle; // vehicle steering angle -} amp_serial_pkt_control_t; - -typedef struct amp_serial_pkt_dac_t { - uint8_t d_voltage; // voltage to set to the DAC -} amp_serial_pkt_dac_t; - -typedef struct amp_serial_pkt_pwm_t { - uint16_t p_freq; // frequency to set to the PWM -} amp_serial_pkt_pwm_t; - -typedef struct amp_serial_pkt_kill_t { - uint8_t k_flag; // whether or not to kill the kart -} amp_serial_pkt_kill_t; - - -/* - * Taken from libserialport.h to configure the serial port in a more - * concise manner. - */ -struct sp_port_config { - int baudrate; // Baudrate of the Serial Port - int bits; // Bits to Send per Byte - enum sp_parity parity; // Parity Checking of the Port - int stopbits; // Number of Stopbits - enum sp_rts rts; - enum sp_cts cts; - enum sp_dtr dtr; - enum sp_dsr dsr; - enum sp_xonxoff xon_xoff; -}; - -/* - * Structure of the AMP serial packet. The size of the packet is limited - * to 0xFF bytes. All packets must start with the hex 0x02 & 0x03 respectively. - */ -typedef struct amp_serial_pkt_t { - amp_serial_pkt_id_t id; // packet id - uint8_t size; // packet size - uint8_t msg[AMP_SERIAL_MAX_PKT_SIZE]; // contains the data of the packet - uint8_t crc; // cyclical redundancy check (2's complement) - uint32_t timeout; // timeout for receiving the packet -} amp_serial_pkt_t; - - -// FUNCTION DECLARATIONS -------------------------------------------------- -amp_err_code_t amp_serial_jetson_initialize(); - -void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config); - -void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config); - -//void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg); - -//void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg); - -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); - -amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte); - -amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes); - -uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf); - -amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte); - -int float_to_int(float max, float min, float num); - -void amp_serial_jetson_enable_kart(); - -void amp_serial_jetson_enable_drive(); - -void amp_serial_jetson_enable_default(); - -int check(enum sp_return result, amp_err_code_t amp_err); - -void end_program(amp_err_code_t amp_err); - -const char *parity_name(enum sp_parity parity); - -#endif /* SRC_AMP_SERIAL_H_ */ 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 deleted file mode 100755 index cf6b331..0000000 Binary files a/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST and /dev/null differ 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 deleted file mode 100644 index a96f796..0000000 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_ECU_TEST.cpp +++ /dev/null @@ -1,721 +0,0 @@ -/* - * amp_serial_jetson.cpp - * - * Created on: Apr 18, 2019 - * Author: David Pimley - */ - -// Standard Defines -#include -#include -#include -#include -#include - -// ROS Defines -//#include "ros/ros.h" -//#include "geometry_msgs/Twist.h" - -// External Libraries -#include - -// User Defined Libraries / Headers -#include "amp_err.h" -#include "amp_serial_jetson.h" - -using namespace std; - -// Global Variables Regarding the Serial Port -const char* port_name = "/dev/ttyACM0"; // Name of the Serial Port -amp_serial_state_t port_state = AMP_SERIAL_STATE_IDLE; // Current State of the Serial Port -struct sp_port * port = NULL; // Serial Port Handle -struct sp_port_config config; // Configuration of the Serial Port - -// Global Variables Concerning State of Control -amp_control_state_t amp_control_state = AMP_CONTROL_AUTONOMOUS; - -//Debug Intialization -#if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) -FILE * fptr1 = fopen("debug.txt", "w"); -#endif - -#ifdef DEBUG_TX -FILE * fptr2 = fopen("debug_tx.txt", "w"); -#endif - -#ifdef DEBUG_RX -FILE * fptr3 = fopen("debug_rx.txt", "w"); -#endif - -int main(int argc, char** argv) { - - int size; - - // Global Configuration Parameters - config.baudrate = AMP_SERIAL_CONFIG_BAUD; - config.bits = AMP_SERIAL_CONFIG_BITS; - config.parity = AMP_SERIAL_CONFIG_PARY; - config.stopbits = AMP_SERIAL_CONFIG_STOP; - config.cts = AMP_SERIAL_CONFIG_CTS; - config.dsr = AMP_SERIAL_CONFIG_DSR; - config.dtr = AMP_SERIAL_CONFIG_DTR; - config.rts = AMP_SERIAL_CONFIG_RTS; - config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - // Initialize the Serial Port - amp_serial_jetson_initialize(); - - //amp_serial_jetson_enable_default(); - - // Set the kart to the enable state - amp_serial_jetson_enable_kart(); - - // Set the kart to the drive state - //amp_serial_jetson_enable_drive(); - - while(true) { - // Declare & Initialize Local Variables - amp_serial_pkt_t s_pkt; // Full Serial Packet - amp_serial_pkt_control_t c_pkt; // Control Data Packet - - - 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, 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; - s_pkt.size = 2; - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - #ifdef DEBUG - fprintf(fptr1, "Sending Packet...\n"); - #endif - sleep(0.5); - amp_serial_jetson_tx_pkt(&s_pkt, &size); - #ifdef DEBUG - fprintf(fptr1, "Receiving Packet...\n"); - #endif - sleep(0.5); - //amp_serial_jetson_rx_pkt(&s_pkt, size); - } - sleep(1); - } - } - - #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) - fclose(fptr1); - #endif - - #ifdef DEBUG_TX - fclose(fptr2); - #endif - - #ifdef DEBUG_RX - fclose(fptr3); - #endif - - return EXIT_SUCCESS; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) - * - * initializes any given port - */ - amp_err_code_t amp_serial_jetson_initialize() { - // Declare & Initialize Local Variables - - #ifdef DEBUG - fprintf(fptr1, "Initializing Serial Protocol...\n"); - fprintf(fptr1, "Looking for port %s.\n", port_name); - #endif - // Get the Port Handle by the Passed Name - check(sp_get_port_by_name(port_name, &port), AMP_SERIAL_ERROR_INIT); - - // Display some basic information about the port. - #ifdef DEBUG - fprintf(fptr1, "Port name: %s\n", sp_get_port_name(port)); - fprintf(fptr1, "Description: %s\n", sp_get_port_description(port)); - - // Open the Serial Port based on the Previous Handle - fprintf(fptr1, "Opening port...\n"); - #endif - check(sp_open(port, SP_MODE_READ_WRITE), AMP_SERIAL_ERROR_INIT); - - #ifdef DEBUG - fprintf(fptr1, "Setting configurations...\n"); - #endif - amp_serial_jetson_config_port(port, config); - - #ifdef DEBUG - fprintf(fptr1, "Checking configurations...\n"); - #endif - amp_serial_jetson_check_port(port, config); - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) - * - * writes the configurations for any given port - */ -void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) { - - // Set the Configuration Parameters - _config.baudrate = AMP_SERIAL_CONFIG_BAUD; - _config.bits = AMP_SERIAL_CONFIG_BITS; - _config.parity = AMP_SERIAL_CONFIG_PARY; - _config.stopbits = AMP_SERIAL_CONFIG_STOP; - _config.cts = AMP_SERIAL_CONFIG_CTS; - _config.dsr = AMP_SERIAL_CONFIG_DSR; - _config.dtr = AMP_SERIAL_CONFIG_DTR; - _config.rts = AMP_SERIAL_CONFIG_RTS; - _config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - check(sp_set_config(_port, &_config), AMP_SERIAL_ERROR_INIT); - -} - -/* - * FUNCTION: - * - * vvoid amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) - * - * checks the configurations for any given port - */ -void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) { - - int baudrate; - int bits; - int stopbits; - struct sp_port_config * check_config; - enum sp_parity parity; - - check(sp_new_config(&check_config), AMP_SERIAL_ERROR_INIT); - check(sp_get_config(_port, check_config), AMP_SERIAL_ERROR_INIT); - - check(sp_get_config_baudrate(check_config, &baudrate), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_bits(check_config, &bits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_stopbits(check_config, &stopbits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_parity(check_config, &parity), AMP_SERIAL_ERROR_INIT); - sp_free_config(check_config); - - #ifdef DEBUG - fprintf(fptr1, "Baudrate: %d, data bits: %d, parity: %s, stop bits: %d\n", baudrate, bits, parity_name(parity), stopbits); - fprintf(fptr1, "Successfully opened port (%s) at %d\n", port_name , baudrate); - - if(_config.baudrate != baudrate) - { - fprintf(fptr1, "Baudrate mismatch\n"); - } - if(_config.bits != bits) - { - fprintf(fptr1, "Bits mismatch\n"); - } - if(_config.parity != parity) - { - fprintf(fptr1, "Parity mismatch\n"); - } - if(_config.stopbits != stopbits) - { - fprintf(fptr1, "Stopbit mismatch\n"); - } - #endif -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) - * - * takes in an arbitrary packet and sends the data from the - * initialized serial port. - */ -amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) { - // Declare & Initialize Local Variables - uint8_t s_data[AMP_SERIAL_MAX_PKT_SIZE] = {0}; // Contains Raw Data of Packet - uint8_t * s_buf; - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - - #ifdef DEBUG_TX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr2, "Waiting...\n"); - #endif - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - amp_serial_jetson_build_packet(pkt, s_data); - s_buf = (s_data + sizeof(uint8_t)); - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_buf, s_data[0] * sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - *size = s_data[0]; - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - int i; - for(i = 0; i < s_data[0]; i++) { - fprintf(fptr1, "tx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr2, "tx[%d]: %u\n", i, s_buf[i]); - } - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) - * - * builds the data buffer from an arbitrary packet for transmission - */ -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 = 2; // Used to calculate the current CRC - int i; - - // Start Byte - s_data[s_pos++] = (uint8_t)AMP_SERIAL_START_PKT; - - // ID Byte - s_data[s_pos++] = (uint8_t)pkt->id; - c_crc += pkt->id & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size Byte - if(pkt->size > 0) - { - s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; - c_crc += (0xE0 + (uint8_t)pkt->size) & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif - - // Data Byte - for (i = 0; i < pkt->size; i++) { - s_data[s_pos++] = (uint8_t)pkt->msg[i]; - c_crc += pkt->msg[i] & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - } - - // Send the CRC of the Previous Packet - pkt->crc = c_crc; - s_data[s_pos++] = (uint8_t)pkt->crc; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - // Finish off by sending the ETX - s_data[s_pos] = (uint8_t)AMP_SERIAL_STOP_PKT; - - s_data[0] = s_pos; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) - * - * byte by byte transmission for debugging - */ -amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) { - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_byte, sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - fprintf(fptr1, "tx: %u\n", *s_byte); - fprintf(fptr2, "tx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) - * - * receiver for packets from arduino - */ -amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { - // Declare & Initialize Local Variables - uint8_t c_crc; // Used to calculate the current CRC - uint8_t * s_buf = static_cast(malloc(bytes)); // Used for single byte reads i.e. STX, ETX - - - // Verify Input Parameters and the Serial Port - if (pkt == NULL) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: Unable to fill out packet with NULL pointer\n"); - fprintf(fptr3, "ERROR: Unable to fill out packet with NULL pointer\n"); - #endif - return AMP_SERIAL_ERROR_RX; - } - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - port_state = AMP_SERIAL_STATE_RX; - - // Read in the data - check(sp_nonblocking_read(port, s_buf, sizeof(uint8) * bytes), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - // Verify that the input has no errors - if (s_buf[0] != AMP_SERIAL_START_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Start Byte Found\n"); - fprintf(fptr3, "ERROR: No Start Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_START; - } - - if (s_buf[bytes-1] != AMP_SERIAL_STOP_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Stop Byte Found\n"); - fprintf(fptr3, "ERROR: No Stop Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_STOP; - } - - c_crc = amp_serial_jetson_rebuild_packet(pkt, s_buf); - if(c_crc != pkt->crc) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: CRC Mismatch\n"); - fprintf(fptr3, "ERROR: CRC Mismatch\n"); - #endif - return AMP_SERIAL_ERROR_CRC; - } - - #ifdef DEBUG_RX - int i; - for(i = 0; i < bytes; i++) - { - fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); - } - #endif - - // Free all allocated space - free(s_buf); - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) - * - * rebuilds packet from received buffer and returns crc for confirmation - */ -uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) { - - int i; - uint8_t c_crc = 0; - // ID from the Serial Port - pkt->id = (amp_serial_pkt_id_t)s_buf[1]; - c_crc += pkt->id & 0xFF; - #ifdef DEBUG_RX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size from the Serial Port - pkt->size = s_buf[2]; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (uint8_t)pkt->size); - fprintf(fptr2, "size: %u\n", (uint8_t)pkt->size); - #endif - - // Data from the Incoming Packet - for (i = 0; i < pkt->size; i++) - { - pkt->msg[i] = s_buf[i+3]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - - // CRC from the Incoming Packet - pkt->crc = s_buf[i+3]; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - return c_crc; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) - * - * byte by byte reciever for debugging - */ -amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) { - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - // Read in the data - check(sp_nonblocking_read(port, s_byte, sizeof(uint8)), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_RX - fprintf(fptr1, "rx: %u\n", *s_byte); - fprintf(fptr3, "rx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* FUNCTION: - * - * int float_to_int(float, max, float min, float num) - * - * returns converted int from 0 to 255 - */ -int float_to_int(float max, float min, float num) -{ - int val; - - val = (int)roundf((num-min)/(max-min)*255); - - return val; -} - -/* - * FUNCTION: - * - * const char *parity_name(enum sp_parity parity) - * - * returns the parity state of the buffer - */ -const char *parity_name(enum sp_parity parity) -{ - switch (parity) { - case SP_PARITY_INVALID: - return "(Invalid)"; - case SP_PARITY_NONE: - return "None"; - case SP_PARITY_ODD: - return "Odd"; - case SP_PARITY_EVEN: - return "Even"; - case SP_PARITY_MARK: - return "Mark"; - case SP_PARITY_SPACE: - return "Space"; - default: - return NULL; - } -} - -/* - * FUNCTION: - * - * void end_program(amp_err_code_t amp_err) - * - * clean termination program for handling serial comm errors - */ -void end_program(amp_err_code_t amp_err) -{ - /* Free any structures we allocated. */ - if (&config != NULL) - sp_free_config(&config); - if (port != NULL) - sp_free_port(port); - - /* Exit with the given return code. */ - exit(amp_err); -} - -/* - * FUNCTION: - * - * int check(enum sp_return result, amp_err_code_t amp_err) - * - * error handling function for serial communication - */ -int check(enum sp_return result, amp_err_code_t amp_err) -{ - int error_code; - char *error_message; - - switch (result) { - - case SP_ERR_ARG: - /* When SP_ERR_ARG is returned, there was a problem with one - * or more of the arguments passed to the function, e.g. a null - * pointer or an invalid value. This generally implies a bug in - * the calling code. */ - printf("Error: Invalid argument.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Invalid argument.\n"); - #endif - end_program(amp_err); - - case SP_ERR_FAIL: - /* When SP_ERR_FAIL is returned, there was an error from the OS, - * which we can obtain the error code and message for. These - * calls must be made in the same thread as the call that - * returned SP_ERR_FAIL, and before any other system functions - * are called in that thread, or they may not return the - * correct results. */ - error_code = sp_last_error_code(); - error_message = sp_last_error_message(); - printf("Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #ifdef DEBUG - fprintf(fptr1, "Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #endif - /* The error message should be freed after use. */ - sp_free_error_message(error_message); - end_program(amp_err); - - case SP_ERR_SUPP: - /* When SP_ERR_SUPP is returned, the function was asked to do - * something that isn't supported by the current OS or device, - * or that libserialport doesn't know how to do in the current - * version. */ - printf("Error: Not supported.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Not supported.\n"); - #endif - end_program(amp_err); - - case SP_ERR_MEM: - /* When SP_ERR_MEM is returned, libserialport wasn't able to - * allocate some memory it needed. Since the library doesn't - * normally use any large data structures, this probably means - * the system is critically low on memory and recovery will - * require very careful handling. The library itself will - * always try to handle any allocation failure safely. */ - end_program(amp_err); - - case SP_OK: - default: - #ifdef DEBUG - fprintf(fptr1, "Operation succeeded.\n"); - #endif - return result; - } -} - -void amp_serial_jetson_enable_kart() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Enabling Kart through Packet ID\n"); - #endif - - // Enable the kart - t_pkt.id = AMP_SERIAL_ENABLE; - t_pkt.size = 0; - size = 0; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_drive() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Drive Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DRIVE; - t_pkt.size = 0; - size = 0; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_default() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Default Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DEFAULT; - t_pkt.size = 0; - size = 0; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} diff --git a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST b/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST deleted file mode 100755 index 95c1325..0000000 Binary files a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST and /dev/null differ 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 deleted file mode 100644 index e0dac89..0000000 --- a/src/ti_comm/src/new_lib/amp_serial_jetson_SERIAL_TEST.cpp +++ /dev/null @@ -1,792 +0,0 @@ -/* - * amp_serial_jetson_SERIAL_TEST.cpp - * Used for testing/debugging serial communications between the jetson and the hardware - */ - -// Standard Defines -#include -#include -#include -#include -#include - - -// ROS Defines -//#include "ros/ros.h" -//#include "geometry_msgs/Twist.h" - -// External Libraries -#include - -// User Defined Libraries / Headers -#include "amp_err.h" -#include "amp_serial_jetson.h" - -using namespace std; - -// Global Variables Regarding the Serial Port -const char* port_name = "/dev/ttyACM0"; // Name of the Serial Port -amp_serial_state_t port_state = AMP_SERIAL_STATE_IDLE; // Current State of the Serial Port -struct sp_port * port = NULL; // Serial Port Handle -struct sp_port_config config; // Configuration of the Serial Port - -// 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"); -#endif - -#ifdef DEBUG_TX -FILE * fptr2 = fopen("debug_tx.txt", "w"); -#endif - -#ifdef DEBUG_RX -FILE * fptr3 = fopen("debug_rx.txt", "w"); -#endif - -int main(int argc, char** argv) { - // Command pair structured as: - queue > commands; - // File to read debug commands from - string filename = "debug.txt"; - // Default delay value of 0.05 - float delay = 0.05; - float speed; - float angle; - - 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 - while (fin >> speed && fin >> angle) { - 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}; - //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; - config.bits = AMP_SERIAL_CONFIG_BITS; - config.parity = AMP_SERIAL_CONFIG_PARY; - config.stopbits = AMP_SERIAL_CONFIG_STOP; - config.cts = AMP_SERIAL_CONFIG_CTS; - config.dsr = AMP_SERIAL_CONFIG_DSR; - config.dtr = AMP_SERIAL_CONFIG_DTR; - config.rts = AMP_SERIAL_CONFIG_RTS; - config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - // Initialize the Serial Port - amp_serial_jetson_initialize(); - - //amp_serial_jetson_enable_default(); - - // Set the kart to the enable state - amp_serial_jetson_enable_kart(); - - // Set the kart to the drive state - //amp_serial_jetson_enable_drive(); - - /*char s_data[7]; - uint8_t s_pos = 7; - s_data[0] = '4'; - s_data[1] = '0'; - s_data[2] = '0'; - s_data[3] = '0'; - s_data[4] = 'd'; - s_data[5] = '\0'; - 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(!commands.empty()) { - // Get speed and angle for current command - 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); //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; - s_pkt.size = 2; - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - #ifdef DEBUG - fprintf(fptr1, "Sending Packet...\n"); - #endif - sleep(delay); - amp_serial_jetson_tx_pkt(&s_pkt, &size); - #ifdef DEBUG - fprintf(fptr1, "Receiving Packet...\n"); - #endif - sleep(delay); - //amp_serial_jetson_rx_pkt(&s_pkt, size); - i++; - } - - #if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) - fclose(fptr1); - #endif - - #ifdef DEBUG_TX - fclose(fptr2); - #endif - - #ifdef DEBUG_RX - fclose(fptr3); - #endif - - return EXIT_SUCCESS; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) - * - * initializes any given port - */ - amp_err_code_t amp_serial_jetson_initialize() { - // Declare & Initialize Local Variables - - #ifdef DEBUG - fprintf(fptr1, "Initializing Serial Protocol...\n"); - fprintf(fptr1, "Looking for port %s.\n", port_name); - #endif - // Get the Port Handle by the Passed Name - check(sp_get_port_by_name(port_name, &port), AMP_SERIAL_ERROR_INIT); - - // Display some basic information about the port. - #ifdef DEBUG - fprintf(fptr1, "Port name: %s\n", sp_get_port_name(port)); - fprintf(fptr1, "Description: %s\n", sp_get_port_description(port)); - - // Open the Serial Port based on the Previous Handle - fprintf(fptr1, "Opening port...\n"); - #endif - check(sp_open(port, SP_MODE_READ_WRITE), AMP_SERIAL_ERROR_INIT); - - #ifdef DEBUG - fprintf(fptr1, "Setting configurations...\n"); - #endif - amp_serial_jetson_config_port(port, config); - - #ifdef DEBUG - fprintf(fptr1, "Checking configurations...\n"); - #endif - amp_serial_jetson_check_port(port, config); - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) - * - * writes the configurations for any given port - */ -void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) { - - // Set the Configuration Parameters - _config.baudrate = AMP_SERIAL_CONFIG_BAUD; - _config.bits = AMP_SERIAL_CONFIG_BITS; - _config.parity = AMP_SERIAL_CONFIG_PARY; - _config.stopbits = AMP_SERIAL_CONFIG_STOP; - _config.cts = AMP_SERIAL_CONFIG_CTS; - _config.dsr = AMP_SERIAL_CONFIG_DSR; - _config.dtr = AMP_SERIAL_CONFIG_DTR; - _config.rts = AMP_SERIAL_CONFIG_RTS; - _config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - check(sp_set_config(_port, &_config), AMP_SERIAL_ERROR_INIT); - -} - -/* - * FUNCTION: - * - * vvoid amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) - * - * checks the configurations for any given port - */ -void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) { - - int baudrate; - int bits; - int stopbits; - struct sp_port_config * check_config; - enum sp_parity parity; - - check(sp_new_config(&check_config), AMP_SERIAL_ERROR_INIT); - check(sp_get_config(_port, check_config), AMP_SERIAL_ERROR_INIT); - - check(sp_get_config_baudrate(check_config, &baudrate), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_bits(check_config, &bits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_stopbits(check_config, &stopbits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_parity(check_config, &parity), AMP_SERIAL_ERROR_INIT); - sp_free_config(check_config); - - #ifdef DEBUG - fprintf(fptr1, "Baudrate: %d, data bits: %d, parity: %s, stop bits: %d\n", baudrate, bits, parity_name(parity), stopbits); - fprintf(fptr1, "Successfully opened port (%s) at %d\n", port_name , baudrate); - - if(_config.baudrate != baudrate) - { - fprintf(fptr1, "Baudrate mismatch\n"); - } - if(_config.bits != bits) - { - fprintf(fptr1, "Bits mismatch\n"); - } - if(_config.parity != parity) - { - fprintf(fptr1, "Parity mismatch\n"); - } - if(_config.stopbits != stopbits) - { - fprintf(fptr1, "Stopbit mismatch\n"); - } - #endif -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) - * - * takes in an arbitrary packet and sends the data from the - * initialized serial port. - */ -amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) { - // Declare & Initialize Local Variables - uint8_t s_data[AMP_SERIAL_MAX_PKT_SIZE] = {0}; // Contains Raw Data of Packet - uint8_t * s_buf; - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - - #ifdef DEBUG_TX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr2, "Waiting...\n"); - #endif - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - amp_serial_jetson_build_packet(pkt, s_data); - s_buf = (s_data + sizeof(uint8_t)); - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_buf, s_data[0] * sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - *size = s_data[0]; - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - int i; - for(i = 0; i < s_data[0]; i++) { - fprintf(fptr1, "tx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr2, "tx[%d]: %u\n", i, s_buf[i]); - } - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) - * - * builds the data buffer from an arbitrary packet for transmission - */ -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 - int i; - - // Start Byte - s_data[s_pos++] = (uint8_t)AMP_SERIAL_START_PKT; - - // ID Byte - s_data[s_pos++] = (uint8_t)pkt->id; - c_crc += pkt->id & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size Byte - if(pkt->size > 0) - { - s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; - c_crc += (0xE0 + (uint8_t)pkt->size) & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif - - // Data Byte - for (i = 0; i < pkt->size; i++) { - s_data[s_pos++] = (uint8_t)pkt->msg[i]; - c_crc += pkt->msg[i] & 0xFF; - - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - } - - // Send the CRC of the Previous Packet - pkt->crc = c_crc; - s_data[s_pos++] = (uint8_t)pkt->crc; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - // Finish off by sending the ETX - s_data[s_pos] = (uint8_t)AMP_SERIAL_STOP_PKT; - - s_data[0] = s_pos; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) - * - * byte by byte transmission for debugging - */ -amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) { - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_byte, sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - fprintf(fptr1, "tx: %u\n", *s_byte); - fprintf(fptr2, "tx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) - * - * receiver for packets from arduino - */ -amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { - // Declare & Initialize Local Variables - uint8_t c_crc; // Used to calculate the current CRC - uint8_t * s_buf = static_cast(malloc(bytes)); // Used for single byte reads i.e. STX, ETX - - - // Verify Input Parameters and the Serial Port - if (pkt == NULL) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: Unable to fill out packet with NULL pointer\n"); - fprintf(fptr3, "ERROR: Unable to fill out packet with NULL pointer\n"); - #endif - return AMP_SERIAL_ERROR_RX; - } - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - port_state = AMP_SERIAL_STATE_RX; - - // Read in the data - check(sp_nonblocking_read(port, s_buf, sizeof(uint8) * bytes), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - // Verify that the input has no errors - if (s_buf[0] != AMP_SERIAL_START_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Start Byte Found\n"); - fprintf(fptr3, "ERROR: No Start Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_START; - } - - if (s_buf[bytes-1] != AMP_SERIAL_STOP_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Stop Byte Found\n"); - fprintf(fptr3, "ERROR: No Stop Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_STOP; - } - - c_crc = amp_serial_jetson_rebuild_packet(pkt, s_buf); - if(c_crc != pkt->crc) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: CRC Mismatch\n"); - fprintf(fptr3, "ERROR: CRC Mismatch\n"); - #endif - return AMP_SERIAL_ERROR_CRC; - } - - #ifdef DEBUG_RX - int i; - for(i = 0; i < bytes; i++) - { - fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); - } - #endif - - // Free all allocated space - free(s_buf); - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) - * - * rebuilds packet from received buffer and returns crc for confirmation - */ -uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) { - - int i; - uint8_t c_crc = 0; - // ID from the Serial Port - pkt->id = (amp_serial_pkt_id_t)s_buf[1]; - c_crc += pkt->id & 0xFF; - #ifdef DEBUG_RX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size from the Serial Port - pkt->size = s_buf[2]; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (uint8_t)pkt->size); - fprintf(fptr2, "size: %u\n", (uint8_t)pkt->size); - #endif - - // Data from the Incoming Packet - for (i = 0; i < pkt->size; i++) - { - pkt->msg[i] = s_buf[i+3]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - - // CRC from the Incoming Packet - pkt->crc = s_buf[i+3]; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - return c_crc; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) - * - * byte by byte reciever for debugging - */ -amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) { - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - // Read in the data - check(sp_nonblocking_read(port, s_byte, sizeof(uint8)), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_RX - fprintf(fptr1, "rx: %u\n", *s_byte); - fprintf(fptr3, "rx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* FUNCTION: - * - * int float_to_int(float, max, float min, float num) - * - * returns converted int from 0 to 255 - */ -int float_to_int(float max, float min, float num) -{ - int val; - - val = (int)roundf((num-min)/(max-min)*255); - - return val; -} - -/* - * FUNCTION: - * - * const char *parity_name(enum sp_parity parity) - * - * returns the parity state of the buffer - */ -const char *parity_name(enum sp_parity parity) -{ - switch (parity) { - case SP_PARITY_INVALID: - return "(Invalid)"; - case SP_PARITY_NONE: - return "None"; - case SP_PARITY_ODD: - return "Odd"; - case SP_PARITY_EVEN: - return "Even"; - case SP_PARITY_MARK: - return "Mark"; - case SP_PARITY_SPACE: - return "Space"; - default: - return NULL; - } -} - -/* - * FUNCTION: - * - * void end_program(amp_err_code_t amp_err) - * - * clean termination program for handling serial comm errors - */ -void end_program(amp_err_code_t amp_err) -{ - /* Free any structures we allocated. */ - if (&config != NULL) - sp_free_config(&config); - if (port != NULL) - sp_free_port(port); - - /* Exit with the given return code. */ - exit(amp_err); -} - -/* - * FUNCTION: - * - * int check(enum sp_return result, amp_err_code_t amp_err) - * - * error handling function for serial communication - */ -int check(enum sp_return result, amp_err_code_t amp_err) -{ - int error_code; - char *error_message; - - switch (result) { - - case SP_ERR_ARG: - /* When SP_ERR_ARG is returned, there was a problem with one - * or more of the arguments passed to the function, e.g. a null - * pointer or an invalid value. This generally implies a bug in - * the calling code. */ - printf("Error: Invalid argument.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Invalid argument.\n"); - #endif - end_program(amp_err); - - case SP_ERR_FAIL: - /* When SP_ERR_FAIL is returned, there was an error from the OS, - * which we can obtain the error code and message for. These - * calls must be made in the same thread as the call that - * returned SP_ERR_FAIL, and before any other system functions - * are called in that thread, or they may not return the - * correct results. */ - error_code = sp_last_error_code(); - error_message = sp_last_error_message(); - printf("Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #ifdef DEBUG - fprintf(fptr1, "Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #endif - /* The error message should be freed after use. */ - sp_free_error_message(error_message); - end_program(amp_err); - - case SP_ERR_SUPP: - /* When SP_ERR_SUPP is returned, the function was asked to do - * something that isn't supported by the current OS or device, - * or that libserialport doesn't know how to do in the current - * version. */ - printf("Error: Not supported.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Not supported.\n"); - #endif - end_program(amp_err); - - case SP_ERR_MEM: - /* When SP_ERR_MEM is returned, libserialport wasn't able to - * allocate some memory it needed. Since the library doesn't - * normally use any large data structures, this probably means - * the system is critically low on memory and recovery will - * require very careful handling. The library itself will - * always try to handle any allocation failure safely. */ - end_program(amp_err); - - case SP_OK: - default: - #ifdef DEBUG - fprintf(fptr1, "Operation succeeded.\n"); - #endif - return result; - } -} - -void amp_serial_jetson_enable_kart() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Enabling Kart through Packet ID\n"); - #endif - - // Enable the kart - t_pkt.id = AMP_SERIAL_ENABLE; - t_pkt.size = 0; - size = 0; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_drive() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Drive Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DRIVE; - t_pkt.size = 0; - size = 0; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_default() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Default Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DEFAULT; - t_pkt.size = 0; - size = 0; - - 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 << " 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") { - delay = stof(*++i); - } - } -} - diff --git a/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.cpp b/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.cpp deleted file mode 100644 index 5f1a646..0000000 --- a/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.cpp +++ /dev/null @@ -1,740 +0,0 @@ -/* - * amp_serial_jetson.cpp - * - * Created on: Apr 18, 2019 - * Author: David Pimley - */ - -// Standard Defines -#include -#include -#include -#include -#include - -// ROS Defines -#include "ros/ros.h" -#include "geometry_msgs/Twist.h" - -// External Libraries -#include - -// User Defined Libraries / Headers -#include "amp_err.h" -#include "amp_serial_jetson.h" - -using namespace std; - -// Global Variables Regarding the Serial Port -const char* port_name = "/dev/ttyACM0"; // Name of the Serial Port -amp_serial_state_t port_state = AMP_SERIAL_STATE_IDLE; // Current State of the Serial Port -struct sp_port * port = NULL; // Serial Port Handle -struct sp_port_config config; // Configuration of the Serial Port - -// Global Variables Concerning State of Control -amp_control_state_t amp_control_state = AMP_CONTROL_AUTONOMOUS; - -//Debug Intialization -#if defined(DEBUG) || defined(DEBUG_TX) || defined(DEBUG_RX) -FILE * fptr1 = fopen("debug.txt", "w"); -#endif - -#ifdef DEBUG_TX -FILE * fptr2 = fopen("debug_tx.txt", "w"); -#endif - -#ifdef DEBUG_RX -FILE * fptr3 = fopen("debug_rx.txt", "w"); -#endif - -void dummy_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg) { - ROS_INFO("cmd_vel speed in x dir: [%d]", (int)(msg->linear.x)); -} - -int main(int argc, char** argv) { - - // Global Configuration Parameters - config.baudrate = AMP_SERIAL_CONFIG_BAUD; - config.bits = AMP_SERIAL_CONFIG_BITS; - config.parity = AMP_SERIAL_CONFIG_PARY; - config.stopbits = AMP_SERIAL_CONFIG_STOP; - config.cts = AMP_SERIAL_CONFIG_CTS; - config.dsr = AMP_SERIAL_CONFIG_DSR; - config.dtr = AMP_SERIAL_CONFIG_DTR; - config.rts = AMP_SERIAL_CONFIG_RTS; - config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - // Initialize the Serial Port - amp_serial_jetson_initialize(port); - - //amp_serial_jetson_enable_default(); - - // Set the kart to the enable state - //amp_serial_jetson_enable_kart(); - - // Set the kart to the drive state - //amp_serial_jetson_enable_drive(); - - - // Start the ROS Node - ros::init(argc, argv, "cmd_vel_listener"); - - // Create a Handle and have it Subscribe to the Command Vel Messages - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("cmd_vel", 1000, cmd_vel_callback); - //ros::Subscriber sub = n.subscribe("cmd_vel", 10, key_cmd_callback); - // TODO(ihagedo): Replace call to the dummy callback with the real one once - // testing with MCU is complete. - //ros::Subscriber dummy_sub = n.subscribe("cmd_vel", 10, dummy_cmd_callback); - - // Spin as new Messages come in - ros::spin(); - - return EXIT_SUCCESS; -} - -void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // Declare & Initialize Local Variables - amp_serial_pkt_t s_pkt; // Full Serial Packet - amp_serial_pkt_control_t c_pkt; // Control Data Packet - int size; - - // Create Control Packet - c_pkt.v_speed = float_to_int(AMP_MAX_VEL, AMP_MIN_VEL, msg->linear.x); - c_pkt.v_angle = float_to_int(AMP_MAX_ANG, AMP_MIN_ANG, msg->angular.z); - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt, &size); - - printf("Sending Packet...\n"); - - return; -} - -void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { - // Declare & Initialize Local Variables - float translational_velocity = msg->linear.x; // Translational Velocity Command - float drive_angle = msg->angular.z; // Steering Angle Command - amp_serial_pkt_t s_pkt; // Full Serial Packet - amp_serial_pkt_control_t c_pkt; // Control Data Format - int size; - - // Check Current Status of the Car's Control (RC / Autonomous) - if (AMP_CONTROL_REMOTE == amp_control_state) { - return; - } - - // Create Control Packet - c_pkt.v_speed = translational_velocity; - c_pkt.v_angle = drive_angle; - - // Create Full Serial Packet - s_pkt.id = AMP_SERIAL_CONTROL; - s_pkt.size = sizeof(amp_serial_pkt_control_t); - - // Copy From the Control Packet to the Serial Packet - memcpy(s_pkt.msg, &c_pkt, sizeof(amp_serial_pkt_control_t)); - - // Send the Packet - amp_serial_jetson_tx_pkt(&s_pkt, &size); - - return; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) - * - * initializes any given port - */ - amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) { - // Declare & Initialize Local Variables - - #ifdef DEBUG - fprintf(fptr1, "Initializing Serial Protocol...\n"); - fprintf(fptr1, "Looking for port %s.\n", port_name); - #endif - // Get the Port Handle by the Passed Name - check(sp_get_port_by_name(port_name, &port), AMP_SERIAL_ERROR_INIT); - - // Display some basic information about the port. - #ifdef DEBUG - fprintf(fptr1, "Port name: %s\n", sp_get_port_name(port)); - fprintf(fptr1, "Description: %s\n", sp_get_port_description(port)); - - // Open the Serial Port based on the Previous Handle - fprintf(fptr1, "Opening port...\n"); - #endif - check(sp_open(port, SP_MODE_READ_WRITE), AMP_SERIAL_ERROR_INIT); - - #ifdef DEBUG - fprintf(fptr1, "Setting configurations...\n"); - #endif - amp_serial_jetson_config_port(port, config); - - #ifdef DEBUG - fprintf(fptr1, "Checking configurations...\n"); - #endif - amp_serial_jetson_check_port(port, config); - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) - * - * writes the configurations for any given port - */ -void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config) { - - // Set the Configuration Parameters - _config.baudrate = AMP_SERIAL_CONFIG_BAUD; - _config.bits = AMP_SERIAL_CONFIG_BITS; - _config.parity = AMP_SERIAL_CONFIG_PARY; - _config.stopbits = AMP_SERIAL_CONFIG_STOP; - _config.cts = AMP_SERIAL_CONFIG_CTS; - _config.dsr = AMP_SERIAL_CONFIG_DSR; - _config.dtr = AMP_SERIAL_CONFIG_DTR; - _config.rts = AMP_SERIAL_CONFIG_RTS; - _config.xon_xoff = AMP_SERIAL_CONFIG_XST; - - check(sp_set_config(_port, &_config), AMP_SERIAL_ERROR_INIT); - -} - -/* - * FUNCTION: - * - * vvoid amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) - * - * checks the configurations for any given port - */ -void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config) { - - int baudrate; - int bits; - int stopbits; - struct sp_port_config * check_config; - enum sp_parity parity; - - check(sp_new_config(&check_config), AMP_SERIAL_ERROR_INIT); - check(sp_get_config(_port, check_config), AMP_SERIAL_ERROR_INIT); - - check(sp_get_config_baudrate(check_config, &baudrate), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_bits(check_config, &bits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_stopbits(check_config, &stopbits), AMP_SERIAL_ERROR_INIT); - check(sp_get_config_parity(check_config, &parity), AMP_SERIAL_ERROR_INIT); - sp_free_config(check_config); - - #ifdef DEBUG - fprintf(fptr1, "Baudrate: %d, data bits: %d, parity: %s, stop bits: %d\n", baudrate, bits, parity_name(parity), stopbits); - fprintf(fptr1, "Successfully opened port (%s) at %d\n", port_name , baudrate); - - if(_config.baudrate != baudrate) - { - fprintf(fptr1, "Baudrate mismatch\n"); - } - if(_config.bits != bits) - { - fprintf(fptr1, "Bits mismatch\n"); - } - if(_config.parity != parity) - { - fprintf(fptr1, "Parity mismatch\n"); - } - if(_config.stopbits != stopbits) - { - fprintf(fptr1, "Stopbit mismatch\n"); - } - #endif -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) - * - * takes in an arbitrary packet and sends the data from the - * initialized serial port. - */ -amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size) { - // Declare & Initialize Local Variables - uint8_t s_data[AMP_SERIAL_MAX_PKT_SIZE] = {0}; // Contains Raw Data of Packet - uint8_t * s_buf; - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - - #ifdef DEBUG_TX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr2, "Waiting...\n"); - #endif - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - amp_serial_jetson_build_packet(pkt, s_data); - s_buf = (s_data + sizeof(uint8_t)); - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_buf, s_data[0] * sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - *size = s_data[0]; - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - int i; - for(i = 0; i < s_data[0]; i++) { - fprintf(fptr1, "tx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr2, "tx[%d]: %u\n", i, s_buf[i]); - } - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data) - * - * builds the data buffer from an arbitrary packet for transmission - */ -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 - int i; - - // Start Byte - s_data[s_pos++] = (uint8_t)AMP_SERIAL_START_PKT; - - // ID Byte - s_data[s_pos++] = (uint8_t)pkt->id; - c_crc += pkt->id & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size Byte - s_data[s_pos++] = 0xE0 + (uint8_t)pkt->size; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - fprintf(fptr2, "size: %u\n", (0xE0 + (uint8_t)pkt->size)); - #endif - - // Data Byte - for (i = 0; i < pkt->size; i++) { - s_data[s_pos++] = (uint8_t)pkt->msg[i]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - - // Send the CRC of the Previous Packet - pkt->crc = c_crc; - s_data[s_pos++] = (uint8_t)pkt->crc; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - // Finish off by sending the ETX - s_data[s_pos] = (uint8_t)AMP_SERIAL_STOP_PKT; - - s_data[0] = s_pos; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) - * - * byte by byte transmission for debugging - */ -amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte) { - - // Wait for data to be sent out of the port - while (port_state != AMP_SERIAL_STATE_IDLE || sp_output_waiting(port) > 0) { - } - - // Indicate that the Serial Port is now Transmitting - port_state = AMP_SERIAL_STATE_TX; - - // Send the Packet - check(sp_nonblocking_write(port, (const void *)s_byte, sizeof(uint8_t)), AMP_SERIAL_ERROR_TX); - - // Indicate that the Serial Port is now Free - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_TX - fprintf(fptr1, "tx: %u\n", *s_byte); - fprintf(fptr2, "tx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) - * - * receiver for packets from arduino - */ -amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes) { - // Declare & Initialize Local Variables - uint8_t c_crc; // Used to calculate the current CRC - uint8_t * s_buf = static_cast(malloc(bytes)); // Used for single byte reads i.e. STX, ETX - - - // Verify Input Parameters and the Serial Port - if (pkt == NULL) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: Unable to fill out packet with NULL pointer\n"); - fprintf(fptr3, "ERROR: Unable to fill out packet with NULL pointer\n"); - #endif - return AMP_SERIAL_ERROR_RX; - } - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - port_state = AMP_SERIAL_STATE_RX; - - // Read in the data - check(sp_nonblocking_read(port, s_buf, sizeof(uint8) * bytes), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - // Verify that the input has no errors - if (s_buf[0] != AMP_SERIAL_START_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Start Byte Found\n"); - fprintf(fptr3, "ERROR: No Start Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_START; - } - - if (s_buf[bytes-1] != AMP_SERIAL_STOP_PKT) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: No Stop Byte Found\n"); - fprintf(fptr3, "ERROR: No Stop Byte Found\n"); - #endif - return AMP_SERIAL_ERROR_RX_NO_STOP; - } - - c_crc = amp_serial_jetson_rebuild_packet(pkt, s_buf); - if(c_crc != pkt->crc) { - #ifdef DEBUG_RX - fprintf(fptr1, "ERROR: CRC Mismatch\n"); - fprintf(fptr3, "ERROR: CRC Mismatch\n"); - #endif - return AMP_SERIAL_ERROR_CRC; - } - - #ifdef DEBUG_RX - int i; - for(i = 0; i < bytes; i++) - { - fprintf(fptr1, "rx[%d]: %u\n", i, s_buf[i]); - fprintf(fptr3, "rx[%d]: %u\n", i, s_buf[i]); - } - #endif - - // Free all allocated space - free(s_buf); - return AMP_ERROR_NONE; -} - -/* - * FUNCTION: - * - * uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) - * - * rebuilds packet from received buffer and returns crc for confirmation - */ -uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf) { - - int i; - uint8_t c_crc = 0; - // ID from the Serial Port - pkt->id = (amp_serial_pkt_id_t)s_buf[1]; - c_crc += pkt->id & 0xFF; - #ifdef DEBUG_RX - fprintf(fptr1, "Packet contents\n"); - fprintf(fptr2, "Packet contents\n"); - fprintf(fptr1, "id: %u\n", (uint8_t)pkt->id); - fprintf(fptr2, "id: %u\n", (uint8_t)pkt->id); - #endif - - // Size from the Serial Port - pkt->size = s_buf[2]; - c_crc += pkt->size & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "size: %u\n", (uint8_t)pkt->size); - fprintf(fptr2, "size: %u\n", (uint8_t)pkt->size); - #endif - - // Data from the Incoming Packet - for (i = 0; i < pkt->size; i++) - { - pkt->msg[i] = s_buf[i+3]; - c_crc += pkt->msg[i] & 0xFF; - #ifdef DEBUG_TX - fprintf(fptr1, "msg: %u\n", (uint8_t)pkt->msg[i]); - fprintf(fptr2, "msg: %u\n", (uint8_t)pkt->msg[i]); - #endif - } - - // CRC from the Incoming Packet - pkt->crc = s_buf[i+3]; - #ifdef DEBUG_TX - fprintf(fptr1, "crc: %u\n", (uint8_t)pkt->crc); - fprintf(fptr2, "crc: %u\n", (uint8_t)pkt->crc); - #endif - - return c_crc; -} - -/* - * FUNCTION: - * - * amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) - * - * byte by byte reciever for debugging - */ -amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte) { - - // Verify the Current State of the Serial Port - while (port_state != AMP_SERIAL_STATE_IDLE) { - } - - // Verify that there is Data to read - while(sp_input_waiting(port) == 0) { - #ifdef DEBUG_RX - fprintf(fptr1, "Waiting...\n"); - fprintf(fptr3, "Waiting...\n"); - #endif - } - - // Read in the data - check(sp_nonblocking_read(port, s_byte, sizeof(uint8)), AMP_SERIAL_ERROR_RX); - - port_state = AMP_SERIAL_STATE_IDLE; - - #ifdef DEBUG_RX - fprintf(fptr1, "rx: %u\n", *s_byte); - fprintf(fptr3, "rx: %u\n", *s_byte); - #endif - - return AMP_ERROR_NONE; -} - -/* FUNCTION: - * - * int float_to_int(float, max, float min, float num) - * - * returns converted int from 0 to 255 - */ -int float_to_int(float max, float min, float num) -{ - int val; - - val = (int)roundf((num-min)/(max-min)*255); - - return val; -} - -/* - * FUNCTION: - * - * const char *parity_name(enum sp_parity parity) - * - * returns the parity state of the buffer - */ -const char *parity_name(enum sp_parity parity) -{ - switch (parity) { - case SP_PARITY_INVALID: - return "(Invalid)"; - case SP_PARITY_NONE: - return "None"; - case SP_PARITY_ODD: - return "Odd"; - case SP_PARITY_EVEN: - return "Even"; - case SP_PARITY_MARK: - return "Mark"; - case SP_PARITY_SPACE: - return "Space"; - default: - return NULL; - } -} - -/* - * FUNCTION: - * - * void end_program(amp_err_code_t amp_err) - * - * clean termination program for handling serial comm errors - */ -void end_program(amp_err_code_t amp_err) -{ - /* Free any structures we allocated. */ - if (&config != NULL) - sp_free_config(&config); - if (port != NULL) - sp_free_port(port); - - /* Exit with the given return code. */ - exit(amp_err); -} - -/* - * FUNCTION: - * - * int check(enum sp_return result, amp_err_code_t amp_err) - * - * error handling function for serial communication - */ -int check(enum sp_return result, amp_err_code_t amp_err) -{ - int error_code; - char *error_message; - - switch (result) { - - case SP_ERR_ARG: - /* When SP_ERR_ARG is returned, there was a problem with one - * or more of the arguments passed to the function, e.g. a null - * pointer or an invalid value. This generally implies a bug in - * the calling code. */ - printf("Error: Invalid argument.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Invalid argument.\n"); - #endif - end_program(amp_err); - - case SP_ERR_FAIL: - /* When SP_ERR_FAIL is returned, there was an error from the OS, - * which we can obtain the error code and message for. These - * calls must be made in the same thread as the call that - * returned SP_ERR_FAIL, and before any other system functions - * are called in that thread, or they may not return the - * correct results. */ - error_code = sp_last_error_code(); - error_message = sp_last_error_message(); - printf("Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #ifdef DEBUG - fprintf(fptr1, "Error: Failed: OS error code: %d, message: '%s'\n", error_code, error_message); - #endif - /* The error message should be freed after use. */ - sp_free_error_message(error_message); - end_program(amp_err); - - case SP_ERR_SUPP: - /* When SP_ERR_SUPP is returned, the function was asked to do - * something that isn't supported by the current OS or device, - * or that libserialport doesn't know how to do in the current - * version. */ - printf("Error: Not supported.\n"); - #ifdef DEBUG - fprintf(fptr1, "Error: Not supported.\n"); - #endif - end_program(amp_err); - - case SP_ERR_MEM: - /* When SP_ERR_MEM is returned, libserialport wasn't able to - * allocate some memory it needed. Since the library doesn't - * normally use any large data structures, this probably means - * the system is critically low on memory and recovery will - * require very careful handling. The library itself will - * always try to handle any allocation failure safely. */ - end_program(amp_err); - - case SP_OK: - default: - #ifdef DEBUG - fprintf(fptr1, "Operation succeeded.\n"); - #endif - return result; - } -} - -void amp_serial_jetson_enable_kart() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Enabling Kart through Packet ID\n"); - #endif - - // Enable the kart - t_pkt.id = AMP_SERIAL_ENABLE; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_drive() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Drive Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DRIVE; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} - -void amp_serial_jetson_enable_default() { - // Declare & Initialize Local Variables - amp_serial_pkt_t t_pkt; - int size; - - #ifdef DEBUG - fprintf(fptr1, "Putting Kart in Default Mode\n"); - #endif - - t_pkt.id = AMP_SERIAL_DEFAULT; - t_pkt.size = 1; - t_pkt.msg[0] = 0xFF; - - amp_serial_jetson_tx_pkt(&t_pkt, &size); -} diff --git a/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.h b/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.h deleted file mode 100644 index 088291e..0000000 --- a/src/ti_comm/src/new_lib/ignore/amp_serial_jetson.h +++ /dev/null @@ -1,199 +0,0 @@ -/* - * amp_serial.h - * - * Created on: Mar 13, 2019 - * Author: David Pimley - */ - -#ifndef SRC_AMP_SERIAL_H_ -#define SRC_AMP_SERIAL_H_ - -#include "util.h" -#include "amp_err.h" - -#define AMP_SERIAL_MAX_PKT_SIZE 0xFF // max packet size - -#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_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 -#define AMP_SERIAL_CONFIG_CTS SP_CTS_INVALID // The Configuration CTS -#define AMP_SERIAL_CONFIG_DSR SP_DSR_INVALID // The Configuration DSR -#define AMP_SERIAL_CONFIG_DTR SP_DTR_INVALID // The Configuration DTR -#define AMP_SERIAL_CONFIG_RTS SP_RTS_INVALID // The Configuration RTS -#define AMP_SERIAL_CONFIG_XST SP_XONXOFF_INVALID // The Configuration XON/XOFF - - -#define AMP_SERIAL_RC_CTRL_MAX_VEL 10.0f // Max Remote Control Velocity in (m/s) -#define AMP_SERIAL_RC_CTRL_MAX_ANG 20.0f // Max Remote Control Steering Angle (rads) -#define AMP_MAX_VEL 10.0f -#define AMP_MIN_VEL 0.0f -#define AMP_MAX_ANG 5.0f -#define AMP_MIN_ANG 0.0f - -#define PS3_BUTTON_SELECT 0 -#define PS3_BUTTON_STICK_LEFT 1 -#define PS3_BUTTON_STICK_RIGHT 2 -#define PS3_BUTTON_START 3 -#define PS3_BUTTON_CROSS_UP 4 -#define PS3_BUTTON_CROSS_RIGHT 5 -#define PS3_BUTTON_CROSS_DOWN 6 -#define PS3_BUTTON_CROSS_LEFT 7 -#define PS3_BUTTON_REAR_LEFT_2 8 -#define PS3_BUTTON_REAR_RIGHT_2 9 -#define PS3_BUTTON_REAR_LEFT_1 10 -#define PS3_BUTTON_REAR_RIGHT_1 11 -#define PS3_BUTTON_ACTION_TRIANGLE 12 -#define PS3_BUTTON_ACTION_CIRCLE 13 -#define PS3_BUTTON_ACTION_CROSS 14 -#define PS3_BUTTON_ACTION_SQUARE 15 -#define PS3_BUTTON_PAIRING 16 - -#define PS3_AXIS_STICK_LEFT_LEFTWARDS 0 -#define PS3_AXIS_STICK_LEFT_UPWARDS 1 -#define PS3_AXIS_STICK_RIGHT_LEFTWARDS 2 -#define PS3_AXIS_STICK_RIGHT_UPWARDS 3 -#define PS3_AXIS_BUTTON_CROSS_UP 4 -#define PS3_AXIS_BUTTON_CROSS_RIGHT 5 -#define PS3_AXIS_BUTTON_CROSS_DOWN 6 -#define PS3_AXIS_BUTTON_CROSS_LEFT 7 -#define PS3_AXIS_BUTTON_REAR_LEFT_2 8 -#define PS3_AXIS_BUTTON_REAR_RIGHT_2 9 -#define PS3_AXIS_BUTTON_REAR_LEFT_1 10 -#define PS3_AXIS_BUTTON_REAR_RIGHT_1 11 -#define PS3_AXIS_BUTTON_ACTION_TRIANGLE 12 -#define PS3_AXIS_BUTTON_ACTION_CIRCLE 13 -#define PS3_AXIS_BUTTON_ACTION_CROSS 14 -#define PS3_AXIS_BUTTON_ACTION_SQUARE 15 -#define PS3_AXIS_ACCELEROMETER_LEFT 16 -#define PS3_AXIS_ACCELEROMETER_FORWARD 17 -#define PS3_AXIS_ACCELEROMETER_UP 18 -#define PS3_AXIS_GYRO_YAW 19 - - -/* - * Contains all packet ids possible from the NVIDIA Jetson TX2 On-Board - * Computer to the TI Micro. Any packet id not found in the below enumeration - * results in a kill kart function to stop the motion of the kart. - */ -typedef enum amp_serial_pkt_id_t { - AMP_SERIAL_CONTROL = 0xF1, // packet of steering & translational floats - //AMP_SERIAL_DAC_CONTROL, // packet for control of DAC - //AMP_SERIAL_PWM_CONTROL, // packet for control of PWM - AMP_SERIAL_DEFAULT = 0xF4, // packet for default mode of kart - AMP_SERIAL_ENABLE = 0xF0, // packet to enter enabled state (Power to the MC/servo) - AMP_SERIAL_DRIVE = 0xF5, // packet to enter drive forward state (will follow throttle/steering commands in FWD direction) - //AMP_SERIAL_DRIVE_REV, // packet to enter drive reverse state - AMP_SERIAL_KILL_KART = 0xF2 // packet for stopping all motion -} amp_serial_pkt_id_t; - -/* - * States of the Current Serial Port. The point of creating the states - * is to ensure that the software is only sending or receiving at one - * time. - */ -typedef enum amp_serial_state_t { - AMP_SERIAL_STATE_IDLE, - AMP_SERIAL_STATE_RX, - AMP_SERIAL_STATE_TX -} amp_serial_state_t; - -/* - * State of the current control of the system. Will be used as a hard toggle - * i.e. starts in autonomous and can be controlled by rc if desired but can't be - * switched back - */ -typedef enum amp_control_state_t { - AMP_CONTROL_AUTONOMOUS, - AMP_CONTROL_REMOTE -} amp_control_state_t; - -// DECLARE PACKET DATA STRUCTURES -typedef struct amp_serial_pkt_control_t { - int v_angle; // vehicle steering angle - int v_speed; // vehicle speed -} amp_serial_pkt_control_t; - -typedef struct amp_serial_pkt_dac_t { - uint8_t d_voltage; // voltage to set to the DAC -} amp_serial_pkt_dac_t; - -typedef struct amp_serial_pkt_pwm_t { - uint16_t p_freq; // frequency to set to the PWM -} amp_serial_pkt_pwm_t; - -typedef struct amp_serial_pkt_kill_t { - uint8_t k_flag; // whether or not to kill the kart -} amp_serial_pkt_kill_t; - - -/* - * Taken from libserialport.h to configure the serial port in a more - * concise manner. - */ -struct sp_port_config { - int baudrate; // Baudrate of the Serial Port - int bits; // Bits to Send per Byte - enum sp_parity parity; // Parity Checking of the Port - int stopbits; // Number of Stopbits - enum sp_rts rts; - enum sp_cts cts; - enum sp_dtr dtr; - enum sp_dsr dsr; - enum sp_xonxoff xon_xoff; -}; - -/* - * Structure of the AMP serial packet. The size of the packet is limited - * to 0xFF bytes. All packets must start with the hex 0x02 & 0x03 respectively. - */ -typedef struct amp_serial_pkt_t { - amp_serial_pkt_id_t id; // packet id - uint8_t size; // packet size - uint8_t msg[AMP_SERIAL_MAX_PKT_SIZE]; // contains the data of the packet - uint8_t crc; // cyclical redundancy check (2's complement) - uint32_t timeout; // timeout for receiving the packet -} amp_serial_pkt_t; - - -// FUNCTION DECLARATIONS -------------------------------------------------- -amp_err_code_t amp_serial_jetson_initialize(sp_port * _port); - -void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config); - -void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config); - -void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg); - -void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg); - -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); - -amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte); - -amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes); - -uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf); - -amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte); - -int float_to_int(float max, float min, float num); - -void amp_serial_jetson_enable_kart(); - -void amp_serial_jetson_enable_drive(); - -void amp_serial_jetson_enable_default(); - -int check(enum sp_return result, amp_err_code_t amp_err); - -void end_program(amp_err_code_t amp_err); - -const char *parity_name(enum sp_parity parity); - -#endif /* SRC_AMP_SERIAL_H_ */ diff --git a/src/ti_comm/src/new_lib/ti_comm.py b/src/ti_comm/src/new_lib/ti_comm.py deleted file mode 100644 index 9eb1324..0000000 --- a/src/ti_comm/src/new_lib/ti_comm.py +++ /dev/null @@ -1,209 +0,0 @@ -import rospy -import time -import serial -import math -import ctypes -import sys -import struct - -import subprocess - -from geometry_msgs.msg import Twist, Vector3 - -""" -Constants concering the configuration of the serial port -""" - -# Linux Serial Port, May be Something different on Windows (i.e. COM3) -LINUX_SERIAL_PORT = '/dev/ttyUSB0' - -# Baudrate for sending / receiving -BAUDRATE = 4800 - -# Bytesize of Data -BYTESIZE = serial.EIGHTBITS - -# No Parity Checking -PARITY = serial.PARITY_NONE - -# One Stop Bit -STOPBITS = serial.STOPBITS_ONE - -# Timeout for Reading Bytes (seconds) -READ_TIMEOUT = 10 - -class ti_comm(): - def __init__(self, ser): - - """ - Enumeration values for use in message construction - """ - - # Create Flags for Message Choice to be used by data_to_msg() - self.AMP_SERIAL_MAX_PKT_SIZE = 0xFF - - self.AMP_SERIAL_START_PKT = 0x02 - self.AMP_SERIAL_STOP_PKT = 0x03 - - self.AMP_SERIAL_CONTROL = 0x00 - self.AMP_SERIAL_DAC_CONTROL = 0x01 - self.AMP_SERIAL_PWM_CONTROL = 0x02 - self.AMP_SERIAL_KILL_KART = 0xFF - - self.serial_port = ser - - - # Start the transmission - self.cmd_vel_listener() - - - """ - Format the given data value into either a DAC message - or a PWM Message using the given 'd' or 'f' chars at the end - of the message. - - If the given parameter is - - IMPORTANT: The Serial Message is being parsed by a /r carriage - return. Must end the serial message with the carriage return. - - The Input message is assumed to be in a type int - """ - def control_msg(self, vel, ang): - # Verify the msg sent as input is a type of int - - - # CRC Calculation - crc_calc = ctypes.c_ubyte(0) - - # Declare and Intialize Output Variable - msg_out = [] - - # Format the Message Start - msg_out.append(self.AMP_SERIAL_START_PKT) - - # Message ID - msg_out.append(self.AMP_SERIAL_CONTROL) - - crc_calc.value = (crc_calc.value + self.AMP_SERIAL_CONTROL) & 0xFF - - # Message Size - msg_out.append(0x08) - - crc_calc.value = (crc_calc.value + 0x08) & 0xFF - - # Add Data to the Message Packet - msg_out.append(vel) - msg_out.append(ang) - - crc_calc.value = self.calculate_crc_data([vel, ang], crc_calc.value) - - # Add CRC Byte - msg_out.append(crc_calc.value) - - # Add ETX Byte - msg_out.append(self.AMP_SERIAL_STOP_PKT) - - return msg_out - - """ - Send the message over UART through the specified port - """ - def send_UART_msg(self, ser, msg): - try: - bytes_written = ser.write(msg.encode('utf-8')) - except serial.SerialTimeoutException as err: - print "Serial Write has Timed Out." - raise - - # Check the number of bytes written to the serial port - if bytes_written != len(msg): - print "Bytes Dropped in send_UART_msg:" + msg - - """ - Define the callback function for the cmd_vel topic messages. - """ - def cmd_vel_callback(self, cmd): - # Parse cmd_vel values into the correct objects - translational_velocity = ctypes.c_float(cmd.linear.x) - drive_angle = ctypes.c_float(cmd.angular.z) - - # Format the Values into the Correct Message - translational_msg = self.control_msg(translational_velocity, drive_angle) - - # Send each of the messages through the Serial Port - for byte in translational_msg: - self.send_UART_msg(self.serial_port, byte) - - """ - Create a rospy subscriber to read in the cmd_vel values that are output - through ROS & move_base. - """ - def cmd_vel_listener(self): - rospy.init_node("cmd_vel_listener", anonymous=True) - rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) - rospy.spin() - - - def calculate_crc_data(self, data, crc): - for val in data: - ba = bytearray(struct.pack('f', val)) - for byte in ba: - crc = (crc + byte) & 0xFF - - return crc - - -""" -Initialize the Serial Port with the Values -Indicated on the 'Defines' Above -""" -def init_port(): - if not verify_port(): - print "Stopping Script.\n" - - try: - ser = serial.Serial( - port=LINUX_SERIAL_PORT, - baudrate=BAUDRATE, - bytesize=BYTESIZE, - parity=PARITY, - stopbits=STOPBITS, - timeout=READ_TIMEOUT - ) - except ValueError as v_err: - print("Current Parameters are out of range. Please re-init with different values.\n") - raise - except serial.SerialException as s_err: - print("Unable to Configure Serial Port.\n") - raise - - # Verify the Serial Port is open - if (ser.isOpen() == False): - ser.open() - - # Return the Serial Object if Successful - return ser - -""" -Verify that a serial port exists on the system -""" -def verify_port(): - bash_cmd = "ls " + LINUX_SERIAL_PORT - try: - subprocess.check_call(bash_cmd, shell=True) - except subprocess.CalledProcessError: - print "Unable to Find Specified Serial Port.\n" - return False - - return True - -if __name__ == "__main__": - ser = init_port() - - try: - ti_comm(ser) - except rospy.ROSInterruptException: - rospy.loginfo("Serial Communication Interrupted") - - diff --git a/src/ti_comm/src/new_lib/util.h b/src/ti_comm/src/new_lib/util.h deleted file mode 100644 index ebcf264..0000000 --- a/src/ti_comm/src/new_lib/util.h +++ /dev/null @@ -1,29 +0,0 @@ -/***************************************** -File: -dict.h -Description: -general utility -*****************************************/ - -#ifndef _UTIL_H -#define _UTIL_H - -//data types -typedef unsigned char byte; -typedef unsigned short word; -typedef unsigned int dword; -typedef signed char int8_t; -typedef unsigned char uint8_t; -typedef signed short int int16_t; -typedef unsigned short int uint16_t; -typedef signed int int32_t; -typedef unsigned int uint32_t; -typedef int8_t int8; -typedef uint8_t uint8; -typedef int16_t int16; -typedef uint16_t uint16; -typedef int32_t int32; -typedef uint32_t uint32; -typedef unsigned char uchar_t; - -#endif /* _UTIL_H */ \ No newline at end of file diff --git a/src/ti_comm/src/writeandread.c b/src/ti_comm/src/writeandread.c deleted file mode 100644 index 43fdc8b..0000000 --- a/src/ti_comm/src/writeandread.c +++ /dev/null @@ -1,33 +0,0 @@ -#include -#include - -int main(int argc, char** argv) { -float one, two; -FILE * fp = fopen("data.txt", "w"); -one = 13.45624; -two = 1.3221; -fprintf(fp, "%f %f\n", one, two); -one = 12.4323; -two = 2.55723; -fprintf(fp, "%f %f\n", one, two); -one = 9.234572; -two = 4.3257; -fprintf(fp, "%f %f\n", one, two); -one = 6.457614; -two = 5; -fprintf(fp, "%f %f\n", one, two); - -fclose(fp); - -FILE * fp1 = fopen("data.txt", "r"); -fscanf(fp1, "%f %f\n", &one, &two); -printf("%f %f\n", one, two); -fscanf(fp1, "%f %f\n", &one, &two); -printf("%f %f\n", one, two); -fscanf(fp1, "%f %f\n", &one, &two); -printf("%f %f\n", one, two); -fscanf(fp1, "%f %f\n", &one, &two); -printf("%f %f\n", one, two); - -fclose(fp1); -}