yikestone/qt_pi
1
0
Fork 0
mirror of https://gitlab.com/yikestone/qt_pi.git synced 2025-08-03 05:34:13 +05:30
Code Issues Projects Releases Wiki Activity

Temp Commit

Browse Source
This commit is contained in:
yikestone 2020-10-23 16:47:19 +05:30
parent 868eee170d
commit 7255417d7a
82 changed files with 1430 additions and 1315 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,3 +1,4 @@
*.pyc
.ccls-cache
compile_commands.json
tags

3
CMakeLists.txt
View File

@ -1,3 +0,0 @@
cmake_minimum_required(VERSION 3.5)
ADD_SUBDIRECTORY( qt_pi )
ADD_SUBDIRECTORY( gazebo_sim )

0
qt_pi/README.md → README.md
View File

4
qt_pi/msg/EEG_data.msg
View File

@ -1,4 +0,0 @@
int channel[5]
float32 data[5]
int cq[5]
int batt_lvl

87
qt_pi/nodes/arduino_node.py
View File

@ -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)

0
qt_pi/config/70-emotiv.rules → qt_pi/qt_pi/config/70-emotiv.rules
View File

0
qt_pi/config/base_local_planner_params.yaml → qt_pi/qt_pi/config/base_local_planner_params.yaml
View File

0
qt_pi/config/costmap_common_params.yaml → qt_pi/qt_pi/config/costmap_common_params.yaml
View File

0
qt_pi/config/global_costmap_params.yaml → qt_pi/qt_pi/config/global_costmap_params.yaml
View File

0
qt_pi/config/local_costmap_params.yaml → qt_pi/qt_pi/config/local_costmap_params.yaml
View File

0
qt_pi/config/qt_pi_arduino_params.yaml → qt_pi/qt_pi/config/qt_pi_arduino_params.yaml
View File

0
qt_pi/launch/arduino.launch → qt_pi/qt_pi/launch/arduino.launch
View File

0
qt_pi/launch/depth_to_laser.launch → qt_pi/qt_pi/launch/depth_to_laser.launch
View File

0
qt_pi/launch/gmapping.launch → qt_pi/qt_pi/launch/gmapping.launch
View File

0
qt_pi/launch/move_base.launch → qt_pi/qt_pi/launch/move_base.launch
View File

0
qt_pi/launch/qt_pi.launch → qt_pi/qt_pi/launch/qt_pi.launch
View File

34
qt_pi/CMakeLists.txt → qt_pi/qt_pi_eeg/CMakeLists.txt
View File

@ -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)

0
qt_pi/doc/Connecting on debian.md → qt_pi/qt_pi_eeg/doc/Connecting on debian.md
View File

0
qt_pi/include/ble_connect/ble_connect.h → qt_pi/qt_pi_eeg/include/ble_connect/ble_connect.h
View File

11
qt_pi/qt_pi_eeg/package.xml Normal file
View File

@ -0,0 +1,11 @@
<package format="2">
<name>qt_pi_eeg</name>
<version>0.0.0</version>
<description>EEG processing package of qt_pi</description>
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
</package>

0
qt_pi/src/ble_connect.c → qt_pi/qt_pi_eeg/src/ble_connect.c
View File

0
qt_pi/tests/ble_test.cpp → qt_pi/qt_pi_eeg/tests/ble_test.cpp
View File

109
qt_pi/qt_pi_embedded/CMakeLists.txt.tmp Normal file
View File

@ -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)

0
qt_pi/src/libraries/Arduino/batt.cpp → qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp
View File

0
qt_pi/src/libraries/Arduino/batt.h → qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h
View File

374
qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp Normal file
View File

@ -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 ();
}

0
qt_pi/src/libraries/Arduino/comm.h → qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h
View File

35
qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h Normal file
View File

@ -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

8
qt_pi/src/libraries/Arduino/diff_controller.cpp → qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp
View File

