diff --git a/.gitignore b/.gitignore index eb0f44a..f0c9da8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *.pyc .ccls-cache compile_commands.json +tags diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 191ccf8..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -ADD_SUBDIRECTORY( qt_pi ) -ADD_SUBDIRECTORY( gazebo_sim ) diff --git a/qt_pi/README.md b/README.md similarity index 100% rename from qt_pi/README.md rename to README.md diff --git a/qt_pi/msg/EEG_data.msg b/qt_pi/msg/EEG_data.msg deleted file mode 100644 index 60767b2..0000000 --- a/qt_pi/msg/EEG_data.msg +++ /dev/null @@ -1,4 +0,0 @@ -int channel[5] -float32 data[5] -int cq[5] -int batt_lvl diff --git a/qt_pi/nodes/arduino_node.py b/qt_pi/nodes/arduino_node.py deleted file mode 100755 index 0a692c8..0000000 --- a/qt_pi/nodes/arduino_node.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -import rospy -from qt_pi.arduino_driver import Arduino -from qt_pi.base_controller import BaseController -from geometry_msgs.msg import Twist -import os, time -import thread -from serial.serialutil import SerialException - -class ArduinoROS(): - def __init__(self): - rospy.init_node('arduino', log_level=rospy.INFO) - - # Get the actual node name in case it is set in the launch file - self.name = rospy.get_name() - - # Cleanup when termniating the node - rospy.on_shutdown(self.shutdown) - - self.port = rospy.get_param("~port", "/dev/ttyACM0") - self.baud = int(rospy.get_param("~baud", 57600)) - self.timeout = rospy.get_param("~timeout", 0.5) - self.base_frame = rospy.get_param("~base_frame", 'base_link') - self.camera_frame = rospy.get_param("~camera_frame", 'depth_camera_link') - self.motors_reversed = rospy.get_param("~motors_reversed", False) - # Overall loop rate: should be faster than fastest sensor rate - self.rate = int(rospy.get_param("~rate", 50)) - r = rospy.Rate(self.rate) - - self.use_base_controller = rospy.get_param("~use_base_controller", False) - - # Initialize a Twist message - self.cmd_vel = Twist() - - # A cmd_vel publisher so we can stop the robot when shutting down - self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) - - # Initialize the controlller - self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed) - - # Make the connection - self.controller.connect() - - rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") - - # Reserve a thread lock - mutex = thread.allocate_lock() - - # Initialize the base controller if used - if self.use_base_controller: - self.qt_pi_BaseController = BaseController(self.controller, self.base_frame, self.camera_frame, self.name + "_base_controller") - - # Start polling the sensors and base controller - while not rospy.is_shutdown(): - if self.use_base_controller: - mutex.acquire() - self.qt_pi_BaseController.poll() - mutex.release() - - r.sleep() - - def shutdown(self): - rospy.loginfo("Shutting down Arduino Node...") - - # Stop the robot - try: - rospy.loginfo("Stopping the robot...") - self.cmd_vel_pub.Publish(Twist()) - rospy.sleep(2) - except: - pass - - # Close the serial port - try: - self.controller.close() - except: - pass - finally: - rospy.loginfo("Serial port closed.") - os._exit(0) - -if __name__ == '__main__': - try: - qt_pi_Arduino = ArduinoROS() - except SerialException: - rospy.logerr("Serial exception trying to open port.") - os._exit(0) diff --git a/qt_pi/config/70-emotiv.rules b/qt_pi/qt_pi/config/70-emotiv.rules similarity index 100% rename from qt_pi/config/70-emotiv.rules rename to qt_pi/qt_pi/config/70-emotiv.rules diff --git a/qt_pi/config/base_local_planner_params.yaml b/qt_pi/qt_pi/config/base_local_planner_params.yaml similarity index 100% rename from qt_pi/config/base_local_planner_params.yaml rename to qt_pi/qt_pi/config/base_local_planner_params.yaml diff --git a/qt_pi/config/costmap_common_params.yaml b/qt_pi/qt_pi/config/costmap_common_params.yaml similarity index 100% rename from qt_pi/config/costmap_common_params.yaml rename to qt_pi/qt_pi/config/costmap_common_params.yaml diff --git a/qt_pi/config/global_costmap_params.yaml b/qt_pi/qt_pi/config/global_costmap_params.yaml similarity index 100% rename from qt_pi/config/global_costmap_params.yaml rename to qt_pi/qt_pi/config/global_costmap_params.yaml diff --git a/qt_pi/config/local_costmap_params.yaml b/qt_pi/qt_pi/config/local_costmap_params.yaml similarity index 100% rename from qt_pi/config/local_costmap_params.yaml rename to qt_pi/qt_pi/config/local_costmap_params.yaml diff --git a/qt_pi/config/qt_pi_arduino_params.yaml b/qt_pi/qt_pi/config/qt_pi_arduino_params.yaml similarity index 100% rename from qt_pi/config/qt_pi_arduino_params.yaml rename to qt_pi/qt_pi/config/qt_pi_arduino_params.yaml diff --git a/qt_pi/launch/arduino.launch b/qt_pi/qt_pi/launch/arduino.launch similarity index 100% rename from qt_pi/launch/arduino.launch rename to qt_pi/qt_pi/launch/arduino.launch diff --git a/qt_pi/launch/depth_to_laser.launch b/qt_pi/qt_pi/launch/depth_to_laser.launch similarity index 100% rename from qt_pi/launch/depth_to_laser.launch rename to qt_pi/qt_pi/launch/depth_to_laser.launch diff --git a/qt_pi/launch/gmapping.launch b/qt_pi/qt_pi/launch/gmapping.launch similarity index 100% rename from qt_pi/launch/gmapping.launch rename to qt_pi/qt_pi/launch/gmapping.launch diff --git a/qt_pi/launch/move_base.launch b/qt_pi/qt_pi/launch/move_base.launch similarity index 100% rename from qt_pi/launch/move_base.launch rename to qt_pi/qt_pi/launch/move_base.launch diff --git a/qt_pi/launch/qt_pi.launch b/qt_pi/qt_pi/launch/qt_pi.launch similarity index 100% rename from qt_pi/launch/qt_pi.launch rename to qt_pi/qt_pi/launch/qt_pi.launch diff --git a/qt_pi/CMakeLists.txt b/qt_pi/qt_pi_eeg/CMakeLists.txt similarity index 54% rename from qt_pi/CMakeLists.txt rename to qt_pi/qt_pi_eeg/CMakeLists.txt index d858d67..3b21b38 100644 --- a/qt_pi/CMakeLists.txt +++ b/qt_pi/qt_pi_eeg/CMakeLists.txt @@ -1,31 +1,14 @@ cmake_minimum_required(VERSION 2.8.3) -project(qt_pi) +project(qt_pi_eeg) set(CMAKE_CXX_FLAGS "-std=c++0x") #-fpermissive #ros setup find_package(catkin REQUIRED COMPONENTS -qt_build roscpp roslib ) -catkin_python_setup() -catkin_package(DEPENDS) - -#Qt setup and MOC generation -find_package(Qt5 REQUIRED COMPONENTS Core Gui OpenGL) -set(MOC include/speller/speller.h include/mapview/mapview.h) -set(HPP include/mapview/vertex.h) -set(SRCS src/speller.cpp src/mapview.cpp ) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -qt5_wrap_cpp(MOC_HPP ${MOC}) -qt5_add_resources(RCC resources/resources.qrc) - - - - - #Mcrypt library includes find_package(PkgConfig REQUIRED) find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS /usr/include) @@ -39,7 +22,6 @@ pkg_search_module(GATTLIB REQUIRED gattlib) pkg_search_module(GLIB REQUIRED glib-2.0) include_directories(${GLIB_INCLUDE_DIRS}) - include_directories(include ${catkin_INCLUDE_DIRS} ) @@ -48,21 +30,7 @@ include_directories(include add_library(ble_connect include/ble_connect/ble_connect.h src/ble_connect.c) target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS}) -#speller library generation -add_library(speller ${SRCS} ${FORMS_HPP} ${MOC_HPP} ${RCC} ${HPP}) -target_link_libraries(speller Qt5::Widgets Qt5::Core Qt5::OpenGL Qt5::Gui ${catkin_LIBRARIES} ble_connect) - - #ble_connect_test executable generation add_executable(ble_connect_test tests/ble_test.cpp) target_link_libraries(ble_connect_test ble_connect) -#QImage view test executable generation -add_executable(map_load_test tests/map_load_test.cpp) -target_link_libraries(map_load_test speller) - - - -#main executable generation -add_executable(speller_ui src/main.cpp) -target_link_libraries(speller_ui speller) diff --git a/qt_pi/doc/Connecting on debian.md b/qt_pi/qt_pi_eeg/doc/Connecting on debian.md similarity index 100% rename from qt_pi/doc/Connecting on debian.md rename to qt_pi/qt_pi_eeg/doc/Connecting on debian.md diff --git a/qt_pi/include/ble_connect/ble_connect.h b/qt_pi/qt_pi_eeg/include/ble_connect/ble_connect.h similarity index 100% rename from qt_pi/include/ble_connect/ble_connect.h rename to qt_pi/qt_pi_eeg/include/ble_connect/ble_connect.h diff --git a/qt_pi/qt_pi_eeg/package.xml b/qt_pi/qt_pi_eeg/package.xml new file mode 100644 index 0000000..33deb46 --- /dev/null +++ b/qt_pi/qt_pi_eeg/package.xml @@ -0,0 +1,11 @@ + + qt_pi_eeg + 0.0.0 + EEG processing package of qt_pi + Rishabh Kundu + MIT + + catkin + + roscpp + diff --git a/qt_pi/src/ble_connect.c b/qt_pi/qt_pi_eeg/src/ble_connect.c similarity index 100% rename from qt_pi/src/ble_connect.c rename to qt_pi/qt_pi_eeg/src/ble_connect.c diff --git a/qt_pi/tests/ble_test.cpp b/qt_pi/qt_pi_eeg/tests/ble_test.cpp similarity index 100% rename from qt_pi/tests/ble_test.cpp rename to qt_pi/qt_pi_eeg/tests/ble_test.cpp diff --git a/qt_pi/qt_pi_embedded/CMakeLists.txt.tmp b/qt_pi/qt_pi_embedded/CMakeLists.txt.tmp new file mode 100644 index 0000000..6abbf4c --- /dev/null +++ b/qt_pi/qt_pi_embedded/CMakeLists.txt.tmp @@ -0,0 +1,109 @@ +# I target a recent cmake, it shouldn't be a problem on a dev machine +cmake_minimum_required(VERSION 2.8.3) +# Project name +project(qt_pi_embedded_firmware) + +## AVR Chip Configuration +set(F_CPU 16000000UL) +# CPU, you can find the list here: +# https://gcc.gnu.org/onlinedocs/gcc/AVR-Options.html +set(MCU atmega328p) +# Default Baudrate for UART, read avr include/util/setbaud.h for usage +set(BAUDRATE 115200) +# The programmer to use, read avrdude manual for list +set(PROG_TYPE arduino) + +set(UPLOAD_BAUD 57600) +# AVR Fuses, must be in concordance with your hardware and F_CPU +# http://eleccelerator.com/fusecalc/fusecalc.php?chip=atmega328p +set(E_FUSE 0xfd) +set(H_FUSE 0xda) +set(L_FUSE 0xfd) +set(LOCK_BIT 0xff) + + +# Use AVR GCC toolchain +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_CXX_COMPILER avr-g++) +set(CMAKE_C_COMPILER avr-gcc) +set(CMAKE_ASM_COMPILER avr-gcc) +set(ARDUINO_SDK /home/yikes/arduino-1.8.13) + +# Pass defines to compiler +add_definitions( + -DF_CPU=${F_CPU} + -DBAUD=${BAUD} + -DARDUINO=10813 + -DARDUINO_AVR_NANO + -DARDUINO_ARCH_AVR +) +# mmcu MUST be passed to bot the compiler and linker, this handle the linker +set(CMAKE_EXE_LINKER_FLAGS -mmcu=${MCU}) + +add_compile_options( + -mmcu=${MCU} # MCU + -std=gnu++11 # C99 standard + -Os # optimize + -Wall # enable warnings + -Wno-main + -Wundef + -pedantic + -Wstrict-prototypes + -Werror + -Wfatal-errors + -Wl,--relax,--gc-sections + -g + -gdwarf-2 + -funsigned-char # a few optimizations + -funsigned-bitfields + -fpack-struct + -fshort-enums + -ffunction-sections + -fdata-sections + -fno-split-wide-types + -fno-tree-scev-cprop + -c + -MMD + -w + -fno-exceptions + -fno-threadsafe-statics + -flto + +) + +file(GLOB SRC_FILES "src/*.cpp") # Load all files in src folder +file(GLOB_RECURSE ARDUINO_SDK_FILES + ${ARDUINO_SDK}/hardware/arduino/avr/variants/* +) + +file(GLOB_RECURSE ARDUINO_SDK_LIBS + ${ARDUINO_SDK}/hardware/arduino/avr/libraries/* + ) +message(STATUS "Hello" ${ARDUINO_SDK_FILES}) + +include_directories( + include + ${ARDUINO_SDK}/hardware/arduino/avr/cores/arduino + ${ARDUINO_SDK_FILES} + ${ARDUINO_SDK_LIBS} + ${ARDUINO_EXT_LIB} +) + +# Create one target +add_executable(${PROJECT_NAME}_exe ${ARDUINO_SDK}/hardware/arduino/avr/cores/arduino/main.cpp) + +# Rename the output to .elf as we will create multiple files +set_target_properties(${PRODUCT_NAME} PROPERTIES OUTPUT_NAME ${PRODUCT_NAME}.elf) + +# Strip binary for upload +#add_custom_target(strip ALL avr-strip ${PRODUCT_NAME}.elf DEPENDS ${PRODUCT_NAME}) + +# Transform binary into hex file, we ignore the eeprom segments in the step +#add_custom_target(hex ALL avr-objcopy -R .eeprom -O ihex ${PRODUCT_NAME}.elf ${PRODUCT_NAME}.hex DEPENDS strip) +# Transform binary into hex file, this is the eeprom part (empty if you don't +# use eeprom static variables) +#add_custom_target(eeprom avr-objcopy -j .eeprom --set-section-flags=.eeprom="alloc,load" --change-section-lma .eeprom=0 -O ihex ${PRODUCT_NAME}.elf ${PRODUCT_NAME}.eep DEPENDS strip) + +# Upload the firmware with avrdude +#add_custom_target(upload avrdude -C${ARDUINO_SDK}/hardware/tools/avr/etc/avrdude.conf -c${PROG_TYPE} -v -V -p${MCU} -D -b${UPLOAD_BAUD} -Uflash:w:${PRODUCT_NAME}.hex:i DEPENDS hex) + diff --git a/qt_pi/src/libraries/Arduino/batt.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp similarity index 100% rename from qt_pi/src/libraries/Arduino/batt.cpp rename to qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp diff --git a/qt_pi/src/libraries/Arduino/batt.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h similarity index 100% rename from qt_pi/src/libraries/Arduino/batt.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp new file mode 100644 index 0000000..f290970 --- /dev/null +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp @@ -0,0 +1,374 @@ +#include "qt_pi.h" +/* +#define WAITING_FOR_NEW_COM 0 +#define FIRST_BYTE_VERIFIED 1 +#define SECOND_BYTE_VERIFIED 2 +#define LENGTH_INPUT 3 +#define READING_PAYLOAD 4 +#define READING_COMPLETE 5 +#define COM_RECEIVED 6 +*/ +enum COMM_STATE +{ + WAITING_FOR_NEW_COM, + FIRST_BYTE_VERIFIED, + SECOND_BYTE_VERIFIED, + LENGTH_INPUT, + READING_PAYLOAD, + READING_COMPLETE, + COM_RECEIVED +}; + +int curState = WAITING_FOR_NEW_COM; + +int com_len; + +uint8_t in_buf[buf_size]; +int in_buf_i; + +uint8_t ser_write_buf[buf_size]; +uint8_t com_recv[buf_size]; + +int +init_comm () +{ + + ser_write_buf[MB1P] = magic_start1; + ser_write_buf[MB2P] = magic_start2; + + Serial.begin (BAUDRATE); + Serial.setTimeout (0); + + resetComm (); + + curState = WAITING_FOR_NEW_COM; +} + +int +sendCommand (int16_t len) +{ + if ((len + net_extra_len) > buf_size) + return -1; + + memcpy (ser_write_buf + start_magic_len, &len, len_len); + + ser_write_buf[preamble_len + len] = magic_end; + + return (Serial.write (ser_write_buf, len + net_extra_len)); +} + +int +sendData (int len, uint8_t *buf) +{ + + if ((len + net_extra_len) > buf_size) + return -1; + + memcpy (ser_write_buf + preamble_len, buf, len); + + return sendCommand (len); +} + +int +runInpCommand () +{ + + int data_len = com_len - com_preamble_len; + + switch (com_recv[COMB]) + { + + case GET_BAUDRATE: + + if (data_len != GET_BAUDRATE_DATA_LEN) + { + data_len = -1; + break; + } + + memcpy (ser_write_buf + DATASTRBP, &BAUDRATE, 4); + + data_len = 4; + + break; + + case GET_ENCODERS: + { + + if (data_len != GET_ENCODERS_DATA_LEN) + { + data_len = -1; + break; + } + + long l = readEncoder (LEFT_WHEEL); + long r = readEncoder (RIGHT_WHEEL); + + memcpy (ser_write_buf + DATASTRBP, &l, 4); + memcpy (ser_write_buf + DATASTRBP + 4, &r, 4); + + data_len = 8; + + break; + } + + case RESET_ENCODERS: + + if (data_len != RESET_ENCODERS_DATA_LEN) + { + data_len = -1; + break; + } + + resetEncoders (); + resetPID (); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + case SET_SPEEDS: + + if (data_len != SET_SPEEDS_DATA_LEN) + { + data_len = -1; + break; + } + + move_cmd_timeout_reset (); + + if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0) + { + + setMotorSpeeds (0, 0); + + resetPID (); + + setMoving (false); + } + else + { + setMoving (true); + } + + setTicksPerFrame (com_recv + DATASTRB); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + case SET_PID_L: + + if (data_len != SET_PID_DATA_LEN) + { + data_len = -1; + break; + } + + setPID_L (com_recv + DATASTRB); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + case SET_PID_R: + + if (data_len != SET_PID_DATA_LEN) + { + data_len = -1; + break; + } + + setPID_R (com_recv + DATASTRB); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + case GET_PID_P: + + if (data_len != GET_PID_P_DATA_LEN) + { + data_len = -1; + break; + } + + getPID (ser_write_buf + DATASTRBP); + + data_len = 8; + + break; + + case GET_BATTERY_LVL: + + if (data_len != GET_BATTERY_LVL_DATA_LEN) + { + data_len = -1; + break; + } + + getBattLvl (ser_write_buf + DATASTRBP); + + data_len = 2; + + break; + + case EN_IMU: + + if (data_len != EN_IMU_DATA_LEN) + { + data_len = -1; + break; + } + + enable_imu (com_recv[DATASTRB]); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + case EN_ODOM: + + if (data_len != EN_ODOM_DATA_LEN) + { + data_len = -1; + break; + } + + enable_odom (com_recv[DATASTRB]); + + ser_write_buf[DATASTRBP] = 1; + data_len = 1; + + break; + + default: + + ser_write_buf[DATASTRBP] = com_recv[COMB]; + com_recv[COMB] = INVALID; + data_len = 1; + + break; + } + + if (data_len == -1) + { + ser_write_buf[COMBP] = INVALID_LENGTH; + ser_write_buf[CIDBP] = com_recv[CIDB]; + ser_write_buf[DATASTRBP] = com_recv[COMB]; + data_len = 1; + } + else + { + ser_write_buf[COMBP] = com_recv[COMB]; + ser_write_buf[CIDBP] = com_recv[CIDB]; + } + + sendCommand (data_len + com_preamble_len); +} + +void +resetComm () +{ + com_len = 1; + in_buf_i = 0; +} + +uint8_t temp_buf[64]; +void +serCommTimeout () +{ + Serial.readBytes (temp_buf, 64); +} + +void +proc_serial () +{ + + if (!Serial.available ()) + return; + + move_cmd_timeout_reset (); + + switch (curState) + { + + case WAITING_FOR_NEW_COM: + + if (Serial.read () == magic_start1) + { + curState = FIRST_BYTE_VERIFIED; + } + + break; + + case FIRST_BYTE_VERIFIED: + + if (Serial.read () == magic_start2) + { + curState = SECOND_BYTE_VERIFIED; + } + else + { + curState = WAITING_FOR_NEW_COM; + } + + break; + + case SECOND_BYTE_VERIFIED: + + com_len = Serial.read (); + curState = LENGTH_INPUT; + + break; + + case LENGTH_INPUT: + + com_len = com_len | (Serial.read () << 8); + + if (com_len < 20) + { + curState = READING_PAYLOAD; + } + else + { + curState = WAITING_FOR_NEW_COM; + } + + break; + + case READING_PAYLOAD: + + in_buf_i += Serial.readBytes (&in_buf[in_buf_i], com_len - in_buf_i); + + if (in_buf_i == com_len) + { + curState = READING_COMPLETE; + } + break; + + case READING_COMPLETE: + + if (Serial.read () == magic_end) + { + + memcpy (com_recv, in_buf, com_len); + + runInpCommand (); + + curState = WAITING_FOR_NEW_COM; + } + + resetComm (); + + break; + } + + if ((Serial.available () > 0) && curState != WAITING_FOR_NEW_COM) + proc_serial (); +} diff --git a/qt_pi/src/libraries/Arduino/comm.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h similarity index 100% rename from qt_pi/src/libraries/Arduino/comm.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h new file mode 100644 index 0000000..2a520a4 --- /dev/null +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h @@ -0,0 +1,35 @@ +#ifndef COMMANDS_H +#define COMMANDS_H + +#define INVALID 0xFF +#define INVALID_LENGTH 0xFE + +#define GET_BAUDRATE 0x51 +#define GET_ENCODERS 0x52 +#define SET_SPEEDS 0x53 +#define RESET_ENCODERS 0x54 +#define SET_PID_L 0x55 +#define SET_PID_R 0x56 +#define GET_PID_P 0x57 +#define GET_BATTERY_LVL 0x58 +#define EN_IMU 0x59 +#define EN_ODOM 0x60 +#define ODOM_DATA 0x61 +#define IMU_DATA 0x62 + +#define GET_BAUDRATE_DATA_LEN 0x01 +#define GET_ENCODERS_DATA_LEN 0x01 +#define SET_SPEEDS_DATA_LEN 0x03 +#define RESET_ENCODERS_DATA_LEN 0x01 +#define SET_PID_DATA_LEN 0x04 +#define GET_PID_P_DATA_LEN 0x01 +#define GET_BATTERY_LVL_DATA_LEN 0x01 +#define EN_IMU_DATA_LEN 0x01 +#define EN_ODOM_DATA_LEN 0x01 +#define ODOM_DATA_LEN 0x08 +#define IMU_DATA_LEN 0x08 + +#define LEFT_WHEEL 0x00 +#define RIGHT_WHEEL 0x01 + +#endif diff --git a/qt_pi/src/libraries/Arduino/diff_controller.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp similarity index 92% rename from qt_pi/src/libraries/Arduino/diff_controller.cpp rename to qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp index 5cf245d..34bd27a 100644 --- a/qt_pi/src/libraries/Arduino/diff_controller.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp @@ -29,14 +29,14 @@ unsigned char moving = 0; void resetPID(){ leftPID.TargetTicksPerFrame = 0.0; - leftPID.Encoder = readEncoder(LEFT); + leftPID.Encoder = readEncoder(LEFT_WHEEL); leftPID.PrevEnc = leftPID.Encoder; leftPID.output = 0; leftPID.PrevInput = 0; leftPID.ITerm = 0; rightPID.TargetTicksPerFrame = 0.0; - rightPID.Encoder = readEncoder(RIGHT); + rightPID.Encoder = readEncoder(RIGHT_WHEEL); rightPID.PrevEnc = rightPID.Encoder; rightPID.output = 0; rightPID.PrevInput = 0; @@ -85,8 +85,8 @@ void doPID(SetPointInfo * p) { void updatePID() { - leftPID.Encoder = readEncoder(LEFT); - rightPID.Encoder = readEncoder(RIGHT); + leftPID.Encoder = readEncoder(LEFT_WHEEL); + rightPID.Encoder = readEncoder(RIGHT_WHEEL); if (!moving){ diff --git a/qt_pi/src/libraries/Arduino/diff_controller.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.h similarity index 100% rename from qt_pi/src/libraries/Arduino/diff_controller.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.h diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino b/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino new file mode 100644 index 0000000..5ab24d0 --- /dev/null +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino @@ -0,0 +1,90 @@ +#include "qt_pi.h" + +#define _TASK_SLEEP_ON_IDLE_RUN +#define _TASK_INLINE +#define _TASK_TIMEOUT + +#include + +Task *batt_mon; +Task *diff_control; +Task *move_cmd_timeout; +Task *odom_out; +Task *imu_out; +Task *ser_comm_timeout; + +Scheduler ts; + +void setup() { + + init_comm(); + + init_enc(); + + initMotorController(); + + batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, + &initBatt, NULL); + diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, + &resetPID, NULL); + move_cmd_timeout = + new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL); + odom_out = + new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL); + imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false, &initIMU, + NULL); + ser_comm_timeout = + new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL); +} + +void loop() { + + ts.execute(); + + proc_serial(); + +} + +void move_cmd_timeout_reset() { + // move_cmd_timeout->resetTimeout(); +} + +void enable_odom(bool a) { + switch (a) { + case 0: + switch (odom_out->isEnabled()) { + case 1: + odom_out->disable(); + break; + } + break; + + case 1: + switch (odom_out->isEnabled()) { + case 0: + odom_out->enable(); + break; + } + break; + } +} + +void enable_imu(bool a) { + switch (a) { + case 0: + switch (imu_out->isEnabled()) { + case 1: + imu_out->disable(); + break; + } + break; + + case 1: + switch (imu_out->isEnabled()) { + case 0: + imu_out->enable(); + break; + } + break; + } +} diff --git a/qt_pi/src/libraries/Arduino/encoder_driver.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp similarity index 94% rename from qt_pi/src/libraries/Arduino/encoder_driver.cpp rename to qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp index 3516eb1..614e9d6 100644 --- a/qt_pi/src/libraries/Arduino/encoder_driver.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp @@ -51,7 +51,7 @@ void init_enc(){ long readEncoder(int i) { - if (i == LEFT) + if (i == LEFT_WHEEL) return left_enc_pos; else @@ -61,7 +61,7 @@ long readEncoder(int i) { void resetEncoder(int i) { - if (i == LEFT){ + if (i == LEFT_WHEEL){ left_enc_pos=0L; return; } @@ -74,8 +74,8 @@ long readEncoder(int i) { void resetEncoders() { - resetEncoder(LEFT); - resetEncoder(RIGHT); + resetEncoder(LEFT_WHEEL); + resetEncoder(RIGHT_WHEEL); } diff --git a/qt_pi/src/libraries/Arduino/encoder_driver.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.h similarity index 100% rename from qt_pi/src/libraries/Arduino/encoder_driver.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.h diff --git a/qt_pi/src/libraries/Arduino/imu.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp similarity index 100% rename from qt_pi/src/libraries/Arduino/imu.cpp rename to qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp diff --git a/qt_pi/src/libraries/Arduino/imu.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h similarity index 100% rename from qt_pi/src/libraries/Arduino/imu.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h diff --git a/qt_pi/src/libraries/Arduino/motor_driver.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp similarity index 90% rename from qt_pi/src/libraries/Arduino/motor_driver.cpp rename to qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp index 8a25dc3..34f5f5a 100644 --- a/qt_pi/src/libraries/Arduino/motor_driver.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp @@ -32,7 +32,7 @@ void setMotorSpeed(int i, int spd) { { switch(i){ - case LEFT: + case LEFT_WHEEL: switch(reverse) { @@ -47,7 +47,7 @@ void setMotorSpeed(int i, int spd) { } break; - case RIGHT: + case RIGHT_WHEEL: switch(reverse) { case 0: @@ -66,7 +66,7 @@ void setMotorSpeed(int i, int spd) { void setMotorSpeeds(int leftSpeed, int rightSpeed) { - setMotorSpeed(LEFT, leftSpeed); - setMotorSpeed(RIGHT, rightSpeed); + setMotorSpeed(LEFT_WHEEL, leftSpeed); + setMotorSpeed(RIGHT_WHEEL, rightSpeed); } diff --git a/qt_pi/src/libraries/Arduino/motor_driver.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.h similarity index 100% rename from qt_pi/src/libraries/Arduino/motor_driver.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.h diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/payload_specs.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/payload_specs.h new file mode 100644 index 0000000..c4cb202 --- /dev/null +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/payload_specs.h @@ -0,0 +1,33 @@ +#ifndef PAYLOAD_SPECS_H +#define PAYLOAD_SPECS_H + +#define buf_size 32 + +#define MB1P 0 +#define MB2P 1 +#define LB1P 2 +#define LB2P 3 +#define CIDBP 4 +#define COMBP 5 +#define DATASTRBP 6 + +#define start_magic_len 2 +#define end_magic_len 1 +#define len_len 2 + +#define BID 0xFF + +#define CIDB 0 +#define COMB 1 +#define DATASTRB 2 + +const int com_preamble_len = 2; + +const int preamble_len = start_magic_len + len_len; +const int net_extra_len = preamble_len + end_magic_len; + +const unsigned char magic_start1 = 0xEE; +const unsigned char magic_start2 = 0xAE; +const unsigned char magic_end = 0xFC; + +#endif diff --git a/qt_pi/src/libraries/Arduino/qt_pi.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h similarity index 100% rename from qt_pi/src/libraries/Arduino/qt_pi.h rename to qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h index 4c36a83..6c82459 100644 --- a/qt_pi/src/libraries/Arduino/qt_pi.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h @@ -6,12 +6,12 @@ #include "commands.h" #include "payload_specs.h" -#include "imu.h" -#include "comm.h" #include "batt.h" -#include "motor_driver.h" -#include "encoder_driver.h" +#include "comm.h" #include "diff_controller.h" +#include "encoder_driver.h" +#include "imu.h" +#include "motor_driver.h" extern void move_cmd_timeout_reset(); extern void enable_imu(bool); diff --git a/qt_pi/qt_pi_gui/CMakeLists.txt b/qt_pi/qt_pi_gui/CMakeLists.txt new file mode 100644 index 0000000..399e92b --- /dev/null +++ b/qt_pi/qt_pi_gui/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(qt_pi_gui) + +set(CMAKE_CXX_FLAGS "-std=c++0x") +#-fpermissive +#ros setup +find_package(catkin REQUIRED COMPONENTS +qt_build +roscpp +roslib +qt_pi_msgs +) + +#Qt setup and MOC generation +find_package(Qt5 REQUIRED COMPONENTS Core Gui OpenGL) +set(MOC include/speller/speller.h include/mapview/mapview.h) +set(HPP include/mapview/vertex.h) +set(SRCS src/speller.cpp src/mapview.cpp ) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +qt5_wrap_cpp(MOC_HPP ${MOC}) +qt5_add_resources(RCC resources/resources.qrc) + +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +#speller library generation +add_library(speller ${SRCS} ${FORMS_HPP} ${MOC_HPP} ${RCC} ${HPP}) +target_link_libraries(speller Qt5::Widgets Qt5::Core Qt5::OpenGL Qt5::Gui ${catkin_LIBRARIES} ble_connect) + +#QImage view test executable generation +add_executable(map_load_test tests/map_load_test.cpp) +target_link_libraries(map_load_test speller) + +#main executable generation +add_executable(speller_ui src/main.cpp) +target_link_libraries(speller_ui speller) diff --git a/qt_pi/include/mapview/mapview.h b/qt_pi/qt_pi_gui/include/mapview/mapview.h similarity index 100% rename from qt_pi/include/mapview/mapview.h rename to qt_pi/qt_pi_gui/include/mapview/mapview.h diff --git a/qt_pi/include/mapview/vertex.h b/qt_pi/qt_pi_gui/include/mapview/vertex.h similarity index 100% rename from qt_pi/include/mapview/vertex.h rename to qt_pi/qt_pi_gui/include/mapview/vertex.h diff --git a/qt_pi/include/speller/speller.h b/qt_pi/qt_pi_gui/include/speller/speller.h similarity index 100% rename from qt_pi/include/speller/speller.h rename to qt_pi/qt_pi_gui/include/speller/speller.h diff --git a/qt_pi/package.xml b/qt_pi/qt_pi_gui/package.xml similarity index 58% rename from qt_pi/package.xml rename to qt_pi/qt_pi_gui/package.xml index be01063..1448a96 100644 --- a/qt_pi/package.xml +++ b/qt_pi/qt_pi_gui/package.xml @@ -1,18 +1,17 @@ - qt_pi + qt_pi_gui 0.0.0 - The qt_pi package - Rishabh Kundu + GUI package of qt_pi + Rishabh Kundu MIT + catkin + qt_build roscpp roslib - rospy geometry_msgs nav_msgs tf - python-serial - - \ No newline at end of file + diff --git a/qt_pi/resources/resources.qrc b/qt_pi/qt_pi_gui/resources/resources.qrc similarity index 100% rename from qt_pi/resources/resources.qrc rename to qt_pi/qt_pi_gui/resources/resources.qrc diff --git a/qt_pi/resources/shaders/blink.frag b/qt_pi/qt_pi_gui/resources/shaders/blink.frag similarity index 100% rename from qt_pi/resources/shaders/blink.frag rename to qt_pi/qt_pi_gui/resources/shaders/blink.frag diff --git a/qt_pi/resources/shaders/blink.vert b/qt_pi/qt_pi_gui/resources/shaders/blink.vert similarity index 100% rename from qt_pi/resources/shaders/blink.vert rename to qt_pi/qt_pi_gui/resources/shaders/blink.vert diff --git a/qt_pi/resources/shaders/map.frag b/qt_pi/qt_pi_gui/resources/shaders/map.frag similarity index 100% rename from qt_pi/resources/shaders/map.frag rename to qt_pi/qt_pi_gui/resources/shaders/map.frag diff --git a/qt_pi/resources/shaders/map.vert b/qt_pi/qt_pi_gui/resources/shaders/map.vert similarity index 100% rename from qt_pi/resources/shaders/map.vert rename to qt_pi/qt_pi_gui/resources/shaders/map.vert diff --git a/qt_pi/src/main.cpp b/qt_pi/qt_pi_gui/src/main.cpp similarity index 100% rename from qt_pi/src/main.cpp rename to qt_pi/qt_pi_gui/src/main.cpp diff --git a/qt_pi/src/mapview.cpp b/qt_pi/qt_pi_gui/src/mapview.cpp similarity index 100% rename from qt_pi/src/mapview.cpp rename to qt_pi/qt_pi_gui/src/mapview.cpp diff --git a/qt_pi/src/speller.cpp b/qt_pi/qt_pi_gui/src/speller.cpp similarity index 100% rename from qt_pi/src/speller.cpp rename to qt_pi/qt_pi_gui/src/speller.cpp diff --git a/qt_pi/tests/map_load_test.cpp b/qt_pi/qt_pi_gui/tests/map_load_test.cpp similarity index 100% rename from qt_pi/tests/map_load_test.cpp rename to qt_pi/qt_pi_gui/tests/map_load_test.cpp diff --git a/qt_pi/qt_pi_msgs/CMakeLists.txt b/qt_pi/qt_pi_msgs/CMakeLists.txt new file mode 100644 index 0000000..766d877 --- /dev/null +++ b/qt_pi/qt_pi_msgs/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8.3) +project(qt_pi_msgs) + +find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs sensor_msgs nav_msgs message_generation) + +add_message_files( + FILES + Encoder.msg + PID.msg +) + +add_service_files( + FILES + GetBatteryInfo.srv + GetBaudRate.srv + GetEncoderData.srv + GetPIDData.srv + SetPIDData.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + sensor_msgs + nav_msgs +) + +catkin_package( + CATKIN_DEPENDS + message_runtime + std_msgs + geometry_msgs + sensor_msgs + nav_msgs +) diff --git a/qt_pi/qt_pi_msgs/msg/Encoder.msg b/qt_pi/qt_pi_msgs/msg/Encoder.msg new file mode 100644 index 0000000..121cce9 --- /dev/null +++ b/qt_pi/qt_pi_msgs/msg/Encoder.msg @@ -0,0 +1,2 @@ +int64 LEFT +int64 RIGHT diff --git a/qt_pi/qt_pi_msgs/msg/PID.msg b/qt_pi/qt_pi_msgs/msg/PID.msg new file mode 100644 index 0000000..792f971 --- /dev/null +++ b/qt_pi/qt_pi_msgs/msg/PID.msg @@ -0,0 +1,8 @@ +uint8 lkp +uint8 lkd +uint8 lki +uint8 lko +uint8 rkp +uint8 rkd +uint8 rki +uint8 rko diff --git a/qt_pi/qt_pi_msgs/package.xml b/qt_pi/qt_pi_msgs/package.xml new file mode 100644 index 0000000..14172fe --- /dev/null +++ b/qt_pi/qt_pi_msgs/package.xml @@ -0,0 +1,19 @@ + + qt_pi_msgs + 0.0.0 + Custom msgs and srvs package for qt_pi + Rishabh Kundu + MIT + + catkin + message_generation + nav_msgs + sensor_msgs + geometry_msgs + std_msgs + message_runtime + nav_msgs + sensor_msgs + geometry_msgs + std_msgs + diff --git a/qt_pi/qt_pi_msgs/srv/GetBatteryInfo.srv b/qt_pi/qt_pi_msgs/srv/GetBatteryInfo.srv new file mode 100644 index 0000000..111d412 --- /dev/null +++ b/qt_pi/qt_pi_msgs/srv/GetBatteryInfo.srv @@ -0,0 +1,2 @@ +--- +float32 volts diff --git a/qt_pi/qt_pi_msgs/srv/GetBaudRate.srv b/qt_pi/qt_pi_msgs/srv/GetBaudRate.srv new file mode 100644 index 0000000..7376175 --- /dev/null +++ b/qt_pi/qt_pi_msgs/srv/GetBaudRate.srv @@ -0,0 +1,2 @@ +--- +uint32 BAUDRATE diff --git a/qt_pi/qt_pi_msgs/srv/GetEncoderData.srv b/qt_pi/qt_pi_msgs/srv/GetEncoderData.srv new file mode 100644 index 0000000..2e79fcb --- /dev/null +++ b/qt_pi/qt_pi_msgs/srv/GetEncoderData.srv @@ -0,0 +1,2 @@ +--- +qt_pi_msgs/Encoder data diff --git a/qt_pi/qt_pi_msgs/srv/GetPIDData.srv b/qt_pi/qt_pi_msgs/srv/GetPIDData.srv new file mode 100644 index 0000000..3f4899f --- /dev/null +++ b/qt_pi/qt_pi_msgs/srv/GetPIDData.srv @@ -0,0 +1,2 @@ +--- +qt_pi_msgs/PID data diff --git a/qt_pi/qt_pi_msgs/srv/SetPIDData.srv b/qt_pi/qt_pi_msgs/srv/SetPIDData.srv new file mode 100644 index 0000000..0807fe1 --- /dev/null +++ b/qt_pi/qt_pi_msgs/srv/SetPIDData.srv @@ -0,0 +1,2 @@ +qt_pi_msgs/PID data +--- diff --git a/qt_pi/qt_pi_serial/CMakeLists.txt b/qt_pi/qt_pi_serial/CMakeLists.txt new file mode 100644 index 0000000..b3dbd15 --- /dev/null +++ b/qt_pi/qt_pi_serial/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(qt_pi_serial) + +find_package(catkin REQUIRED COMPONENTS + roscpp + qt_pi_msgs + std_msgs + sensor_msgs + nav_msgs + std_srvs +) + +catkin_package() + +set(SRCS src/ros_node.cpp src/serial_driver.cpp) +set(HPP include/qt_pi_serial/commands.h include/qt_pi_serial/ros_node.h include/qt_pi_serial/payload_specs.h include/qt_pi_serial/serial_driver.h) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(serial_comm ${SRCS}) +add_dependencies(serial_comm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(serial_comm ${catkin_LIBRARIES}) + +add_executable(serial_node src/main.cpp) +target_link_libraries(serial_node serial_comm) + +add_executable(serial_comm_test tests/comm_test.cpp) +target_link_libraries(serial_comm_test serial_comm) + diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h new file mode 100644 index 0000000..2a520a4 --- /dev/null +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h @@ -0,0 +1,35 @@ +#ifndef COMMANDS_H +#define COMMANDS_H + +#define INVALID 0xFF +#define INVALID_LENGTH 0xFE + +#define GET_BAUDRATE 0x51 +#define GET_ENCODERS 0x52 +#define SET_SPEEDS 0x53 +#define RESET_ENCODERS 0x54 +#define SET_PID_L 0x55 +#define SET_PID_R 0x56 +#define GET_PID_P 0x57 +#define GET_BATTERY_LVL 0x58 +#define EN_IMU 0x59 +#define EN_ODOM 0x60 +#define ODOM_DATA 0x61 +#define IMU_DATA 0x62 + +#define GET_BAUDRATE_DATA_LEN 0x01 +#define GET_ENCODERS_DATA_LEN 0x01 +#define SET_SPEEDS_DATA_LEN 0x03 +#define RESET_ENCODERS_DATA_LEN 0x01 +#define SET_PID_DATA_LEN 0x04 +#define GET_PID_P_DATA_LEN 0x01 +#define GET_BATTERY_LVL_DATA_LEN 0x01 +#define EN_IMU_DATA_LEN 0x01 +#define EN_ODOM_DATA_LEN 0x01 +#define ODOM_DATA_LEN 0x08 +#define IMU_DATA_LEN 0x08 + +#define LEFT_WHEEL 0x00 +#define RIGHT_WHEEL 0x01 + +#endif diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/payload_specs.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/payload_specs.h new file mode 100644 index 0000000..c4cb202 --- /dev/null +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/payload_specs.h @@ -0,0 +1,33 @@ +#ifndef PAYLOAD_SPECS_H +#define PAYLOAD_SPECS_H + +#define buf_size 32 + +#define MB1P 0 +#define MB2P 1 +#define LB1P 2 +#define LB2P 3 +#define CIDBP 4 +#define COMBP 5 +#define DATASTRBP 6 + +#define start_magic_len 2 +#define end_magic_len 1 +#define len_len 2 + +#define BID 0xFF + +#define CIDB 0 +#define COMB 1 +#define DATASTRB 2 + +const int com_preamble_len = 2; + +const int preamble_len = start_magic_len + len_len; +const int net_extra_len = preamble_len + end_magic_len; + +const unsigned char magic_start1 = 0xEE; +const unsigned char magic_start2 = 0xAE; +const unsigned char magic_end = 0xFC; + +#endif diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h new file mode 100644 index 0000000..319cfe0 --- /dev/null +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h @@ -0,0 +1,46 @@ +#ifndef ROS_NODE_H +#define ROS_NODE_H + +#include + +#include +#include +#include + +#include "qt_pi_msgs/GetBatteryInfo.h" +#include "qt_pi_msgs/GetBaudRate.h" +#include "qt_pi_msgs/GetEncoderData.h" +#include "qt_pi_msgs/GetPIDData.h" +#include "qt_pi_msgs/SetPIDData.h" +#include "serial_driver.h" +#include "std_srvs/SetBool.h" +#include "std_srvs/Trigger.h" + +ros::Publisher imu_pub; +ros::Publisher odom_pub; +ros::ServiceServer get_encoder; +ros::ServiceServer get_batt_state; +ros::ServiceServer get_pid_val; +ros::ServiceServer get_baudrate; +ros::ServiceServer set_pid_val; +ros::ServiceServer en_imu; +ros::ServiceServer en_odom; +ros::ServiceServer reset_odom; + +bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req, + qt_pi_msgs::GetEncoderData::Response &res); +bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req, + qt_pi_msgs::GetBatteryInfo::Response &res); +bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req, + qt_pi_msgs::GetPIDData::Response &res); +bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req, + qt_pi_msgs::GetBaudRate::Response &res); +bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req, + qt_pi_msgs::SetPIDData::Response &res); +bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); +bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); +bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res); +#endif diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h new file mode 100644 index 0000000..edc9c16 --- /dev/null +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h @@ -0,0 +1,27 @@ +#ifndef SERIAL_DRIVER_H +#define SERIAL_DRIVER_H + +#include "commands.h" +#include "payload_specs.h" +#include +#include +#include +#include +#include +#include +#include +#include + +enum com_state { IDLE, COM_SENT, COM_RECEIVED }; + +extern unsigned char com_buf[buf_size]; +extern int com_buf_len; + +extern int init_comm(const char *); +extern void close_comm(); +extern int sendCom(); + +extern void IMU_DATA_PUBLISH(unsigned char *); +extern void ODOM_DATA_PUBLISH(unsigned char *); + +#endif diff --git a/qt_pi/qt_pi_serial/package.xml b/qt_pi/qt_pi_serial/package.xml new file mode 100644 index 0000000..c9bb6a4 --- /dev/null +++ b/qt_pi/qt_pi_serial/package.xml @@ -0,0 +1,25 @@ + + qt_pi_serial + 0.0.0 + Serial communication with microcontroller package of qt_pi + Rishabh Kundu + MIT + + catkin + + roscpp + qt_pi_msgs + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + std_srvs + + roscpp + qt_pi_msgs + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + std_srvs + diff --git a/qt_pi/qt_pi_serial/src/main.cpp b/qt_pi/qt_pi_serial/src/main.cpp new file mode 100644 index 0000000..15bf94d --- /dev/null +++ b/qt_pi/qt_pi_serial/src/main.cpp @@ -0,0 +1,46 @@ +#include "qt_pi_serial/ros_node.h" +#include + +void sighandler(int signum) { + printf("Caught signal %d, Exiting after thread completion\n", signum); + close_comm(); + printf("COMM CLOSED\n"); + ros::shutdown(); + printf("shutting down\n"); +} + +int main(int argc, char **argv) { + + ros::init(argc, argv, "serial_driver"); + + ros::NodeHandle n; + ROS_INFO("REACHED1\n"); + + if (init_comm("/dev/ttyUSB0") < 0) { + ROS_FATAL("Unable to open port"); + exit(0); + } + imu_pub = n.advertise("imu", 1); + ROS_INFO("REACHED2\n"); + odom_pub = n.advertise("odom", 1); + ROS_INFO("REACHED3\n"); + get_encoder = n.advertiseService("get_encoder_state", GET_ENC_SRV_CB); + ROS_INFO("REACHED4\n"); + get_batt_state = n.advertiseService("get_battery_state", GET_BATT_SRV_CB); + ROS_INFO("REACHED5\n"); + get_pid_val = n.advertiseService("get_pid_values", GET_PID_SRV_CB); + ROS_INFO("REACHED6\n"); + get_baudrate = n.advertiseService("get_baudrate", GET_BAUD_SRV_CB); + ROS_INFO("REACHED7\n"); + set_pid_val = n.advertiseService("set_pid_values", SET_PID_SRV_CB); + ROS_INFO("REACHED8\n"); + en_imu = n.advertiseService("set_imu_state", EN_IMU_SRV_CB); + ROS_INFO("REACHED9\n"); + en_odom = n.advertiseService("set_odom_state", EN_ODOM_SRV_CB); + ROS_INFO("REACHED10\n"); + reset_odom = n.advertiseService("reset_odom", RESET_ODOM_SRV_CB); + ROS_INFO("REACHED11\n"); + + signal(SIGINT, sighandler); + ros::spin(); +} diff --git a/qt_pi/qt_pi_serial/src/ros_node.cpp b/qt_pi/qt_pi_serial/src/ros_node.cpp new file mode 100644 index 0000000..b70b3cf --- /dev/null +++ b/qt_pi/qt_pi_serial/src/ros_node.cpp @@ -0,0 +1,194 @@ +#include "qt_pi_serial/ros_node.h" + +sensor_msgs::Imu data; + +bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req, + qt_pi_msgs::GetEncoderData::Response &res) { + + com_buf[COMB] = GET_ENCODERS; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = 3; + + int ret = sendCom(); + + if (ret) + return false; + + memcpy(&res.data.LEFT, com_buf + DATASTRB, 4); + memcpy(&res.data.RIGHT, com_buf + DATASTRB, 4); + + return true; +} + +bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req, + qt_pi_msgs::GetBatteryInfo::Response &res) { + com_buf[COMB] = GET_BATTERY_LVL; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = 3; + + int ret = sendCom(); + + if (ret) + return false; + + uint16_t volts; + + memcpy(&volts, com_buf + DATASTRB, 2); + + res.volts = volts / 100.0; +} + +bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req, + qt_pi_msgs::GetPIDData::Response &res) { + + com_buf[COMB] = GET_PID_P; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = 3; + + int ret = sendCom(); + + if (ret) + return false; + + res.data.lkp = com_buf[DATASTRB]; + res.data.lkd = com_buf[DATASTRB + 1]; + res.data.lki = com_buf[DATASTRB + 2]; + res.data.lko = com_buf[DATASTRB + 3]; + res.data.rkp = com_buf[DATASTRB + 4]; + res.data.rkd = com_buf[DATASTRB + 5]; + res.data.rki = com_buf[DATASTRB + 6]; + res.data.rko = com_buf[DATASTRB + 7]; + + return true; +} + +bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req, + qt_pi_msgs::GetBaudRate::Response &res) { + com_buf[COMB] = GET_BAUDRATE; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = 3; + ROS_INFO("CALLED\n"); + int ret = sendCom(); + + ROS_INFO("CALLED\n"); + if (ret) + return false; + + ROS_INFO("CALLED\n"); + memcpy(&(res.BAUDRATE), com_buf + DATASTRB, 4); + + ROS_INFO("CALLED\n"); + return true; +} + +bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req, + qt_pi_msgs::SetPIDData::Response &res) { + com_buf[COMB] = GET_PID_P; + com_buf[CIDB] = 1; + + com_buf[DATASTRB] = req.data.lkp; + com_buf[DATASTRB + 1] = req.data.lkd; + com_buf[DATASTRB + 2] = req.data.lki; + com_buf[DATASTRB + 3] = req.data.lko; + com_buf[DATASTRB + 4] = req.data.rkp; + com_buf[DATASTRB + 5] = req.data.rkd; + com_buf[DATASTRB + 6] = req.data.rki; + com_buf[DATASTRB + 7] = req.data.rko; + + com_buf_len = 10; + + int ret = sendCom(); + + if (ret) + return false; + + return true; +} +bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) { + com_buf[COMB] = EN_IMU; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = req.data; + + com_buf_len = 3; + + res.success = 0; + + int ret = sendCom(); + + if (ret) + return false; + + res.success = 1; + + return true; +} +bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) { + com_buf[COMB] = EN_ODOM; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = req.data; + + com_buf_len = 3; + + res.success = 0; + + int ret = sendCom(); + + if (ret) + return false; + + res.success = 1; + + return true; +} +bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { + com_buf[COMB] = RESET_ENCODERS; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = 3; + + int ret = sendCom(); + + res.success = 0; + + if (ret) + return false; + + res.success = 1; + + return true; +} + +void ODOM_DATA_PUBLISH(unsigned char *buf) {} + +void IMU_DATA_PUBLISH(unsigned char *buf) { + + int16_t tmp; + + memcpy(&tmp, buf, 2); + data.linear_acceleration.x = tmp * 9.8 / 16384.0; + memcpy(&tmp, buf + 2, 2); + data.linear_acceleration.y = tmp * 9.8 / 16384.0; + memcpy(&tmp, buf + 4, 2); + data.linear_acceleration.z = tmp * 9.8 / 16384.0; + + memcpy(&tmp, buf + 8, 2); + data.angular_velocity.x = tmp * (M_PI / 180.0) / 131.0; + memcpy(&tmp, buf + 10, 2); + data.angular_velocity.y = tmp * (M_PI / 180.0) / 131.0; + memcpy(&tmp, buf + 12, 2); + data.angular_velocity.z = tmp * (M_PI / 180.0) / 131.0; + + imu_pub.publish(data); +} diff --git a/qt_pi/src/serial_driver.cpp b/qt_pi/qt_pi_serial/src/serial_driver.cpp similarity index 50% rename from qt_pi/src/serial_driver.cpp rename to qt_pi/qt_pi_serial/src/serial_driver.cpp index cab7038..53dcbe6 100644 --- a/qt_pi/src/serial_driver.cpp +++ b/qt_pi/qt_pi_serial/src/serial_driver.cpp @@ -1,13 +1,47 @@ -#include +#include "qt_pi_serial/serial_driver.h" +#include "stdio.h" + +unsigned char com_buf[buf_size]; +int com_buf_len; + +int sig = 1; + +pthread_t thread_id; +int fd; + +unsigned char in_out_buf[buf_size]; +int payload_len; +com_state state = IDLE; + +pthread_cond_t data_wait_cond; +pthread_mutex_t data_wait_mutex; + +struct timespec ts; + +int open_port(const char *); +void *serialDataIn(void *); + +int init_comm(const char *port) { + + if ((fd = open_port(port)) < 0) + return -1; + + pthread_create(&thread_id, NULL, serialDataIn, NULL); + + pthread_mutex_init(&data_wait_mutex, NULL); + pthread_cond_init(&data_wait_cond, NULL); + + return 0; +} int open_port(const char *port) { int fd; fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); if (fd == -1) { - perror("open_port: Unable to open - "); + return -1; } else - fcntl(fd, F_SETFL, FNDELAY); + fcntl(fd, F_SETFL, 0); struct termios options; @@ -17,79 +51,59 @@ int open_port(const char *port) { cfsetospeed(&options, B115200); options.c_cflag |= (CLOCAL | CREAD); - options.c_cflag &= ~PARENB; options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSTOPB; options.c_cflag &= ~HUPCL; options.c_cflag |= CS8; - options.c_cc[VTIME] = 100; + options.c_cc[VTIME] = 5; options.c_cc[VMIN] = 1; if (tcsetattr(fd, TCSANOW, &options)) { - perror("Setting options failed.\n"); close(fd); + return -1; } return (fd); } -volatile int sig = 1; - void runRecvCom(uint16_t com_len, unsigned char *com) { switch (com[COMB]) { case GET_BAUDRATE: - - unsigned long baud; - memcpy(&baud, com + DATASTRB, 4); - - BAUDRATE_CB(com[CIDB], baud); - - break; - - case GET_ENCODERS: { - - long l, r; - - memcpy(&l, com + DATASTRB, 4); - memcpy(&r, com + DATASTRB + 4, 4); - - ENCODERS_CB(com[CIDB], l, r); - - break; - } - + case GET_ENCODERS: case GET_PID_P: - - PID_P_CB(com[CIDB], com + DATASTRB); - - break; - case GET_BATTERY_LVL: + case EN_ODOM: + case EN_IMU: + case SET_PID_L: + case SET_PID_R: - uint16_t batt_lvl; - memcpy(&batt_lvl, com + DATASTRB, 2); + if (state != COM_SENT) + return; - BATT_LVL_CB(com[CIDB], batt_lvl); + pthread_mutex_lock(&data_wait_mutex); + + com_buf_len = com_len; + memcpy(com_buf, com, com_len); + state = COM_RECEIVED; + + pthread_cond_signal(&data_wait_cond); + + pthread_mutex_unlock(&data_wait_mutex); break; case IMU_DATA: - IMU_DATA_CB(com[CIDB], com + DATASTRB); + IMU_DATA_PUBLISH(com + DATASTRB); break; case ODOM_DATA: - long l, r; - - memcpy(&l, com + DATASTRB, 4); - memcpy(&r, com + DATASTRB + 4, 4); - - ODOM_DATA_CB(com[CIDB], l, r); + ODOM_DATA_PUBLISH(com + DATASTRB); break; } @@ -97,12 +111,6 @@ void runRecvCom(uint16_t com_len, unsigned char *com) { void *serialDataIn(void *vargp) { - int *fd = (int *)vargp; - - const uint8_t magic_start1 = 0xEE; - const uint8_t magic_start2 = 0xAE; - const uint8_t magic_end = 0xFC; - enum PACKET_STATE { WAITING_FOR_NEW_COM, FIRST_BYTE_VERIFIED, @@ -122,10 +130,11 @@ void *serialDataIn(void *vargp) { uint16_t com_len; PACKET_STATE curState = WAITING_FOR_NEW_COM; + pthread_yield(); while (sig) { - inL = read(*fd, &inChar, 1); + inL = read(fd, &inChar, 1); switch (curState) { @@ -194,60 +203,55 @@ void *serialDataIn(void *vargp) { } } } -} -void hex2bin(const char *in, size_t len, unsigned char *out) { +int sendCom() { - static const unsigned char TBL[] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 58, 59, 60, 61, - 62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75, - 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, - 90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15}; + if (state != IDLE) + return 2; - static const unsigned char *LOOKUP = TBL - 48; + int data_len = com_buf_len - com_preamble_len; - const char *end = in + len; + in_out_buf[MB1P] = magic_start1; + in_out_buf[MB2P] = magic_start2; + memcpy(in_out_buf + LB1P, &com_buf_len, len_len); + in_out_buf[CIDBP] = com_buf[CIDB]; + in_out_buf[COMBP] = com_buf[COMB]; + memcpy(in_out_buf + DATASTRBP, com_buf + DATASTRB, data_len); + in_out_buf[preamble_len + com_buf_len] = magic_end; - while (in < end) - *(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)]; -} + pthread_mutex_lock(&data_wait_mutex); -int main(int argc, char **argv) { - int l = 0; - int fd = open_port(argv[1]); - if (fd < 0) - return 0; - usleep(2000000); - pthread_t thread_id; - pthread_create(&thread_id, NULL, tRead, (void *)&fd); + state = COM_SENT; - unsigned char buffer[25]; - unsigned char in_string[50]; + write(fd, in_out_buf, net_extra_len + com_buf_len); - while (sig) { - fgets(in_string, 50, stdin); - if (in_string[0] == 'q') { - sig = 0; - break; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += 2; + + while (state != COM_RECEIVED) { + if (pthread_cond_timedwait(&data_wait_cond, &data_wait_mutex, &ts) == + ETIMEDOUT) { + + pthread_mutex_unlock(&data_wait_mutex); + return 1; } - - l = strlen(in_string) / 2; - - hex2bin(in_string, 2 * l, buffer); - - int n = write(fd, buffer, l); - printf("%d written\n", n); - for (int i = 0; i < n; i++) - printf("%x ", buffer[i]); - printf("\n"); } - pthread_join(thread_id, NULL); - // int res = read(fd, buffer, 25); + memcpy(com_buf, in_out_buf + preamble_len, com_buf_len); - // if(res > 0) - // for(int i = 0; i < res; i++) - // printf("%x ", buffer[i]); - // printf("\n"); + pthread_mutex_unlock(&data_wait_mutex); + + state = IDLE; + + return 0; +} + +void close_comm() { + sig = 0; + pthread_yield(); + pthread_cancel(thread_id); + pthread_join(thread_id, nullptr); + pthread_mutex_destroy(&data_wait_mutex); + pthread_cond_destroy(&data_wait_cond); close(fd); } diff --git a/qt_pi/qt_pi_serial/tests/comm_test.cpp b/qt_pi/qt_pi_serial/tests/comm_test.cpp new file mode 100644 index 0000000..422fb8f --- /dev/null +++ b/qt_pi/qt_pi_serial/tests/comm_test.cpp @@ -0,0 +1,107 @@ +#include +#include +#include +#include +#include +#include +#include +void hex2bin(const char *in, size_t len, unsigned char *out) { + + static const unsigned char TBL[] = { + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 58, 59, 60, 61, + 62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75, + 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, + 90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15}; + + static const unsigned char *LOOKUP = TBL - 48; + + const char *end = in + len; + + while (in < end) + *(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)]; +} + +int open_port(const char *port) { + int fd; + + fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) { + return -1; + } else + fcntl(fd, F_SETFL, 0); + + struct termios options; + + tcgetattr(fd, &options); + + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSIZE; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~HUPCL; + options.c_cflag |= CS8; + options.c_cc[VTIME] = 5; + options.c_cc[VMIN] = 1; + + if (tcsetattr(fd, TCSANOW, &options)) { + close(fd); + return -1; + } + + return (fd); +} + +pthread_t thread_id; +int fd; + +void *serialDataIn(void *); + +void *serialDataIn(void *vargp) { + + int inL = 0; + + unsigned char com_recv[25]; + unsigned char in_buf[25]; + unsigned char inChar; + int in_buf_i = 0; + uint16_t com_len; + + pthread_yield(); + + while (1) { + printf("Waiting\n"); + inL = read(fd, &inChar, 1); + printf("%x\n", inChar); + } +} + +int main(int argc, char **argv) { + int l = 0; + fd = open_port(argv[1]); + if (fd < 0) + return 0; + usleep(2000000); + + pthread_create(&thread_id, NULL, serialDataIn, NULL); + unsigned char buffer[32]; + char in_string[64]; + + while (1) { + + fgets(in_string, 64, stdin); + if (in_string[0] == 'q') { + pthread_cancel(thread_id); + close(fd); + return 0; + } + + l = strlen(in_string) / 2; + printf("%d\n", l); + hex2bin(in_string, 2 * l, buffer); + + int n = write(fd, buffer, l); + } +} diff --git a/qt_pi/scripts/qt_pi/__init__.py b/qt_pi/scripts/qt_pi/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/qt_pi/scripts/qt_pi/arduino_driver.py b/qt_pi/scripts/qt_pi/arduino_driver.py deleted file mode 100755 index 6ced6c4..0000000 --- a/qt_pi/scripts/qt_pi/arduino_driver.py +++ /dev/null @@ -1,290 +0,0 @@ - -import thread -from math import pi as PI, degrees, radians -import os -import time -import sys, traceback -from serial.serialutil import SerialException -from serial import Serial - -class Arduino: - - def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False): - - self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. - self.PID_INTERVAL = 1000 / 30 - - self.port = port - self.baudrate = baudrate - self.timeout = timeout - self.encoder_count = 0 - self.writeTimeout = timeout - self.interCharTimeout = timeout / 30. - self.motors_reversed = motors_reversed - # Keep things thread safe - self.mutex = thread.allocate_lock() - - def connect(self): - try: - print "Connecting to Arduino on port", self.port, "..." - self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) - # The next line is necessary to give the firmware time to wake up. - time.sleep(1) - test = self.get_baud() - if test != self.baudrate: - time.sleep(1) - test = self.get_baud() - if test != self.baudrate: - raise SerialException - print "Connected at", self.baudrate - print "Arduino is ready." - - except SerialException: - print "Serial Exception:" - print sys.exc_info() - print "Traceback follows:" - traceback.print_exc(file=sys.stdout) - print "Cannot connect to Arduino!" - os._exit(1) - - def open(self): - ''' Open the serial port. - ''' - self.port.open() - - def close(self): - ''' Close the serial port. - ''' - self.port.close() - - def send(self, cmd): - ''' This command should not be used on its own: it is called by the execute commands - below in a thread safe manner. - ''' - self.port.write(cmd + '\r') - - def recv(self, timeout=0.5): - timeout = min(timeout, self.timeout) - ''' This command should not be used on its own: it is called by the execute commands - below in a thread safe manner. Note: we use read() instead of readline() since - readline() tends to return garbage characters from the Arduino - ''' - c = '' - value = '' - attempts = 0 - while c != '\r': - c = self.port.read(1) - value += c - attempts += 1 - if attempts * self.interCharTimeout > timeout: - return None - - value = value.strip('\r') - - return value - - def recv_ack(self): - ''' This command should not be used on its own: it is called by the execute commands - below in a thread safe manner. - ''' - ack = self.recv(self.timeout) - return ack == 'OK' - - def recv_int(self): - ''' This command should not be used on its own: it is called by the execute commands - below in a thread safe manner. - ''' - value = self.recv(self.timeout) - try: - return int(value) - except: - return None - - - def execute(self, cmd): - ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. - ''' - self.mutex.acquire() - - try: - self.port.flushInput() - except: - pass - - ntries = 1 - attempts = 0 - - try: - self.port.write(cmd + '\r') - value = self.recv(self.timeout) - while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): - try: - self.port.flushInput() - self.port.write(cmd + '\r') - value = self.recv(self.timeout) - except: - print "Exception executing command: " + cmd - attempts += 1 - except: - self.mutex.release() - print "Exception executing command: " + cmd - value = None - - self.mutex.release() - return int(value) - - def recv_array(self): - ''' This command should not be used on its own: it is called by the execute commands - below in a thread safe manner. - ''' - try: - values = self.recv(self.timeout * 8).split() - return map(int, values) - except: - return [] - - def execute_array(self, cmd): - ''' Thread safe execution of "cmd" on the Arduino returning an array. - ''' - self.mutex.acquire() - try: - self.port.flushInput() - except: - pass - ntries = 1 - attempts = 0 - - try: - self.port.write(cmd + '\r') - values = self.recv_array() - while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): - try: - self.port.flushInput() - self.port.write(cmd + '\r') - values = self.recv_array() - except: - print("Exception executing command: " + cmd) - attempts += 1 - except: - self.mutex.release() - print "Exception executing command: " + cmd - raise SerialException - return [] - - try: - values = map(int, values) - except: - values = [] - self.mutex.release() - return values - - def execute_ack(self, cmd): - ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. - ''' - self.mutex.acquire() - - try: - self.port.flushInput() - except: - pass - - ntries = 1 - attempts = 0 - - try: - self.port.write(cmd + '\r') - ack = self.recv(self.timeout) - while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): - try: - self.port.flushInput() - self.port.write(cmd + '\r') - ack = self.recv(self.timeout) - except: - print "Exception executing command: " + cmd - attempts += 1 - except: - self.mutex.release() - print "execute_ack exception when executing", cmd - print sys.exc_info() - return 0 - - self.mutex.release() - return ack == 'OK' - - def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo): - ''' Set the PID parameters on the Arduino - ''' - print "Updating PID parameters" - cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo) - self.execute_ack(cmd) - cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo) - self.execute_ack(cmd) - - def get_baud(self): - ''' Get the current baud rate on the serial port. - ''' - try: - return int(self.execute('b')); - except: - return None - - def get_encoder_counts(self): - values = self.execute_array('e') - if len(values) != 2: - print "Encoder count was not 2" - raise SerialException - return None - else: - if self.motors_reversed: - values[0], values[1] = -values[0], -values[1] - return values - - def reset_encoders(self): - ''' Reset the encoder counts to 0 - ''' - return self.execute_ack('r') - - def drive(self, right, left): - ''' Speeds are given in encoder ticks per PID interval - ''' - if self.motors_reversed: - left, right = -left, -right - return self.execute_ack('m %d %d' %(right, left)) - - def drive_m_per_s(self, right, left): - ''' Set the motor speeds in meters per second. - ''' - left_revs_per_second = float(left) / (self.wheel_diameter * PI) - right_revs_per_second = float(right) / (self.wheel_diameter * PI) - - left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) - right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) - - self.drive(right_ticks_per_loop , left_ticks_per_loop ) - - def stop(self): - ''' Stop both motors. - ''' - self.drive(0, 0) - def get_batt_lvl(self): - return self.execute('B') -if __name__ == "__main__": - if os.name == "posix": - portName = "/dev/ttyACM0" - else: - portName = "COM43" # Windows style COM port. - - baudRate = 57600 - - qt_pi_Arduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5) - qt_pi_Arduino.connect() - - print "Sleeping for 1 second..." - time.sleep(1) - - print "Connection test successful.", - - qt_pi_Arduino.stop() - qt_pi_Arduino.close() - - print "Shutting down Arduino." diff --git a/qt_pi/scripts/qt_pi/arduino_driver.pyc b/qt_pi/scripts/qt_pi/arduino_driver.pyc deleted file mode 100644 index 3cd3230..0000000 Binary files a/qt_pi/scripts/qt_pi/arduino_driver.pyc and /dev/null differ diff --git a/qt_pi/scripts/qt_pi/base_controller.py b/qt_pi/scripts/qt_pi/base_controller.py deleted file mode 100755 index 1de8693..0000000 --- a/qt_pi/scripts/qt_pi/base_controller.py +++ /dev/null @@ -1,248 +0,0 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('qt_pi') -import rospy -import os - -from math import sin, cos, pi -from geometry_msgs.msg import Quaternion, Twist, Pose -from std_msgs.msg import Float32 -from nav_msgs.msg import Odometry -from tf.broadcaster import TransformBroadcaster - -""" Class to receive Twist commands and publish Odometry data """ -class BaseController: - def __init__(self, arduino, base_frame, camera_frame, name="base_controllers"): - self.arduino = arduino - self.name = name - self.base_frame = base_frame - self.camera_frame = camera_frame - self.rate = float(rospy.get_param("~base_controller_rate", 10)) - self.timeout = rospy.get_param("~base_controller_timeout", 1.0) - self.stopped = False - - pid_params = dict() - pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") - pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") - pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") - pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) - pid_params['lKp'] = rospy.get_param("~lKp", 20) - pid_params['lKd'] = rospy.get_param("~lKd", 12) - pid_params['lKi'] = rospy.get_param("~lKi", 0) - pid_params['lKo'] = rospy.get_param("~lKo", 50) - pid_params['rKp'] = rospy.get_param("~rKp", 20) - pid_params['rKd'] = rospy.get_param("~rKd", 12) - pid_params['rKi'] = rospy.get_param("~rKi", 0) - pid_params['rKo'] = rospy.get_param("~rKo", 50) - - - self.accel_limit = rospy.get_param('~accel_limit', 0.1) - self.motors_reversed = rospy.get_param("~motors_reversed", False) - self.gear_reduction = rospy.get_param("~gear_reduction", 1.0) - # Set up PID parameters and check for missing values - self.setup_pid(pid_params) - - # How many encoder ticks are there per meter? - self.ticks_per_meter = self.encoder_resolution / (self.wheel_diameter * pi) - - # What is the maximum acceleration we will tolerate when changing wheel speeds? - self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate - - # Track how often we get a bad encoder count (if any) - self.bad_encoder_count = 0 - - now = rospy.Time.now() - self.then = now # time for determining dx/dy - self.t_delta = rospy.Duration(1.0 / self.rate) - self.t_next = now + self.t_delta - - # Internal data - self.enc_left = None # encoder readings - self.enc_right = None - self.x = 0 # position in xy plane - self.y = 0 - self.th = 0 # rotation in radians - self.v_left = 0 - self.v_right = 0 - self.v_des_left = 0 # cmd_vel setpoint - self.v_des_right = 0 - self.last_cmd_vel = now - self.batt_lvl = 0.0 - # Subscriptions - rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) - - # Clear any old odometry info - self.arduino.reset_encoders() - - # Set up the odometry broadcaster - self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) - self.batt_lvl_pub = rospy.Publisher('battery_level', Float32, queue_size=1) - self.odomBroadcaster = TransformBroadcaster() - self.cameraBroadcaster = TransformBroadcaster() - rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") - rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") - - def setup_pid(self, pid_params): - # Check to see if any PID parameters are missing - missing_params = False - for param in pid_params: - if pid_params[param] == "": - print("*** PID Parameter " + param + " is missing. ***") - missing_params = True - - if missing_params: - os._exit(1) - - self.wheel_diameter = pid_params['wheel_diameter'] - self.wheel_track = pid_params['wheel_track'] - self.encoder_resolution = pid_params['encoder_resolution'] - - self.lKp = pid_params['lKp'] - self.lKd = pid_params['lKd'] - self.lKi = pid_params['lKi'] - self.lKo = pid_params['lKo'] - - self.rKp = pid_params['rKp'] - self.rKd = pid_params['rKd'] - self.rKi = pid_params['rKi'] - self.rKo = pid_params['rKo'] - - self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo) - - def poll(self): - now = rospy.Time.now() - if now > self.t_next: - # Read the encoders - try: - left_enc, right_enc = self.arduino.get_encoder_counts() - except: - self.bad_encoder_count += 1 - rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) - return - - dt = now - self.then - self.then = now - dt = dt.to_sec() - - # Calculate odometry - if self.enc_left == None: - dright = 0 - dleft = 0 - else: - dright = (right_enc - self.enc_right) / self.ticks_per_meter - dleft = (left_enc - self.enc_left) / self.ticks_per_meter - - self.enc_right = right_enc - self.enc_left = left_enc - - dxy_ave = (dright + dleft) / 2.0 - dth = (dright - dleft) / self.wheel_track - vxy = dxy_ave / dt - vth = dth / dt - - if (dxy_ave != 0): - dx = cos(dth) * dxy_ave - dy = -sin(dth) * dxy_ave - self.x += (cos(self.th) * dx - sin(self.th) * dy) - self.y += (sin(self.th) * dx + cos(self.th) * dy) - - if (dth != 0): - self.th += dth - - quaternion = Quaternion() - quaternion.x = 0.0 - quaternion.y = 0.0 - quaternion.z = sin(self.th / 2.0) - quaternion.w = cos(self.th / 2.0) - - # Create the odometry transform frame broadcaster. - self.odomBroadcaster.sendTransform( - (self.x, self.y, 0), - (quaternion.x, quaternion.y, quaternion.z, quaternion.w), - rospy.Time.now(), - self.base_frame, - "odom" - ) - #self.cameraBroadcaster.sendTransform((self.x + 0.1, self.y - 0.05, 0.125), - self.cameraBroadcaster.sendTransform((0.1, -0.05, 0.125), - (0,0,0,1), - rospy.Time.now(), - self.camera_frame, - self.base_frame) - - odom = Odometry() - odom.header.frame_id = "odom" - odom.child_frame_id = self.base_frame - odom.header.stamp = now - odom.pose.pose.position.x = self.x - odom.pose.pose.position.y = self.y - odom.pose.pose.position.z = 0 - odom.pose.pose.orientation = quaternion - odom.twist.twist.linear.x = vxy - odom.twist.twist.linear.y = 0 - odom.twist.twist.angular.z = vth - - self.odomPub.publish(odom) - - if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): - self.v_des_left = 0 - self.v_des_right = 0 - - if self.v_left < self.v_des_left: - self.v_left += self.max_accel - if self.v_left > self.v_des_left: - self.v_left = self.v_des_left - else: - self.v_left -= self.max_accel - if self.v_left < self.v_des_left: - self.v_left = self.v_des_left - - if self.v_right < self.v_des_right: - self.v_right += self.max_accel - if self.v_right > self.v_des_right: - self.v_right = self.v_des_right - else: - self.v_right -= self.max_accel - if self.v_right < self.v_des_right: - self.v_right = self.v_des_right - - # Set motor speeds in encoder ticks per PID loop - if not self.stopped: - self.arduino.drive(self.v_left, self.v_right) - - self.t_next = now + self.t_delta - self.batt_lvl = self.arduino.get_batt_lvl() / 100.0 - - self.batt_lvl_pub.publish(self.batt_lvl) - def stop(self): - self.stopped = True - self.arduino.drive(0, 0) - - def cmdVelCallback(self, req): - # Handle velocity-based movement requests - self.last_cmd_vel = rospy.Time.now() - - x = req.linear.x # m/s - th = req.angular.z # rad/s - - if x == 0: - # Turn in place - right = th * self.wheel_track * self.gear_reduction / 2.0 - left = -right - elif th == 0: - # Pure forward/backward motion - left = right = x - else: - # Rotation about a point in space - left = x - th * self.wheel_track * self.gear_reduction / 2.0 - right = x + th * self.wheel_track * self.gear_reduction / 2.0 - - self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE) - self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE) - - - - - - - diff --git a/qt_pi/scripts/qt_pi/base_controller.pyc b/qt_pi/scripts/qt_pi/base_controller.pyc deleted file mode 100644 index 21c829a..0000000 Binary files a/qt_pi/scripts/qt_pi/base_controller.pyc and /dev/null differ diff --git a/qt_pi/setup.py b/qt_pi/setup.py deleted file mode 100644 index 656ccde..0000000 --- a/qt_pi/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['qt_pi'], - package_dir={'': 'scripts'}, - ) - -setup(**d) diff --git a/qt_pi/src/libraries/Arduino/Arduino.ino b/qt_pi/src/libraries/Arduino/Arduino.ino deleted file mode 100644 index 7fe501c..0000000 --- a/qt_pi/src/libraries/Arduino/Arduino.ino +++ /dev/null @@ -1,96 +0,0 @@ -#include "qt_pi.h" - -#define _TASK_SLEEP_ON_IDLE_RUN -#define _TASK_INLINE -#define _TASK_TIMEOUT - -#include - -Task *batt_mon; -Task *diff_control; -Task *move_cmd_timeout; -Task *odom_out; -Task *imu_out; -Task *ser_comm_timeout; - -Scheduler ts; - -void setup() { - - init_comm(); - - init_enc(); - - initMotorController(); - - batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, &initBatt, NULL); - diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, &resetPID, NULL); - move_cmd_timeout = new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL); - odom_out = new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL); - imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false, &initIMU, NULL); - ser_comm_timeout = new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL); - -} - -void loop() -{ - - ts.execute(); - proc_serial(); - -} - -void move_cmd_timeout_reset() -{ -// move_cmd_timeout->resetTimeout(); -} - -void enable_odom(bool a) -{ - switch(a) - { - case 0: - switch(odom_out->isEnabled()) - { - case 1: - odom_out->disable(); - break; - } - break; - - case 1: - switch(odom_out->isEnabled()) - { - case 0: - odom_out->enable(); - break; - } - break; - } - -} - -void enable_imu(bool a) -{ - switch(a) - { - case 0: - switch(imu_out->isEnabled()) - { - case 1: - imu_out->disable(); - break; - } - break; - - case 1: - switch(imu_out->isEnabled()) - { - case 0: - imu_out->enable(); - break; - } - break; - } - -} diff --git a/qt_pi/src/libraries/Arduino/comm.cpp b/qt_pi/src/libraries/Arduino/comm.cpp deleted file mode 100644 index fd67488..0000000 --- a/qt_pi/src/libraries/Arduino/comm.cpp +++ /dev/null @@ -1,365 +0,0 @@ -#include "qt_pi.h" - -const uint8_t magic_start1 = 0xEE; -const uint8_t magic_start2 = 0xAE; -const uint8_t magic_end = 0xFC; - -#define buf_size 32 - -/* -#define WAITING_FOR_NEW_COM 0 -#define FIRST_BYTE_VERIFIED 1 -#define SECOND_BYTE_VERIFIED 2 -#define LENGTH_INPUT 3 -#define READING_PAYLOAD 4 -#define READING_COMPLETE 5 -#define COM_RECEIVED 6 -*/ -enum COMM_STATE { - WAITING_FOR_NEW_COM, - FIRST_BYTE_VERIFIED, - SECOND_BYTE_VERIFIED, - LENGTH_INPUT, - READING_PAYLOAD, - READING_COMPLETE, - COM_RECEIVED -}; - -int curState = WAITING_FOR_NEW_COM; - -int com_len; - -uint8_t in_buf[buf_size]; -int in_buf_i; - -uint8_t ser_write_buf[buf_size]; -uint8_t com_recv[buf_size]; - -int init_comm(){ - - ser_write_buf[MB1P] = magic_start1; - ser_write_buf[MB2P] = magic_start2; - - Serial.begin(BAUDRATE); - Serial.setTimeout(0); - - resetComm(); - - curState = WAITING_FOR_NEW_COM; - -} - -int sendCommand(int16_t len) -{ - if((len + net_extra_len) > buf_size) - return -1; - - memcpy(ser_write_buf + start_magic_len, &len, len_len); - - ser_write_buf[preamble_len + len] = magic_end; - - return(Serial.write(ser_write_buf, len + net_extra_len)); - -} - -int sendData(int len, uint8_t *buf){ - - if((len + net_extra_len) > buf_size) - return -1; - - memcpy(ser_write_buf + preamble_len, buf, len); - - return sendCommand(len); - -} - -int runInpCommand() { - - int data_len = com_len; - - switch(com_recv[COMB]) { - - case GET_BAUDRATE: - - if(com_len != GET_BAUDRATE_LEN) - { - data_len = -1; - break; - } - - memcpy(ser_write_buf + DATASTRBP, &BAUDRATE, 4); - - data_len = 4; - - break; - - case GET_ENCODERS: - { - - if(com_len != GET_ENCODERS_LEN) - { - data_len = -1; - break; - } - - long l = readEncoder(LEFT); - long r = readEncoder(RIGHT); - - memcpy(ser_write_buf + DATASTRBP, &l, 4); - memcpy(ser_write_buf + DATASTRBP + 4, &r, 4); - - data_len = 8; - - break; - } - - case RESET_ENCODERS: - - if(com_len != RESET_ENCODERS_LEN) - { - data_len = -1; - break; - } - - resetEncoders(); - resetPID(); - - data_len = 0; - - break; - - case SET_SPEEDS: - - if(com_len != SET_SPEEDS_LEN) - { - data_len = -1; - break; - } - - move_cmd_timeout_reset(); - - if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0) { - - setMotorSpeeds(0, 0); - - resetPID(); - - setMoving(false); - - } - else - { - setMoving(true); - } - - setTicksPerFrame(com_recv + DATASTRB); - - data_len = 0; - - break; - - case SET_PID_L: - - if(com_len != SET_PID_LEN) - { - data_len = -1; - break; - } - - setPID_L(com_recv + DATASTRB); - - data_len = 0; - - break; - - case SET_PID_R: - - if(com_len != SET_PID_LEN) - { - data_len = -1; - break; - } - - setPID_R(com_recv + DATASTRB); - - data_len = 0; - - break; - - case GET_PID_P: - - if(com_len != GET_PID_P_LEN) - { - data_len = -1; - break; - } - - getPID(ser_write_buf + DATASTRBP); - - data_len = 8; - - break; - - case GET_BATTERY_LVL: - - if(com_len != GET_BATTERY_LVL_LEN) - { - data_len = -1; - break; - } - - getBattLvl(ser_write_buf + DATASTRBP); - - data_len = 2; - - break; - - case EN_IMU: - - if(com_len != EN_IMU_LEN) - { - data_len = -1; - break; - } - - enable_imu(com_recv[DATASTRB]); - - ser_write_buf[DATASTRBP] = 1; - data_len = 1; - - break; - - case EN_ODOM: - - if(com_len != EN_ODOM_LEN) - { - data_len = -1; - break; - } - - enable_odom(com_recv[DATASTRB]); - - ser_write_buf[DATASTRBP] = 1; - data_len = 1; - - break; - - - default: - - ser_write_buf[DATASTRBP] = com_recv[COMB]; - com_recv[COMB] = INVALID; - data_len = 1; - - break; - } - - if(data_len == -1) - { - ser_write_buf[COMBP] = INVALID_LENGTH; - ser_write_buf[CIDBP] = com_recv[CIDB]; - ser_write_buf[DATASTRBP] = com_recv[COMB]; - data_len = 1; - } - else - { - ser_write_buf[COMBP] = com_recv[COMB]; - ser_write_buf[CIDBP] = com_recv[CIDB]; - } - - sendCommand(data_len + com_preamble_len); - -} - -void resetComm(){ - com_len = 1; - in_buf_i = 0; -} - -uint8_t temp_buf[64]; -void serCommTimeout(){ - Serial.readBytes(temp_buf, 64); -} - -void proc_serial(){ - - if(!Serial.available()) - return; - - move_cmd_timeout_reset(); - - switch(curState){ - - case WAITING_FOR_NEW_COM: - - if(Serial.read() == magic_start1) - { - curState = FIRST_BYTE_VERIFIED; - } - - break; - - case FIRST_BYTE_VERIFIED: - - if(Serial.read() == magic_start2) - { - curState = SECOND_BYTE_VERIFIED; - } - else - { - curState = WAITING_FOR_NEW_COM; - } - - break; - - case SECOND_BYTE_VERIFIED: - - com_len = Serial.read(); - curState = LENGTH_INPUT; - - break; - - case LENGTH_INPUT: - - com_len = com_len | (Serial.read() << 8); - - if(com_len < 20) - { - curState = READING_PAYLOAD; - } - else - { - curState = WAITING_FOR_NEW_COM; - } - - break; - - case READING_PAYLOAD: - - in_buf_i += Serial.readBytes(&in_buf[in_buf_i], com_len - in_buf_i); - - if(in_buf_i == com_len) - { - curState = READING_COMPLETE; - } - break; - - case READING_COMPLETE: - - if(Serial.read() == magic_end){ - - memcpy(com_recv, in_buf, com_len); - - runInpCommand(); - - curState = WAITING_FOR_NEW_COM; - } - - resetComm(); - - break; - } - - if((Serial.available() > 0) && curState != WAITING_FOR_NEW_COM) - proc_serial(); - } diff --git a/qt_pi/src/libraries/Arduino/commands.h b/qt_pi/src/libraries/Arduino/commands.h deleted file mode 100644 index 496f7da..0000000 --- a/qt_pi/src/libraries/Arduino/commands.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef COMMANDS_H -#define COMMANDS_H - -#define INVALID 0xFF -#define INVALID_LENGTH 0xFE - -#define GET_BAUDRATE 0x51 -#define GET_ENCODERS 0x52 -#define SET_SPEEDS 0x53 -#define RESET_ENCODERS 0x54 -#define SET_PID_L 0x55 -#define SET_PID_R 0x56 -#define GET_PID_P 0x57 -#define GET_BATTERY_LVL 0x58 -#define EN_IMU 0x59 -#define EN_ODOM 0x60 -#define ODOM_DATA 0x61 -#define IMU_DATA 0x62 - -#define GET_BAUDRATE_LEN 0x01 -#define GET_ENCODERS_LEN 0x01 -#define SET_SPEEDS_LEN 0x03 -#define RESET_ENCODERS_LEN 0x01 -#define SET_PID_LEN 0x05 -#define GET_PID_P_LEN 0x01 -#define GET_BATTERY_LVL_LEN 0x01 -#define EN_IMU_LEN 0x02 -#define EN_ODOM_LEN 0x02 -#define ODOM_DATA_LEN 0x08 -#define IMU_DATA_LEN 0x08 - -#define LEFT 0x00 -#define RIGHT 0x01 - -#endif diff --git a/qt_pi/src/libraries/Arduino/payload_specs.h b/qt_pi/src/libraries/Arduino/payload_specs.h deleted file mode 100644 index bb98ac1..0000000 --- a/qt_pi/src/libraries/Arduino/payload_specs.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef PAYLOAD_SPECS_H -#define PAYLOAD_SPECS_H - -#define MB1P 0 -#define MB2P 1 -#define LB1P 2 -#define LB2P 3 -#define CIDBP 4 -#define COMBP 5 -#define DATASTRBP 6 - -#define start_magic_len 2 -#define end_magic_len 1 -#define len_len 2 - -#define BID 0xFF - -#define CIDB 0 -#define COMB 1 -#define DATASTRB 2 - -const int com_preamble_len = 2; - -const int preamble_len = start_magic_len + len_len; -const int net_extra_len = preamble_len + end_magic_len; - -#endif