@ -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){

0
qt_pi/src/libraries/Arduino/diff_controller.h → qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.h
View File

90
qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino Normal file
View File

@ -0,0 +1,90 @@
#include "qt_pi.h"
#define _TASK_SLEEP_ON_IDLE_RUN
#define _TASK_INLINE
#define _TASK_TIMEOUT
#include <TaskScheduler.h>
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;
}
}

8
qt_pi/src/libraries/Arduino/encoder_driver.cpp → qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp
View File

@ -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);
}

0
qt_pi/src/libraries/Arduino/encoder_driver.h → qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.h
View File

0
qt_pi/src/libraries/Arduino/imu.cpp → qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp
View File

0
qt_pi/src/libraries/Arduino/imu.h → qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h
View File

8
qt_pi/src/libraries/Arduino/motor_driver.cpp → qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp
View File

@ -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);
}

0
qt_pi/src/libraries/Arduino/motor_driver.h → qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.h
View File

33
qt_pi/qt_pi_embedded/src/embedded_firmware/payload_specs.h Normal file
View File

@ -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

8
qt_pi/src/libraries/Arduino/qt_pi.h → qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h
View File

@ -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);

37
qt_pi/qt_pi_gui/CMakeLists.txt Normal file
View File

@ -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)

0
qt_pi/include/mapview/mapview.h → qt_pi/qt_pi_gui/include/mapview/mapview.h
View File

0
qt_pi/include/mapview/vertex.h → qt_pi/qt_pi_gui/include/mapview/vertex.h
View File

0
qt_pi/include/speller/speller.h → qt_pi/qt_pi_gui/include/speller/speller.h
View File

13
qt_pi/package.xml → qt_pi/qt_pi_gui/package.xml
View File

@ -1,18 +1,17 @@
<package format="2">
<name>qt_pi</name>
<name>qt_pi_gui</name>
<version>0.0.0</version>
<description>The qt_pi package</description>
<maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
<description>GUI package of qt_pi</description>
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>qt_build</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>python-serial</exec_depend>
<export></export>
</package>
</package>

0
qt_pi/resources/resources.qrc → qt_pi/qt_pi_gui/resources/resources.qrc
View File

0
qt_pi/resources/shaders/blink.frag → qt_pi/qt_pi_gui/resources/shaders/blink.frag
View File

0
qt_pi/resources/shaders/blink.vert → qt_pi/qt_pi_gui/resources/shaders/blink.vert
View File

0
qt_pi/resources/shaders/map.frag → qt_pi/qt_pi_gui/resources/shaders/map.frag
View File

0
qt_pi/resources/shaders/map.vert → qt_pi/qt_pi_gui/resources/shaders/map.vert
View File

0
qt_pi/src/main.cpp → qt_pi/qt_pi_gui/src/main.cpp
View File

0
qt_pi/src/mapview.cpp → qt_pi/qt_pi_gui/src/mapview.cpp
View File

0
qt_pi/src/speller.cpp → qt_pi/qt_pi_gui/src/speller.cpp
View File

0
qt_pi/tests/map_load_test.cpp → qt_pi/qt_pi_gui/tests/map_load_test.cpp
View File

36
qt_pi/qt_pi_msgs/CMakeLists.txt Normal file
View File

@ -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
)

2
qt_pi/qt_pi_msgs/msg/Encoder.msg Normal file
View File

@ -0,0 +1,2 @@
int64 LEFT
int64 RIGHT

8
qt_pi/qt_pi_msgs/msg/PID.msg Normal file
View File

@ -0,0 +1,8 @@
uint8 lkp
uint8 lkd
uint8 lki
uint8 lko
uint8 rkp
uint8 rkd
uint8 rki
uint8 rko

19
qt_pi/qt_pi_msgs/package.xml Normal file
View File

@ -0,0 +1,19 @@
<package format="2">
<name>qt_pi_msgs</name>
<version>0.0.0</version>
<description>Custom msgs and srvs package for qt_pi</description>
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
</package>

2
qt_pi/qt_pi_msgs/srv/GetBatteryInfo.srv Normal file
View File

@ -0,0 +1,2 @@
---
float32 volts

2
qt_pi/qt_pi_msgs/srv/GetBaudRate.srv Normal file
View File

@ -0,0 +1,2 @@
---
uint32 BAUDRATE

2
qt_pi/qt_pi_msgs/srv/GetEncoderData.srv Normal file
View File

@ -0,0 +1,2 @@
---
qt_pi_msgs/Encoder data

2
qt_pi/qt_pi_msgs/srv/GetPIDData.srv Normal file
View File

@ -0,0 +1,2 @@
---
qt_pi_msgs/PID data

2
qt_pi/qt_pi_msgs/srv/SetPIDData.srv Normal file
View File

@ -0,0 +1,2 @@
qt_pi_msgs/PID data
---

32
qt_pi/qt_pi_serial/CMakeLists.txt Normal file
View File

@ -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)

35
qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h Normal file
View File

@ -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

33
qt_pi/qt_pi_serial/include/qt_pi_serial/payload_specs.h Normal file
View File

@ -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

46
qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h Normal file
View File

@ -0,0 +1,46 @@
#ifndef ROS_NODE_H
#define ROS_NODE_H
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/UInt32.h>
#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

27
qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h Normal file
View File

@ -0,0 +1,27 @@
#ifndef SERIAL_DRIVER_H
#define SERIAL_DRIVER_H
#include "commands.h"
#include "payload_specs.h"
#include <errno.h>
#include <fcntl.h>
#include <pthread.h>
#include <stdint.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
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

25
qt_pi/qt_pi_serial/package.xml Normal file
View File

@ -0,0 +1,25 @@
<package format="2">
<name>qt_pi_serial</name>
<version>0.0.0</version>
<description>Serial communication with microcontroller package of qt_pi</description>
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>qt_pi_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>qt_pi_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
</package>

46
qt_pi/qt_pi_serial/src/main.cpp Normal file
View File

@ -0,0 +1,46 @@
#include "qt_pi_serial/ros_node.h"
#include <csignal>
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<sensor_msgs::Imu>("imu", 1);
ROS_INFO("REACHED2\n");
odom_pub = n.advertise<nav_msgs::Odometry>("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();
}

194
qt_pi/qt_pi_serial/src/ros_node.cpp Normal file
View File

@ -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);
}

190
qt_pi/src/serial_driver.cpp → qt_pi/qt_pi_serial/src/serial_driver.cpp
View File

@ -1,13 +1,47 @@
#include <qt_pi/serial_driver.h>
#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);
}

107
qt_pi/qt_pi_serial/tests/comm_test.cpp Normal file
View File

@ -0,0 +1,107 @@
#include <fcntl.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
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);
}
}

0
qt_pi/scripts/qt_pi/__init__.py
View File

290
qt_pi/scripts/qt_pi/arduino_driver.py
View File

@ -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."

BIN
qt_pi/scripts/qt_pi/arduino_driver.pyc
View File

Binary file not shown.

248
qt_pi/scripts/qt_pi/base_controller.py
View File

@ -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)

BIN
qt_pi/scripts/qt_pi/base_controller.pyc
View File

Binary file not shown.

11
qt_pi/setup.py
View File

@ -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)

96
qt_pi/src/libraries/Arduino/Arduino.ino
View File

@ -1,96 +0,0 @@
#include "qt_pi.h"
#define _TASK_SLEEP_ON_IDLE_RUN
#define _TASK_INLINE
#define _TASK_TIMEOUT
#include <TaskScheduler.h>
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;
}
}

365
qt_pi/src/libraries/Arduino/comm.cpp
View File

@ -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();
}

35
qt_pi/src/libraries/Arduino/commands.h
View File

@ -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

27
qt_pi/src/libraries/Arduino/payload_specs.h
View File

@ -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